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

  /** \brief Computes the gradient parameters of the patch (patch gradient components dx_ dy_, Hessian H_, Shi-Tomasi Score s_, Eigenvalues of the Hessian e0_ and e1_).
   *         The expanded patch patchWithBorder_ must be set.
   *         Sets validGradientParameters_ afterwards to true.
   */
  void computeGradientParameters() const{
    if(!validGradientParameters_){
      H_.setZero();
      const int refStep = patchSize+2;
      float* it_dx = dx_;
      float* it_dy = dy_;
      const float* it;
      Eigen::Vector3f J;
      J.setZero();
      for(int y=0; y<patchSize; ++y){
        it = patchWithBorder_ + (y+1)*refStep + 1;
        for(int x=0; x<patchSize; ++x, ++it, ++it_dx, ++it_dy){
          J[0] = 0.5 * (it[1] - it[-1]);
          J[1] = 0.5 * (it[refStep] - it[-refStep]);
          J[2] = 1;
          *it_dx = J[0];
          *it_dy = J[1];
          H_ += J*J.transpose();
        }
      }
      const float dXX = H_(0,0)/(patchSize*patchSize);
      const float dYY = H_(1,1)/(patchSize*patchSize);
      const float dXY = H_(0,1)/(patchSize*patchSize);

      e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
      e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
      s_ = e0_;
      validGradientParameters_ = true;
    }
  }

  void computeCovarianceMatrix(PointCloudRGB cloud, std::vector<int> indices, Eigen::Vector4f centroid, Eigen::Matrix3f &covariance_matrix)
  {
    // ROS_INFO("Inside computeCovarianceMatrix() ");
    // Initialize to 0
    covariance_matrix.setZero ();
    
    if (indices.empty ())
      return;
  
    // If the data is dense, we don't need to check for NaN
    if (cloud.is_dense)
      {
	// For each point in the cloud
	for (size_t i = 0; i < indices.size (); ++i)
	  {
	    //Eigen::Vector4f ptx = cloud.points[indices[i]].getVector4fMap ();
	    ///std::cout << "Index : "<< i <<" : "<<ptx<<std::endl;
	    Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
	    
	    covariance_matrix (0, 0) += pt.x () * pt.x ();
	    covariance_matrix (0, 1) += pt.x () * pt.y ();
	    covariance_matrix (0, 2) += pt.x () * pt.z ();
	    covariance_matrix (1, 1) += pt.y () * pt.y ();
	    covariance_matrix (1, 2) += pt.y () * pt.z ();	
	    covariance_matrix (2, 2) += pt.z () * pt.z ();
	  }

      }
    // NaN or Inf values could exist => check for them
    else
      {
	//std::cout<<"Cloud is not dense "<<std::endl;
	// For each point in the cloud
	for (size_t i = 0; i < indices.size (); ++i)
	  {
	    // Check if the point is invalid
	    if (!pcl_isfinite (cloud.points[indices[i]].x) || 
		!pcl_isfinite (cloud.points[indices[i]].y) || 
		!pcl_isfinite (cloud.points[indices[i]].z))
	      continue;
	    
	    // Eigen::Vector4f ptx = cloud.points[indices[i]].getVector4fMap ();
	    // std::cout << "Index : "<< i <<" : "<<ptx<<std::endl;
	    Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;
	    
	    covariance_matrix (0, 0) += pt.x () * pt.x ();
	    covariance_matrix (0, 1) += pt.x () * pt.y ();
	    covariance_matrix (0, 2) += pt.x () * pt.z ();
	    covariance_matrix (1, 1) += pt.y () * pt.y ();
	    covariance_matrix (1, 2) += pt.y () * pt.z ();	
	    covariance_matrix (2, 2) += pt.z () * pt.z ();
	    covariance_matrix (1, 1) += pt.y () * pt.y ();
	  }
      }
    covariance_matrix (1, 0) = covariance_matrix (0, 1);
    covariance_matrix (2, 0) = covariance_matrix (0, 2);
    covariance_matrix (2, 1) = covariance_matrix (1, 2);
    
    covariance_matrix /= indices.size();
  }

template <typename PointT> inline void
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, 
                              const std::vector<int> &indices,
                              const Eigen::Vector4f &centroid, 
                              Eigen::Matrix3f &covariance_matrix)
{
  // Initialize to 0
  covariance_matrix.setZero ();

  if (indices.empty ())
    return;
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    // For each point in the cloud
    for (size_t i = 0; i < indices.size (); ++i)
    {
      Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    // For each point in the cloud
    for (size_t i = 0; i < indices.size (); ++i)
    {
      // Check if the point is invalid
      if (!pcl_isfinite (cloud.points[indices[i]].x) || 
          !pcl_isfinite (cloud.points[indices[i]].y) || 
          !pcl_isfinite (cloud.points[indices[i]].z))
        continue;

      Eigen::Vector4f pt = cloud.points[indices[i]].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  covariance_matrix (1, 0) = covariance_matrix (0, 1);
  covariance_matrix (2, 0) = covariance_matrix (0, 2);
  covariance_matrix (2, 1) = covariance_matrix (1, 2);
}

void PointCloudProcessing::getRTGeometricLinearSystem(PointCloudC::Ptr corrRef,
                                                      PointCloudC::Ptr corrNew,
                                                      Eigen::Matrix4f &transMat)
{
  // build linear system Ax = b;
  int rows = 3*corrRef->points.size();
  int cols = 6;
  Eigen::MatrixXf A(rows, cols); A.setZero();
  Eigen::MatrixXf b(rows, 1); b.setZero();
  for(int i=0; i<corrRef->points.size(); i++)
    {
      float X1 = corrRef->points.at(i).x;
      float Y1 = corrRef->points.at(i).y;
      float Z1 = corrRef->points.at(i).z;
      float X0 = corrNew->points.at(i).x;
      float Y0 = corrNew->points.at(i).y;
      float Z0 = corrNew->points.at(i).z;
      A(3*i,   0) = 0;      A(3*i,   1) = Z0+Z1;  A(3*i,   2) = -Y0-Y1; A(3*i,   3) = 1;
      A(3*i+1, 0) = -Z0-Z1; A(3*i+1, 1) = 0;      A(3*i+1, 2) = X0+X1;  A(3*i+1, 4) = 1;
      A(3*i+2, 0) = Y0+Y1;  A(3*i+1, 1) = -X0-X1; A(3*i+2, 2) = 0;      A(3*i+2, 5) = 1;

      b(3*i,   0) = X1-X0;
      b(3*i+1, 0) = Y1-Y0;
      b(3*i+2, 0) = Z1-Z0;
    }
  //    std::cout<<"A = "<<A<<std::endl;
  //    std::cout<<"b = "<<b<<std::endl;
  Eigen::MatrixXf solveX(rows, 1); solveX.setZero();
  solveX = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
  //    std::cout<<"solveX: "<<solveX<<"\n";

  // solveX = [Rx, Ry, Rz, T'x, T'y, T'z];
  Eigen::Matrix3f I_Vx; I_Vx.setOnes();
  I_Vx(0,0) = 1;          I_Vx(0,1) = solveX(2);  I_Vx(0,2) = -solveX(1);
  I_Vx(1,0) = -solveX(2); I_Vx(1,1) = 1;          I_Vx(1,2) = solveX(0);
  I_Vx(2,0) = solveX(1);  I_Vx(2,1) = -solveX(0); I_Vx(2,2) = 1;
  Eigen::Vector3f Tx; Tx.setZero();
  Tx(0) = solveX(3); Tx(1) = solveX(4); Tx(2) = solveX(5);
  Eigen::Vector3f tx; tx.setZero();
  tx = I_Vx.inverse()*Tx;
  transMat(0,3) = tx(0); transMat(1,3) = tx(1); transMat(2,3) = tx(2);

  Eigen::Matrix3f IplusVx; IplusVx.setOnes();
  IplusVx(0,0) = 1;          IplusVx(0,1) = -solveX(2);  IplusVx(0,2) = solveX(1);
  IplusVx(1,0) = solveX(2);  IplusVx(1,1) = 1;           IplusVx(1,2) = -solveX(0);
  IplusVx(2,0) = -solveX(1); IplusVx(2,1) = solveX(0);   IplusVx(2,2) = 1;
  Eigen::Matrix3f Ro; Ro.setZero();
  Ro = I_Vx.inverse()*IplusVx;
  transMat(0,0) = Ro(0,0); transMat(0,1) = Ro(0,1); transMat(0,2) = Ro(0,2); transMat(0,3) = tx(0);
  transMat(1,0) = Ro(1,0); transMat(1,1) = Ro(1,1); transMat(1,2) = Ro(1,2); transMat(1,3) = tx(1);
  transMat(2,0) = Ro(2,0); transMat(2,1) = Ro(2,1); transMat(2,2) = Ro(2,2); transMat(2,3) = tx(2);
  //    std::cout<<"transMat Geometric: "<<transMat<<"\n";
}

void btl::image::CTrackerSimpleFreak::track( const Eigen::Matrix3f& eimHomoInit_,
											 boost::shared_ptr<cv::gpu::GpuMat> acvgmShrPtrPyrBW_[4],
											 const cv::Mat& cvmMaskCurr_,
											 Eigen::Matrix3f* peimHomo_ ){
												 
	//from coarse to fine
	*peimHomo_ =  eimHomoInit_;
	cv::gpu::GpuMat cvgmMaskPrev; cvgmMaskPrev.upload(_cvmMaskPrev);
	cv::gpu::GpuMat cvgmMaskCurr; cvgmMaskCurr.upload(cvmMaskCurr_);
	cv::gpu::GpuMat cvgmBuffer;
	for (unsigned int uLevel = _uPyrHeight-1; uLevel >= -1; uLevel--){
		for (int n = 0; n < 5; n++){ //iterations
			btl::device::cudaFullFrame( *_acgvmShrPtrPyrBWPrev[uLevel], cvgmMaskPrev, *acvgmShrPtrPyrBW_[uLevel], cvgmMaskCurr, peimHomo_->data(), &cvgmBuffer);
			//solve out the delta homography
			Eigen::Matrix3f eimDeltaHomo; eimDeltaHomo.setZero();
			extractHomography(cvgmBuffer,&eimDeltaHomo);
			*peimHomo_ += eimDeltaHomo; 
		}//iterations
	}//for pyramid
	return;
}

 /** \brief Computes and sets the multilevel Shi-Tomasi Score \ref s_, considering a defined pyramid level interval.
  *
  * @param l1 - Start level (l1<l2)
  * @param l2 - End level (l1<l2)
  */
 void computeMultilevelShiTomasiScore(const int l1 = 0, const int l2 = nLevels_-1) const{
   H_.setZero();
   int count = 0;
   for(int i=l1;i<=l2;i++){
     if(isValidPatch_[i]){
       H_ += pow(0.25,i)*patches_[i].getHessian();
       count++;
     }
   }
   if(count > 0){
     const float dXX = H_(0,0)/(count*patchSize*patchSize);
     const float dYY = H_(1,1)/(count*patchSize*patchSize);
     const float dXY = H_(0,1)/(count*patchSize*patchSize);
     e0_ = 0.5 * (dXX + dYY - sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
     e1_ = 0.5 * (dXX + dYY + sqrtf((dXX + dYY) * (dXX + dYY) - 4 * (dXX * dYY - dXY * dXY)));
     s_ = e0_;
   } else {
     e0_ = 0;
     e1_ = 0;
     s_ = -1;
   }
 }

void computeSensorOffsetAndK(Eigen::Isometry3f &sensorOffset, Eigen::Matrix3f &cameraMatrix, ParameterCamera *cameraParam, int reduction) {
  sensorOffset = Isometry3f::Identity();
  cameraMatrix.setZero();
      
  int cmax = 4;
  int rmax = 3;
  for (int c=0; c<cmax; c++){
    for (int r=0; r<rmax; r++){
      sensorOffset.matrix()(r, c) = cameraParam->offset()(r, c);
      if (c<3)
	cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
    }
  }
  sensorOffset.translation() = Vector3f(0.15f, 0.0f, 0.05f);
  Quaternionf quat = Quaternionf(0.5, -0.5, 0.5, -0.5);
  sensorOffset.linear() = quat.toRotationMatrix();
  sensorOffset.matrix().block<1, 4>(3, 0) << 0.0f, 0.0f, 0.0f, 1.0f;
	
  float scale = 1./reduction;
  cameraMatrix *= scale;
  cameraMatrix(2,2) = 1;
}

/**
 * ComputeCovarianceMatrix
 */
void ZAdaptiveNormals::computeCovarianceMatrix (const v4r::DataMatrix2D<Eigen::Vector3f> &cloud, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov)
{
  Eigen::Vector3f pt;
  cov.setZero ();

  for (unsigned i = 0; i < indices.size (); ++i)
  {
    pt = cloud.data[indices[i]] - mean;

    cov(1,1) += pt[1] * pt[1];
    cov(1,2) += pt[1] * pt[2];
    cov(2,2) += pt[2] * pt[2];

    pt *= pt[0];
    cov(0,0) += pt[0];
    cov(0,1) += pt[1];
    cov(0,2) += pt[2];
  }

  cov(1,0) = cov(0,1);
  cov(2,0) = cov(0,2);
  cov(2,1) = cov(1,2);
}

/**
 * computeCovarianceMatrix
 */
void PlaneEstimationRANSAC::computeCovarianceMatrix (const std::vector<Eigen::Vector3f> &pts, const std::vector<int> &indices, const Eigen::Vector3f &mean, Eigen::Matrix3f &cov)
{
  Eigen::Vector3f pt;
  cov.setZero ();

  for (unsigned i = 0; i < indices.size (); ++i)
  {
    pt = pts[indices[i]] - mean;

    cov(1,1) += pt[1] * pt[1];
    cov(1,2) += pt[1] * pt[2];

    cov(2,2) += pt[2] * pt[2];

    pt *= pt[0];
    cov(0,0) += pt[0];
    cov(0,1) += pt[1];
    cov(0,2) += pt[2];
  }

  cov(1,0) = cov(0,1);
  cov(2,0) = cov(0,2);
  cov(2,1) = cov(1,2);
}

  void ViewerState::load(const std::string& filename){
    clear();
    listWidget->clear();
    graph->clear();
    graph->load(filename.c_str());

    vector<int> vertexIds(graph->vertices().size());
    int k=0;
    for (OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
      vertexIds[k++] = (it->first);
    }

    sort(vertexIds.begin(), vertexIds.end());

    listAssociations.clear();
    size_t maxCount = 20000;
    for(size_t i = 0; i < vertexIds.size() &&  i< maxCount; ++i) {
      OptimizableGraph::Vertex* _v = graph->vertex(vertexIds[i]);
      g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(_v);
      if (! v)
	continue;
      OptimizableGraph::Data* d = v->userData();
      while(d) {
	RGBDData* rgbdData = dynamic_cast<RGBDData*>(d);
	if (!rgbdData){
	  d=d->next();
	  continue;
	}
	// retrieve from the rgb data the index of the parameter
	int paramIndex = rgbdData->paramIndex();
	// retrieve from the graph the parameter given the index  
	g2o::Parameter* _cameraParam = graph->parameter(paramIndex);
	// attempt a cast to a parameter camera  
	ParameterCamera* cameraParam = dynamic_cast<ParameterCamera*>(_cameraParam);
	if (! cameraParam){
	  cerr << "shall thou be damned forever" << endl;
	  return;
	}
	// yayyy we got the parameter
	Eigen::Matrix3f cameraMatrix;
	Eigen::Isometry3f sensorOffset;
	cameraMatrix.setZero();
      
	int cmax = 4;
	int rmax = 3;
	for (int c=0; c<cmax; c++){
	  for (int r=0; r<rmax; r++){
	    sensorOffset.matrix()(r,c)= cameraParam->offset()(r, c);
	    if (c<3)
	      cameraMatrix(r,c) = cameraParam->Kcam()(r, c);
	  }
	}
	char buf[1024];
	sprintf(buf,"%d",v->id());
	QString listItem(buf);
	listAssociations.push_back(rgbdData);
	listWidget->addItem(listItem);
	d=d->next();
      }
    }

  }

//.........这里部分代码省略.........
        new_inlier_indices.push_back(merged_indices);
      }

      model_coefficients = new_model_coefficients;
      inlier_indices = new_inlier_indices;
    }

    for(size_t i=0; i < model_coefficients.size(); i++)
    {
      PlaneModel<PointT> pm;
      pm.coefficients_ = model_coefficients[i];
      pm.cloud_ = input_;
      pm.inliers_ = inlier_indices[i];

      //recompute coefficients based on distance to camera and normal?
      Eigen::Vector4f centroid;
      pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);
      Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);

      Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);

      float sum_w = 0.f;
      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
      {
          const int &idx = inlier_indices[i].indices[k];
          float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();
          float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);
          //w_k = 1.f;
          M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);
          sum_w += w_k;
      }

      Eigen::Matrix3f scatter;
      scatter.setZero ();
      scatter = M_w.transpose() * M_w;

      Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);
      //std::cout << svd.matrixV() << std::endl;

      Eigen::Vector3f n = svd.matrixV().col(2);
      //flip normal if required
      if(n.dot(c*-1) < 0)
          n = n * -1.f;

      float d = n.dot(c) * -1.f;
      //std::cout << "normal:" << n << std::endl;
      //std::cout << "d:" << d << std::endl;

      pm.coefficients_.values[0] = n[0];
      pm.coefficients_.values[1] = n[1];
      pm.coefficients_.values[2] = n[2];
      pm.coefficients_.values[3] = d;

      pcl::PointIndices clean_inlier_indices;
      float dist_threshold_ = 0.01f;

      for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
      {
          const int &idx = inlier_indices[i].indices[k];
          Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();
          float val = n.dot(p) + d;

          if(std::abs(val) <= dist_threshold_)
              clean_inlier_indices.indices.push_back( idx );
      }

template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
                                                               PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
                                                               PointInTPtr & grid, pcl::PointIndices & indices)
{

  Eigen::Vector3f plane_normal;
  plane_normal[0] = -centroid[0];
  plane_normal[1] = -centroid[1];
  plane_normal[2] = -centroid[2];
  Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
  plane_normal.normalize ();
  Eigen::Vector3f axis = plane_normal.cross (z_vector);
  double rotation = -asin (axis.norm ());
  axis.normalize ();

  Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));

  grid->points.resize (processed->points.size ());
  for (size_t k = 0; k < processed->points.size (); k++)
    grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();

  pcl::transformPointCloud (*grid, *grid, transformPC);

  Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
  Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);

  centroid4f = transformPC * centroid4f;
  normal_centroid4f = transformPC * normal_centroid4f;

  Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);

  Eigen::Vector4f farthest_away;
  pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
  farthest_away[3] = 0;

  float max_dist = (farthest_away - centroid4f).norm ();

  pcl::demeanPointCloud (*grid, centroid4f, *grid);

  Eigen::Matrix4f center_mat;
  center_mat.setIdentity (4, 4);
  center_mat (0, 3) = -centroid4f[0];
  center_mat (1, 3) = -centroid4f[1];
  center_mat (2, 3) = -centroid4f[2];

  Eigen::Matrix3f scatter;
  scatter.setZero ();
  float sum_w = 0.f;

  //for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
  for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
  {
    Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
    float d_k = (pvector).norm ();
    float w = (max_dist - d_k);
    Eigen::Vector3f diff = (pvector);
    Eigen::Matrix3f mat = diff * diff.transpose ();
    scatter = scatter + mat * w;
    sum_w += w;
  }

  scatter /= sum_w;

  Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
  Eigen::Vector3f evx = svd.matrixV ().col (0);
  Eigen::Vector3f evy = svd.matrixV ().col (1);
  Eigen::Vector3f evz = svd.matrixV ().col (2);
  Eigen::Vector3f evxminus = evx * -1;
  Eigen::Vector3f evyminus = evy * -1;
  Eigen::Vector3f evzminus = evz * -1;

  float s_xplus, s_xminus, s_yplus, s_yminus;
  s_xplus = s_xminus = s_yplus = s_yminus = 0.f;

  //disambiguate rf using all points
  for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
  {
    Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
    float dist_x, dist_y;
    dist_x = std::abs (evx.dot (pvector));
    dist_y = std::abs (evy.dot (pvector));

    if ((pvector).dot (evx) >= 0)
      s_xplus += dist_x;
    else
      s_xminus += dist_x;

    if ((pvector).dot (evy) >= 0)
      s_yplus += dist_y;
    else
      s_yminus += dist_y;

  }

  if (s_xplus < s_xminus)
    evx = evxminus;

  if (s_yplus < s_yminus)
    evy = evyminus;
//.........这里部分代码省略.........

template <typename PointT> inline unsigned
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              const Eigen::Vector4f &centroid,
                              Eigen::Matrix3f &covariance_matrix)
{
  if (cloud.points.empty ())
    return (0);

  // Initialize to 0
  covariance_matrix.setZero ();

  unsigned point_count;
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    point_count = static_cast<unsigned> (cloud.size ());
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
    }
  }
  // NaN or Inf values could exist => check for them
  else
  {
    point_count = 0;
    // For each point in the cloud
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      Eigen::Vector4f pt = cloud [i].getVector4fMap () - centroid;

      covariance_matrix (1, 1) += pt.y () * pt.y ();
      covariance_matrix (1, 2) += pt.y () * pt.z ();

      covariance_matrix (2, 2) += pt.z () * pt.z ();

      pt *= pt.x ();
      covariance_matrix (0, 0) += pt.x ();
      covariance_matrix (0, 1) += pt.y ();
      covariance_matrix (0, 2) += pt.z ();
      ++point_count;
    }
  }
  covariance_matrix (1, 0) = covariance_matrix (0, 1);
  covariance_matrix (2, 0) = covariance_matrix (0, 2);
  covariance_matrix (2, 1) = covariance_matrix (1, 2);

  return (point_count);
}