bool SnapIt::processModelPlane(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
// first build plane model
geometry_msgs::PolygonStamped target_plane = req.request.target_plane;
// check the size of the points
if (target_plane.polygon.points.size() < 3) {
NODELET_ERROR("not enough points included in target_plane");
return false;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
EigenVector3fVector points;
Eigen::Vector3f n, p;
if (!extractPointsInsidePlanePole(target_plane, inliers, points, n, p)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*points_inside_pole);
publishPointCloud(debug_candidate_points_pub_, points_inside_pole);
// estimate plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(points_inside_pole, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a planar model for the given dataset.");
return false;
}
// extract plane points
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setIndices (plane_inliers);
proj.setInputCloud (points_inside_pole);
proj.setModelCoefficients (plane_coefficients);
proj.filter (*plane_points);
publishPointCloud(debug_candidate_points_pub3_, plane_points);
// next, compute convexhull and centroid
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud (plane_points);
chull.setDimension(2);
chull.setAlpha (0.1);
chull.reconstruct (*cloud_hull);
if (cloud_hull->points.size() < 3) {
NODELET_ERROR("failed to estimate convex hull");
return false;
}
publishConvexHullMarker(cloud_hull);
Eigen::Vector4f C_new_4f;
pcl::compute3DCentroid(*cloud_hull, C_new_4f);
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = C_new_4f[i];
}
Eigen::Vector3f n_prime;
n_prime[0] = plane_coefficients->values[0];
n_prime[1] = plane_coefficients->values[1];
n_prime[2] = plane_coefficients->values[2];
plane_coefficients->values[3] = plane_coefficients->values[3] / n_prime.norm();
n_prime.normalize();
if (n_prime.dot(n) < 0) {
n_prime = - n_prime;
plane_coefficients->values[3] = - plane_coefficients->values[3];
}
Eigen::Vector3f n_cross = n.cross(n_prime);
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
// compute C
Eigen::Vector3f C_orig(0, 0, 0);
for (size_t i = 0; i < points.size(); i++) {
C_orig = C_orig + points[i];
}
C_orig = C_orig / points.size();
// compute C
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = target_plane.header;
debug_centroid_pub_.publish(centroid);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new[0];
centroid_transformed.point.y = C_new[1];
centroid_transformed.point.z = C_new[2];
centroid_transformed.header = target_plane.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
return true;
}
void rgb_pcl::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
// Container for original & filtered data
#if PR2
if(target_frame.find(base_frame) == std::string::npos){
getTransformCloud(input, *input);
}
sensor_msgs::PointCloud2 in = *input;
sensor_msgs::PointCloud2 out;
pcl_ros::transformPointCloud(target_frame, net_transform, in, out);
#endif
// ROS_INFO("Cloud acquired...");
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>),
cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
Mat displayImage = cv::Mat(Size(640, 480), CV_8UC3);
displayImage = Scalar(120);
// Convert to PCL data type
#if PR2
pcl_conversions::toPCL(out, *cloud);
#endif
#if !PR2
pcl_conversions::toPCL(*input, *cloud);
#endif
// ROS_INFO("\t=>Cloud rotated...");
// Perform the actual filtering
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.005, 0.005, 0.005);
sor.filter (*cloud_filtered_blob);
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
ModelCoefficientsPtr coefficients (new pcl::ModelCoefficients);
PointCloudPtr plane_points(new PointCloud), point_points_2d_hull(new PointCloud);
std::vector<PointCloudPtr> object_clouds;
pcl::PointCloud<pcl::PointXYZRGB> combinedCloud;
#if PR2
make_crop_box_marker(marker_publisher, base_frame, 0, 0.2, -1, 0.2, 1.3, 2, 1.3);
// Define your cube with two points in space:
Eigen::Vector4f minPoint;
minPoint[0]=0.2; // define minimum point x
minPoint[1]=-1; // define minimum point y
minPoint[2]=0.2; // define minimum point z
Eigen::Vector4f maxPoint;
maxPoint[0]=1.5; // define max point x
maxPoint[1]=1; // define max point y
maxPoint[2]=1.5; // define max point z
pcl::CropBox<pcl::PointXYZRGB> cropFilter;
cropFilter.setInputCloud (cloud_filtered);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.filter (*cloud_filtered);
#endif
#if !PR2
//Rotate the point cloud
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
// Define a rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
float theta = M_PI; // The angle of rotation in radians
// Define a translation of 0 meters on the x axis
transform_1.translation() << 0.0, 0.0, 1.0;
// The same rotation matrix as before; tetha radians arround X axis
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
// Executing the transformation
pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, transform_1);
#endif
interpretTableScene(cloud_filtered, coefficients, plane_points, point_points_2d_hull, object_clouds);
int c = 0;
#if PUBLISH_CLOUDS
int ID_object = -1;
#endif
for(auto cloudCluster: object_clouds){
// get_cloud_matching(cloudCluster); //histogram matching
#if PUBLISH_CLOUDS
ID_object = c;
#endif
combinedCloud += *cloudCluster;
combinedCloud.header = cloud_filtered->header;
c++;
}
#if DISPLAY
drawPointCloud(combinedCloud, displayImage);
#endif
getTracker(object_clouds, displayImage);
stateDetection();
// ROS_INFO("\t=>Cloud analysed...");
#if PUBLISH_CLOUDS
sensor_msgs::PointCloud2 output;
if(object_clouds.size() >= ID_object && ID_object >= 0){
pcl::toROSMsg(combinedCloud, output);
// Publish the data
pub.publish (output);
}
#endif
end = ros::Time::now();
std::stringstream ss;
ss <<(end-begin);
string s_FPS = ss.str();
#if DISPLAY
cv::putText(displayImage, "FPS: "+to_string((int)1/(stof(s_FPS))) + " Desired: "+to_string(DESIRED_FPS), cv::Point(10, 10), CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(0,0,0));
imshow("RGB", displayImage);
#endif
waitKey(1);
begin = ros::Time::now();
}
