inline double
pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2)
{
// Compute the actual angle
double rad = v1.dot (v2) / sqrt (v1.squaredNorm () * v2.squaredNorm ());
if (rad < -1.0) rad = -1.0;
if (rad > 1.0) rad = 1.0;
return (acos (rad));
}
void
inputCallback (const sensor_msgs::PointCloud2ConstPtr &cloud_in)
{
if (output_cloud_.width != 0)
{
if (!object_released_)
{
output_cloud_.header.stamp = ros::Time::now();
object_pub_.publish (output_cloud_);
float *xzy = (float*)&output_cloud_.data[0];
ROS_INFO("Republished object in frame %s (%g,%g,%g)", output_cloud_.header.frame_id.c_str(), xzy[0], xzy[1], xzy[2]);
}
return;
}
// If nothing is pointed at
if (!to_select_)
return;
// std::cerr << "to_select_" << std::endl;
// If there is no TF data available
ros::Time time = ros::Time::now();
bool found_transform = tf_listener_.waitForTransform("right_hand", "openni_rgb_optical_frame", time, ros::Duration(1));
if (!found_transform)
{
std::cerr << "no transform found" << std::endl;
return;
}
tf::StampedTransform c2h_transform;
tf_listener_.lookupTransform("right_hand", "openni_rgb_optical_frame", time, c2h_transform);
to_select_ = false;
ROS_INFO_STREAM ("[" << getName ().c_str () << "] Received cloud: cloud time " << cloud_in->header.stamp);
// Filter the input dataser
PointCloud cloud_raw, cloud;
pcl::fromROSMsg (*cloud_in, cloud_raw);
vgrid_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
vgrid_.filter (cloud);
// Fit a plane (the table)
pcl::ModelCoefficients table_coeff;
pcl::PointIndices table_inliers;
PointCloud cloud_projected;
pcl::PointCloud<Point> cloud_hull;
// ---[ Estimate the point normals
pcl::PointCloud<pcl::Normal> cloud_normals;
n3d_.setInputCloud (boost::make_shared<PointCloud> (cloud));
n3d_.compute (cloud_normals);
cloud_normals_.reset (new pcl::PointCloud<pcl::Normal> (cloud_normals));
// TODO fabs? ... use parallel to Z.cross(X)!
//z axis in Kinect frame
btVector3 axis(0.0, 0.0, 1.0);
//rotate axis around x in Kinect frame for an angle between base_link and head_tilt_link + 90deg
//todo: get angle automatically
btVector3 axis2 = axis.rotate(btVector3(1.0, 0.0, 0.0), btScalar(base_link_head_tilt_link_angle_ + pcl::deg2rad(90.0)));
//std::cerr << "axis: " << fabs(axis2.getX()) << " " << fabs(axis2.getY()) << " " << fabs(axis2.getZ()) << std::endl;
seg_.setInputCloud (boost::make_shared<PointCloud> (cloud));
seg_.setInputNormals (cloud_normals_);
seg_.setAxis (Eigen::Vector3f(fabs(axis2.getX()), fabs(axis2.getY()), fabs(axis2.getZ())));
// seg_.setIndices (boost::make_shared<pcl::PointIndices> (selection));
seg_.segment (table_inliers, table_coeff);
ROS_INFO ("[%s] Table model: [%f, %f, %f, %f] with %d inliers.", getName ().c_str (),
table_coeff.values[0], table_coeff.values[1], table_coeff.values[2], table_coeff.values[3], (int)table_inliers.indices.size ());
if ((int)table_inliers.indices.size () <= min_table_inliers_)
{
ROS_ERROR ("table has to few inliers");
return;
}
// Project the table inliers using the planar model coefficients
proj_.setInputCloud (boost::make_shared<PointCloud> (cloud));
proj_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
proj_.setModelCoefficients (boost::make_shared<pcl::ModelCoefficients> (table_coeff));
proj_.filter (cloud_projected);
//cloud_pub_.publish (cloud_projected);
// Create a Convex Hull representation of the projected inliers
chull_.setInputCloud (boost::make_shared<PointCloud> (cloud_projected));
chull_.reconstruct (cloud_hull);
ROS_INFO ("Convex hull has: %d data points.", (int)cloud_hull.points.size ());
hull_pub_.publish (cloud_hull);
// //Compute the area of the plane
// std::vector<double> plane_normal(3);
// plane_normal[0] = table_coeff.values[0];
// plane_normal[1] = table_coeff.values[1];
// plane_normal[2] = table_coeff.values[2];
// area_ = compute2DPolygonalArea (cloud_hull, plane_normal);
// //ROS_INFO("[%s] Plane area: %f", getName ().c_str (), area_);
// if (area_ > (expected_table_area_ + 0.01))
// {
// extract_.setInputCloud (boost::make_shared<PointCloud> (cloud));
// extract_.setIndices (boost::make_shared<pcl::PointIndices> (table_inliers));
// extract_.setNegative (true);
// pcl::PointCloud<Point> cloud_extracted;
// extract_.filter (cloud_extracted);
// //cloud_extracted_pub_.publish (cloud_extracted);
// cloud = cloud_extracted;
// }
//end while
//ROS_INFO ("[%s] Publishing convex hull with: %d data points and area %lf.", getName ().c_str (), (int)cloud_hull.points.size (), area_);
//cloud_pub_.publish (cloud_hull);
// pcl::PointXYZRGB point_min;
// pcl::PointXYZRGB point_max;
// pcl::PointXYZ point_center;
// pcl::getMinMax3D (cloud_hull, point_min, point_max);
// //Calculate the centroid of the hull
// point_center.x = (point_max.x + point_min.x)/2;
// point_center.y = (point_max.y + point_min.y)/2;
// point_center.z = (point_max.z + point_min.z)/2;
// tf::Transform transform;
// transform.setOrigin( tf::Vector3(point_center.x, point_center.y, point_center.z));
// transform.setRotation( tf::Quaternion(0, 0, 0) );
// transform_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
// cloud_raw.header.frame_id, rot_table_frame_));
// ---[ Get the objects on top of the table - from the raw cloud!
pcl::PointIndices cloud_object_indices;
prism_.setHeightLimits (cluster_min_height_, cluster_max_height_);
prism_.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
prism_.setInputPlanarHull (boost::make_shared<PointCloud>(cloud_hull));
prism_.segment (cloud_object_indices);
//ROS_INFO ("[%s] Number of object point indices: %d.", getName ().c_str (), (int)cloud_object_indices.indices.size ());
pcl::PointCloud<Point> cloud_object;
pcl::ExtractIndices<Point> extract_object_indices;
//extract_object_indices.setInputCloud (cloud_all_minus_table_ptr);
extract_object_indices.setInputCloud (boost::make_shared<PointCloud> (cloud_raw));
extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
extract_object_indices.filter (cloud_object);
//ROS_INFO ("[%s ] Publishing number of object point candidates: %d.", getName ().c_str (), (int)cloud_objects.points.size ());
std::vector<pcl::PointIndices> clusters;
cluster_.setInputCloud (boost::make_shared<PointCloud>(cloud_object));
cluster_.setClusterTolerance (object_cluster_tolerance_);
cluster_.setMinClusterSize (object_cluster_min_size_);
cluster_.setMaxClusterSize (object_cluster_max_size_);
cluster_.setSearchMethod (clusters_tree_);
cluster_.extract (clusters);
// TODO take closest to ray
for (size_t i = 0; i < clusters.size (); i++)
{
// compute center and see if it is close enough to ray
int cluster_center = clusters[i].indices[clusters[i].indices.size () / 2];
Eigen::Vector4f pt = Eigen::Vector4f (cloud_object.points[cluster_center].x, cloud_object.points[cluster_center].y, cloud_object.points[cluster_center].z, 0);
Eigen::Vector4f c = line_dir_.cross3 (line_point_ - pt); c[3] = 0;
#ifndef TEST
if (c.squaredNorm () / line_dir_squaredNorm_ > 0.25*0.25) // further then 10cm
continue;
#endif
std::cerr << "found cluster" << std::endl;
// TODO make more optimal? + ERRORS/INCONSISTENCY IN PCL: second formula and in c computation!
//tf::transformPoint();
//Eigen::Matrix4f transform_matrix;
//pcl_ros::transformAsMatrix (c2h_transform, transform_matrix);
// broadcast transform
// tf::Transform fixed_transform;
// fixed_transform.setOrigin (tf::Vector3(0.0, 0.0, 0.1));
// fixed_transform.setRotation (tf::Quaternion(0, 0, 0));
// transform_broadcaster_.sendTransform(tf::StampedTransform(fixed_transform, ros::Time::now(), "right_hand", "object_frame"));
// transform object into right_hand frame
pcl::PointCloud<Point> cloud_object_clustered ;
pcl::copyPointCloud (cloud_object, clusters[i], cloud_object_clustered);
// Eigen::Matrix4f eigen_transform;
// pcl_ros::transformAsMatrix (c2h_transform, eigen_transform);
// pcl::transformPointCloud(cloud_object_clustered, output_cloud_, eigen_transform);
// output_cloud_.header.frame_id = ""right_hand";
sensor_msgs::PointCloud2 cluster;
pcl::toROSMsg (cloud_object_clustered, cluster);
// template <typename PointT> void transformPointCloud (const pcl::PointCloud <PointT> &cloud_in, pcl::PointCloud <PointT> &cloud_out, const tf::Transform &transform);
// void transformPointCloud (const std::string &target_frame, const tf::Transform &net_transform, const sensor_msgs::PointCloud2 &in, sensor_msgs::PointCloud2 &out);
pcl_ros::transformPointCloud("right_hand", (tf::Transform)c2h_transform, cluster, output_cloud_);
output_cloud_.header.frame_id = "right_hand"; // TODO needed?
// for (unsigned cp = 0; cp < output_cloud_.points.size (); cp++)
// {
// output_cloud_.points[i].x -= output_cloud_.points[0].x;
// output_cloud_.points[i].y -= output_cloud_.points[0].y;
// output_cloud_.points[i].z -= output_cloud_.points[0].z;
// }
// cloud_objects_pub_.publish (output_cloud_);
object_pub_.publish (output_cloud_);
ROS_INFO("Published object with %d points", (int)clusters[i].indices.size ());
// TODO get a nicer rectangle around the object
unsigned center_idx = cloud_object_indices.indices.at (cluster_center);
unsigned row = center_idx / 640;
unsigned col = center_idx - row * 640;
ros::ServiceClient client = nh_.serviceClient<kinect_cleanup::FilterObject>("/filter_object");
kinect_cleanup::FilterObject srv;
srv.request.min_row = std::max (0, (int)row-50);
srv.request.min_col = std::max (0, (int)col-40);
srv.request.max_row = std::min (479, (int)row+50);
srv.request.max_col = std::min (479, (int)col+40);
srv.request.rgb = cloud_raw.points[640 * srv.request.max_row + srv.request.max_col].rgb;
for (int i=0; i<4; i++)
srv.request.plane_normal[i] = table_coeff.values[i];
if (client.call(srv))
ROS_INFO("Object filter service responded: %s", srv.response.error.c_str ());
else
ROS_ERROR("Failed to call object filter service!");
// take only 1 cluster into account
break;
}
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
{
// Compute the Cartesian difference between the two points
Eigen::Vector4f delta = cloud.points[q_idx].getVector4fMap () - cloud.points[p_idx].getVector4fMap ();
delta[3] = 0;
// Compute the Euclidean norm = || p_idx - q_idx ||
float distance_sqr = delta.squaredNorm ();
if (distance_sqr == 0)
{
ROS_ERROR ("Euclidean distance between points %d and %d is 0!", p_idx, q_idx);
f1 = f2 = f3 = f4 = 0;
return (false);
}
// Estimate f4 = || delta ||
f4 = sqrt (distance_sqr);
// Create a Darboux frame coordinate system u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
pcl::Vector4fMapConst u = normals.points[p_idx].getNormalVector4fMap ();
// Estimate f3 = u * delta / || delta ||
// delta[3] = 0 (line 59)
f3 = u.dot (delta) / f4;
// v = delta * u
Eigen::Vector4f v = Eigen::Vector4f::Zero ();
v = delta.cross3 (u);
distance_sqr = v.squaredNorm ();
if (distance_sqr == 0)
{
ROS_ERROR ("Norm of Delta x U is 0 for point %d and %d!", p_idx, q_idx);
f1 = f2 = f3 = f4 = 0;
return (false);
}
// Copy the q_idx normal
Eigen::Vector4f nq (normals.points[q_idx].normal_x,
normals.points[q_idx].normal_y,
normals.points[q_idx].normal_z,
0);
// Normalize the vector
v /= sqrt (distance_sqr);
// Compute delta (w) = u x v
delta = u.cross3 (v);
// Compute f2 = v * n2;
// v[3] = 0 (line 82)
f2 = v.dot (nq);
// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
// delta[3] = 0 (line 59), nq[3] = 0 (line 97)
f1 = atan2f (delta.dot (nq), u.dot (nq)); // @todo: optimize this
return (true);
}