C++ Matrix3f::setConstant方法代码示例

//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
  const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
  std::vector<int> n_indices;
  std::vector<float> n_sqr_distances;

  searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);

  Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);

  Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();

  double distance = 0.0;
  double sum = 0.0;

  int valid_nn_points = 0;

  for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
  {
    Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
    if (pt.head<3> () == central_point.head<3> ())
		  continue;

    // Difference between current point and origin
    vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
    vij (valid_nn_points, 3) = 0;

    distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);

    // Multiply vij * vij'
    cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());

    sum += distance;
    valid_nn_points++;
  }

  if (valid_nn_points < 5)
  {
    //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
    rf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  cov_m /= sum;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);

  const double& e1c = solver.eigenvalues ()[0];
  const double& e2c = solver.eigenvalues ()[1];
  const double& e3c = solver.eigenvalues ()[2];

  if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
  {
    //PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
    rf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  // Disambiguation
  Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
  Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
  v1.head<3> () = solver.eigenvectors ().col (2);
  v3.head<3> () = solver.eigenvectors ().col (0);

  int plusNormal = 0, plusTangentDirection1=0;
  for (int ne = 0; ne < valid_nn_points; ne++)
  {
    double dp = vij.row (ne).dot (v1);
    if (dp >= 0)
      plusTangentDirection1++;

    dp = vij.row (ne).dot (v3);
    if (dp >= 0)
      plusNormal++;
  }

  //TANGENT
  plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
  if (plusTangentDirection1 == 0)
  {
		int points = 5; //std::min(valid_nn_points*2/2+1, 11);
		int medianIndex = valid_nn_points/2;

		for (int i = -points/2; i <= points/2; i++)
			if ( vij.row (medianIndex - i).dot (v1) > 0)
				plusTangentDirection1 ++;

		if (plusTangentDirection1 < points/2+1)
			v1 *= - 1;
	} else if (plusTangentDirection1 < 0)
    v1 *= - 1;

  //Normal
  plusNormal = 2*plusNormal - valid_nn_points;
  if (plusNormal == 0)
//.........这里部分代码省略.........

template<typename PointInT, typename PointNT, typename PointOutT> float
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computePointLRF (const int &index,
                                                                                        Eigen::Matrix3f &lrf)
{
  //find Z axis

  //extract support points for Rz radius
  std::vector<int> neighbours_indices;
  std::vector<float> neighbours_distances;
  int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);

  //check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
  if (n_neighbours < 6)
  {
    //PCL_WARN(
    //    "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n",
    //    getClassName().c_str(), index);

    //setting lrf to NaN
    lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());

    return (std::numeric_limits<float>::max ());
  }

  //copy neighbours coordinates into eigen matrix
  Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
  for (int i = 0; i < n_neighbours; ++i)
  {
    neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
  }

  Eigen::Vector3f x_axis, y_axis;
  //plane fitting to find direction of Z axis
  Eigen::Vector3f fitted_normal; //z_axis
  Eigen::Vector3f centroid;
  planeFitting (neigh_points_mat, centroid, fitted_normal);

  //disambiguate Z axis with normal mean
  normalDisambiguation (*normals_, neighbours_indices, fitted_normal);

  //setting LRF Z axis
  lrf.row (2).matrix () = fitted_normal;

  /////////////////////////////////////////////////////////////////////////////////////////
  //find X axis

  //extract support points for Rx radius
  if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
  {
    n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
  }

  //find point with the "most different" normal (with respect to fittedNormal)

  float min_normal_cos = std::numeric_limits<float>::max ();
  int min_normal_index = -1;

  bool margin_point_found = false;
  Eigen::Vector3f best_margin_point;
  bool best_point_found_on_margins = false;

  float radius2 = tangent_radius_ * tangent_radius_;

  float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;

  float max_boundary_angle = 0;

  if (find_holes_)
  {
    randomOrthogonalAxis (fitted_normal, x_axis);

    lrf.row (0).matrix () = x_axis;

    for (int i = 0; i < check_margin_array_size_; i++)
    {
      check_margin_array_[i] = false;
      margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
      margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
      margin_array_min_angle_normal_[i] = -1.0;
      margin_array_max_angle_normal_[i] = -1.0;
    }
    max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
  }

  for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
  {
    const int& curr_neigh_idx = neighbours_indices[curr_neigh];
    const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
    if (neigh_distance_sqr <= margin_distance2)
    {
      continue;
    }

    //point normalIndex is inside the ring between marginThresh and Radius
    margin_point_found = true;

    Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();

    float normal_cos = fitted_normal.dot (normal_mean);
    if (normal_cos < min_normal_cos)
//.........这里部分代码省略.........