C++ Vector3f::unitOrthogonal方法代码示例

  void MsgToPoint3D(const TPPLPoint &pt, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message, Eigen::Vector3f &pos, Eigen::Vector3f &normal) {
    if(new_message->params.size()==4) {
      Eigen::Vector3f u,v,origin;
      Eigen::Affine3f transformation;
      normal(0)=new_message->params[0];
      normal(1)=new_message->params[1];
      normal(2)=new_message->params[2];
      origin(0)=new_message->centroid.x;
      origin(1)=new_message->centroid.y;
      origin(2)=new_message->centroid.z;
      //std::cout << "normal: " << normal << std::endl;
      //std::cout << "centroid: " << origin << std::endl;
      v = normal.unitOrthogonal ();
      u = normal.cross (v);
      pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal,  origin, transformation);

      transformation=transformation.inverse();

      Eigen::Vector3f p3;
      p3(0)=pt.x;
      p3(1)=pt.y;
      p3(2)=0;
      pos = transformation*p3;
    }
    else if(new_message->params.size()==5) {
      Eigen::Vector2f v,v2,n2;
      v(0)=pt.x;
      v(1)=pt.y;
      v2=v;
      v2(0)*=v2(0);
      v2(1)*=v2(1);
      n2(0)=new_message->params[3];
      n2(1)=new_message->params[4];

      //dummy normal
      normal(0)=new_message->params[0];
      normal(1)=new_message->params[1];
      normal(2)=new_message->params[2];

      Eigen::Vector3f x,y, origin;
      x(0)=1.f;
      y(1)=1.f;
      x(1)=x(2)=y(0)=y(2)=0.f;

      Eigen::Matrix<float,3,2> proj2plane_;
      proj2plane_.col(0)=normal.cross(y);
      proj2plane_.col(1)=normal.cross(x);

      origin(0)=new_message->centroid.x;
      origin(1)=new_message->centroid.y;
      origin(2)=new_message->centroid.z;

      pos = origin+proj2plane_*v + normal*(v2.dot(n2));
      normal += normal*(v).dot(n2);
    }
  }

  double
  radiusAndOriginFromCloud(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr in_cloud ,
                  std::vector<int>& indices,
                  Eigen::Vector3f& origin,
                  const Eigen::Vector3f& sym_axis)
  {
    //  Transform into cylinder coordinate frame
    Eigen::Affine3f t;
    pcl::getTransformationFromTwoUnitVectorsAndOrigin(sym_axis.unitOrthogonal(), sym_axis, origin, t);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc_trans( new pcl::PointCloud<pcl::PointXYZRGB>() );
    pcl::transformPointCloud(*in_cloud, indices, *pc_trans, t);

    // Inliers of circle model
    pcl::PointIndices inliers;
    // Coefficients of circle model
    pcl::ModelCoefficients coeff;
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    // Optimize coefficients
    seg.setOptimizeCoefficients (true);
    // Set type of method
    //seg.setMethodType (pcl::SAC_MLESAC);
    seg.setMethodType (pcl::SAC_RANSAC);
    // Set number of maximum iterations
    seg.setMaxIterations (10);
    // Set type of model
    seg.setModelType (pcl::SACMODEL_CIRCLE2D);
    // Set threshold of model
    seg.setDistanceThreshold (0.010);
    // Give as input the filtered point cloud
    seg.setInputCloud (pc_trans/*in_cloud*/);
    //boost::shared_ptr<std::vector<int> > indices_ptr(&indices);
    //seg.setIndices(indices_ptr);
    // Call the segmenting method
    seg.segment(inliers, coeff);


    // origin in cylinder coordinates
    Eigen::Vector3f l_origin;
    l_origin << coeff.values[0],coeff.values[1],0;
    origin = t.inverse() * l_origin;

    return coeff.values[2];
  }

vector<ofVec3f> findRectangle(const PCPtr& cloud, const pcl::ModelCoefficients& coefficients) 
{ 
	vector<Eigen::Vector3f> corners; 
	//saveCloudAsFile("toproject.pcd",*cloud);
	pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients(coefficients));
	// Project points onto the table plane 

	PC projectedCloud = *cloud; 

	//saveCloudAsFile("projected.pcd",projectedCloud);

	PCXYZ pto = cloud->at(1);
	float val = coefficients.values[0] * pto.x + 
				coefficients.values[1] * pto.y + 
				coefficients.values[2] * pto.z + 
				coefficients.values[3]; 

	if(abs(val) < 0.009)
		;;

	// store the table top plane parameters 
	Eigen::Vector3f planeNormal; 
	planeNormal.x() = coeff->values[0]; 
	planeNormal.y() = coeff->values[1]; 
	planeNormal.z() = coeff->values[2]; 
	
	// compute an orthogonal normal to the plane normal 
	Eigen::Vector3f v = planeNormal.unitOrthogonal(); 
	
	// take the cross product of the two normals to get 
	// a thirds normal, on the plane 
	Eigen::Vector3f u = planeNormal.cross(v); 

	// project the 3D point onto a 2D plane 
	std::vector<cv::Point2f> points; 
	
	// choose a point on the plane 
	Eigen::Vector3f p0(projectedCloud.points[0].x, 
						projectedCloud.points[0].y, 
						projectedCloud.points[0].z); 
	
	for(unsigned int ii=0; ii<projectedCloud.points.size(); ii++) 
	{ 
		Eigen::Vector3f p3d(projectedCloud.points[ii].x, 
								projectedCloud.points[ii].y, 
								projectedCloud.points[ii].z); 

		// subtract all 3D points with a point in the plane 
		// this will move the origin of the 3D coordinate system 
		// onto the plane 
		p3d = p3d - p0; 

		cv::Point2f p2d; 
		p2d.x = p3d.dot(u); 
		p2d.y = p3d.dot(v); 
		points.push_back(p2d); 
	} 

	cv::Mat pointsMat(points); 
	cv::RotatedRect rrect = cv::minAreaRect(pointsMat); 
	cv::Point2f rrPts[4]; 
	rrect.points(rrPts); 

	//store the table top bounding points in a vector 
	for(unsigned int ii=0; ii<4; ii++) 
	{ 
		Eigen::Vector3f pbbx(rrPts[ii].x*u + rrPts[ii].y*v + p0); 
		corners.push_back(pbbx); 
	} 
	/*Eigen::Vector3f center(rrect.center.x*u + rrect.center.y*v + p0); 
	corners.push_back(center); */

	//Ver si se puede eliminar esto.
	vector<ofVec3f> vecCorners = projectPointsInPlane(corners,coefficients);

	//saveCloudAsFile("projectedrectangle.pcd",vecCorners);
	return vecCorners; 
} 

template<typename Point> void
TableObjectCluster<Point>::calculateBoundingBox(
  const PointCloudPtr& cloud,
  const pcl::PointIndices& indices,
  const Eigen::Vector3f& plane_normal,
  const Eigen::Vector3f& plane_point,
  Eigen::Vector3f& position,
  Eigen::Quaternion<float>& orientation,
  Eigen::Vector3f& size)
{
  // transform to table coordinate frame and project points on X-Y, save max height
  Eigen::Affine3f tf;
  pcl::getTransformationFromTwoUnitVectorsAndOrigin(plane_normal.unitOrthogonal(), plane_normal, plane_point, tf);
  pcl::PointCloud<pcl::PointXYZ>::Ptr pc2d(new pcl::PointCloud<pcl::PointXYZ>);
  float height = 0.0;
  for(std::vector<int>::const_iterator it=indices.indices.begin(); it != indices.indices.end(); ++it)
  {
    Eigen::Vector3f tmp = tf * (*cloud)[*it].getVector3fMap();
    height = std::max<float>(height, fabs(tmp(2)));
    pc2d->push_back(pcl::PointXYZ(tmp(0),tmp(1),0.0));
  }

  // create convex hull of projected points
  #ifdef PCL_MINOR_VERSION >= 6
  pcl::PointCloud<pcl::PointXYZ>::Ptr conv_hull(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ConvexHull<pcl::PointXYZ> chull;
  chull.setDimension(2);
  chull.setInputCloud(pc2d);
  chull.reconstruct(*conv_hull);
  #endif

  /*for(int i=0; i<conv_hull->size(); ++i)
    std::cout << (*conv_hull)[i].x << "," << (*conv_hull)[i].y << std::endl;*/

  // find the minimal bounding rectangle in 2D and table coordinates
  Eigen::Vector2f p1, p2, p3;
  cob_3d_mapping::MinimalRectangle2D mr2d;
  mr2d.setConvexHull(conv_hull->points);
  mr2d.rotatingCalipers(p2, p1, p3);
  /*std::cout << "BB: \n" << p1[0] << "," << p1[1] <<"\n"
            << p2[0] << "," << p2[1] <<"\n"
            << p3[0] << "," << p3[1] <<"\n ---" << std::endl;*/

  // compute center of rectangle
  position[0] = 0.5f*(p1[0] + p3[0]);
  position[1] = 0.5f*(p1[1] + p3[1]);
  position[2] = 0.0f;
  // transform back
  Eigen::Affine3f inv_tf = tf.inverse();
  position = inv_tf * position;
  // set size of bounding box
  float norm_1 = (p3-p2).norm();
  float norm_2 = (p1-p2).norm();
  // BoundingBox coordinates: X:= main direction, Z:= table normal
  Eigen::Vector3f direction; // y direction
  if (norm_1 < norm_2)
  {
    direction = Eigen::Vector3f(p3[0]-p2[0], p3[1]-p2[1], 0) / (norm_1);
    size[0] = norm_2 * 0.5f;
    size[1] = norm_1 * 0.5f;
  }
  else
  {
    direction = Eigen::Vector3f(p1[0]-p2[0], p1[1]-p2[1], 0) / (norm_2);
    size[0] = norm_1 * 0.5f;
    size[1] = norm_2 * 0.5f;
  }
  size[2] = -height;


  direction = inv_tf.rotation() * direction;
  orientation = pcl::getTransformationFromTwoUnitVectors(direction, plane_normal).rotation(); // (y, z-direction)

  return;
  Eigen::Matrix3f M = Eigen::Matrix3f::Identity() - plane_normal * plane_normal.transpose();
  Eigen::Vector3f xn = M * Eigen::Vector3f::UnitX(); // X-axis project on normal
  Eigen::Vector3f xxn = M * direction;
  float cos_phi = acos(xn.normalized().dot(xxn.normalized())); // angle between xn and main direction
  cos_phi = cos(0.5f * cos_phi);
  float sin_phi = sqrt(1.0f-cos_phi*cos_phi);
  //orientation.w() = cos_phi;
  //orientation.x() = sin_phi * plane_normal(0);
  //orientation.y() = sin_phi * plane_normal(1);
  //orientation.z() = sin_phi * plane_normal(2);
}