C++ VectorXf::norm方法代码示例

VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
// 	const int MAX_SAMPLE = 20000;
	const bool fast = true, very_fast = true;
	// Remove the DC (Y : N x M)
	RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
// 	RMatrixXf sY = dY;
// 	if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) {
// 		VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE );
// 		std::sort( samples.data(), samples.data()+samples.size() );
// 		sY = RMatrixXf( samples.size(), dY.cols() );
// 		for( int i=0; i<samples.size(); i++ )
// 			sY.row(i) = dY.row( samples[i] );
// 	}
	
	// ... and use (pc > 0)
	VectorXf lbl = VectorXf::Zero( Y.rows() );
	
	// Find the largest PC of (dY.T * dY) and project onto it
	if( very_fast ) {
		// Find the largest PC using poweriterations
		VectorXf U = VectorXf::Random( dY.cols() );
		U = U.array() / U.norm()+std::numeric_limits<float>::min();
		for( int it=0; it<20; it++ ){
			// Normalize
			VectorXf s = dY.transpose()*(dY*U);
			s.array() /= s.norm()+std::numeric_limits<float>::min();
			if ( (U-s).norm() < 1e-6 )
				break;
			U = s;
		}
		// Project onto the PC
		lbl = dY*U;
	}
	else if(fast) {
		// Compute the eigen values of the covariance (and project onto the largest eigenvector)
		MatrixXf cov = dY.transpose()*dY;
		SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
		MatrixXf ev = eigensolver.eigenvectors();
		lbl = dY * ev.col( ev.cols()-1 );
	}
	else {
		// Use the SVD
		JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
		// Project onto the largest PC
		lbl = svd.matrixU().col(0) * svd.singularValues()[0];
	}
	// Find the representative label
	if( rep_label )
		dY.array().square().rowwise().sum().minCoeff( rep_label );
	
	return (lbl.array() < 0).cast<float>();
}

void PlaneFittingCloudOrienter::_compute() {
    assert(input_cloud_);
    assert(input_intensity_);
    assert(!output_cloud_);

    // -- Fit a plane.
    VectorXf a = fitPlane(*input_cloud_);

    // -- Rotate the points so that the direction of the best plane is the x axis.
    assert(fabs(a.norm() - 1) < 1e-4);
    double theta = M_PI/2. - atan2(a(1), a(0));
    MatrixXf rotation = MatrixXf::Identity(3, 3);
    rotation(0,0) = cos(theta);
    rotation(1,1) = cos(theta);
    rotation(0,1) = sin(theta);
    rotation(1,0) = -sin(theta);

    output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf());
    *output_cloud_ = *input_cloud_ * rotation;

    VectorXf foo = fitPlane(*output_cloud_);
    //cout << "New plane: " << foo.transpose() << endl;

    // -- Subtract off the mean of the points.
    MatrixXf& points = *output_cloud_;
    VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
    for(int i=0; i<points.rows(); ++i)
        points.row(i) -= pt_mean.transpose();

}

IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
					     const std::vector<int>& interest_region_indices)
{
  // -- Put cluster points into matrix form.
  MatrixXf points(interest_region_indices.size(), 3);
  for(size_t i=0; i<interest_region_indices.size(); ++i) {
    points(i, 0) = data.points[interest_region_indices[i]].x;
    points(i, 1) = data.points[interest_region_indices[i]].y;
    points(i, 2) = data.points[interest_region_indices[i]].z;
  }

  // -- Subtract off the mean and flatten to z=0 to prepare for PCA.
  MatrixXf X = points;
  X.col(2) = VectorXf::Zero(X.rows());
  VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
  for(int i=0; i<X.rows(); ++i) {
    X.row(i) -= pt_mean.transpose();
  }
  MatrixXf Xt = X.transpose();
  
  // -- Find the long axis.
  // Start with a random vector.
  VectorXf pc = VectorXf::Zero(3);
  pc(0) = 1; //Chosen by fair dice roll.
  pc(1) = 1;
  pc.normalize();
  
  // Power method.
  VectorXf prev = pc;
  double thresh = 1e-4;
  int ctr = 0;
  while(true) { 
    prev = pc;
    pc =  Xt * (X * pc);
    pc.normalize();
    ctr++;
    if((pc - prev).norm() < thresh)
      break;
  }
  assert(abs(pc(2)) < 1e-4);
  
  // -- Find the short axis.
  VectorXf shrt = VectorXf::Zero(3);
  shrt(1) = -pc(0);
  shrt(0) = pc(1);
  assert(abs(shrt.norm() - 1) < 1e-4);
  assert(abs(shrt.dot(pc)) < 1e-4);
  
  // -- Build the basis of normalized coordinates.
  MatrixXf basis = MatrixXf::Zero(3,3);
  basis.col(0) = pc;
  basis.col(1) = shrt;
  basis(2,2) = -1.0;
  assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
  assert(abs(basis.col(0).norm() - 1) < 1e-4);
  assert(abs(basis.col(1).norm() - 1) < 1e-4);
  assert(abs(basis.col(2).norm() - 1) < 1e-4);


  // -- Put the cluster into normalized coordinates, and choose which axis to project on.
  MatrixXf projected_basis(3, 2);
  if(axis_ == 0) { 
    projected_basis.col(0) = basis.col(1);
    projected_basis.col(1) = basis.col(2);
  }
  else if(axis_ == 1) { 
    projected_basis.col(0) = basis.col(0);
    projected_basis.col(1) = basis.col(2);
  }
  else if(axis_ == 2) { 
    projected_basis.col(0) = basis.col(0);
    projected_basis.col(1) = basis.col(1);
  }
  MatrixXf projected = points * projected_basis;
    
  // -- Transform into pixel units.
  for(int i=0; i<projected.rows(); ++i) {
    projected(i, 0) *= pixels_per_meter_;
    projected(i, 1) *= pixels_per_meter_;
  }

  // -- Find min and max of u and v.  TODO: noise sensitivity?
  float min_v = FLT_MAX;
  float min_u = FLT_MAX;
  float max_v = -FLT_MAX;
  float max_u = -FLT_MAX;
  for(int i=0; i<projected.rows(); ++i) {
    float u = projected(i, 0);
    float v = projected(i, 1);
    if(u < min_u)
      min_u = u;
    if(u > max_u)
      max_u = u;
    if(v < min_v)
      min_v = v;
    if(v > max_v)
      max_v = v;
  }

  // -- Shift the origin based on {u,v}_offset_pct. 
//.........这里部分代码省略.........

MatrixXf D3DCloudOrienter::orientCloud(const sensor_msgs::PointCloud& data,
				    const std::vector<int>& interest_region_indices)
{
  // -- Put cluster points into matrix form.
  MatrixXf points(interest_region_indices.size(), 3);
  for(size_t i=0; i<interest_region_indices.size(); ++i) {
    points(i, 0) = data.points[interest_region_indices[i]].x;
    points(i, 1) = data.points[interest_region_indices[i]].y;
    points(i, 2) = data.points[interest_region_indices[i]].z;
  }

  // -- Subtract off the mean of the points.
  VectorXf pt_mean = points.colwise().sum() / (float)points.rows();
  for(int i=0; i<points.rows(); ++i)
    points.row(i) -= pt_mean.transpose();

  // -- Flatten to z == 0.
  MatrixXf X = points;
  X.col(2) = VectorXf::Zero(X.rows());
  MatrixXf Xt = X.transpose();
  
  // -- Find the long axis.
  // Start with a random vector.
  VectorXf pc = VectorXf::Zero(3);
  pc(0) = 1; //Chosen by fair dice roll.
  pc(1) = 1;
  pc.normalize();
  
  // Power method.
  VectorXf prev = pc;
  double thresh = 1e-4;
  int ctr = 0;
  while(true) { 
    prev = pc;
    pc =  Xt * (X * pc);
    pc.normalize();
    ctr++;
    if((pc - prev).norm() < thresh)
      break;
  }
  assert(abs(pc(2)) < 1e-4);
  
  // -- Find the short axis.
  VectorXf shrt = VectorXf::Zero(3);
  shrt(1) = -pc(0);
  shrt(0) = pc(1);
  assert(abs(shrt.norm() - 1) < 1e-4);
  assert(abs(shrt.dot(pc)) < 1e-4);
  
  // -- Build the basis of normalized coordinates.
  MatrixXf basis = MatrixXf::Zero(3,3);
  basis.col(0) = pc;
  basis.col(1) = shrt;
  basis(2,2) = 1.0;
  assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
  assert(abs(basis.col(0).norm() - 1) < 1e-4);
  assert(abs(basis.col(1).norm() - 1) < 1e-4);
  assert(abs(basis.col(2).norm() - 1) < 1e-4);

  // -- Rotate and return.
  MatrixXf oriented = points * basis;
  return oriented;
}