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);
}