convert_node.cpp
Go to the documentation of this file.
00001 
00002 
00003 #include <ros/ros.h>
00004 #include <pcl17/ModelCoefficients.h>
00005 #include <pcl17/io/pcd_io.h>
00006 #include <pcl17/point_types.h>
00007 #include <pcl17/sample_consensus/method_types.h>
00008 #include <pcl17/sample_consensus/model_types.h>
00009 #include <pcl17/segmentation/sac_segmentation.h>
00010 #include <pcl17/filters/extract_indices.h>
00011 #include <pcl17/common/transforms.h>
00012 #include <pcl17_ros/point_cloud.h>
00013 
00014 #include <pcl17/console/parse.h>
00015 
00016 class ConvertNode {
00017 public:
00018         ConvertNode(int tilt, int skip) {
00019 
00020                 this->tilt = tilt;
00021                 this->skip = skip;
00022                 this->iter = 0;
00023                 ros::NodeHandle nh;
00024 
00025                 pub = nh.advertise<pcl17::PointCloud<pcl17::PointXYZRGB> >("/cloud_pcd",
00026                                 1);
00027                 sub = nh.subscribe<pcl17::PointCloud<pcl17::PointXYZRGB> >(
00028                                 "/camera/depth_registered/points", 1, &ConvertNode::cloud_cb, this);
00029 
00030         }
00031 
00032         void cloud_cb(
00033                         const typename pcl17::PointCloud<pcl17::PointXYZRGB>::ConstPtr& cloud) {
00034 
00035                 iter++;
00036                 if(iter != skip) return;
00037                 iter = 0;
00038 
00039                 pcl17::PointCloud<pcl17::PointXYZRGB> cloud_transformed,
00040                                 cloud_aligned, cloud_filtered;
00041 
00042                 Eigen::Affine3f view_transform;
00043                 view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
00044 
00045                 Eigen::AngleAxis<float> rot(tilt * M_PI / 180,
00046                                 Eigen::Vector3f(0, 1, 0));
00047 
00048                 view_transform.prerotate(rot);
00049 
00050                 pcl17::transformPointCloud(*cloud, cloud_transformed, view_transform);
00051 
00052                 pcl17::ModelCoefficients::Ptr coefficients(
00053                                 new pcl17::ModelCoefficients);
00054                 pcl17::PointIndices::Ptr inliers(new pcl17::PointIndices);
00055 
00056                 pcl17::SACSegmentation<pcl17::PointXYZRGB> seg;
00057 
00058                 seg.setOptimizeCoefficients(true);
00059 
00060                 seg.setModelType(pcl17::SACMODEL_PLANE);
00061                 seg.setMethodType(pcl17::SAC_RANSAC);
00062                 seg.setDistanceThreshold(0.05);
00063                 seg.setProbability(0.99);
00064 
00065                 seg.setInputCloud(cloud_transformed.makeShared());
00066                 seg.segment(*inliers, *coefficients);
00067 
00068                 pcl17::ExtractIndices<pcl17::PointXYZRGB> extract;
00069 
00070                 extract.setInputCloud(cloud_transformed.makeShared());
00071                 extract.setIndices(inliers);
00072                 extract.setNegative(true);
00073                 extract.filter(cloud_transformed);
00074 
00075                 std::cout << "Z vector: " << coefficients->values[0] << " "
00076                                 << coefficients->values[1] << " " << coefficients->values[2]
00077                                 << " " << coefficients->values[3] << std::endl;
00078 
00079                 Eigen::Vector3f z_current(coefficients->values[0],
00080                                 coefficients->values[1], coefficients->values[2]);
00081                 Eigen::Vector3f y(0, 1, 0);
00082 
00083                 Eigen::Affine3f rotation;
00084                 rotation = pcl17::getTransFromUnitVectorsZY(z_current, y);
00085                 rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3]));
00086 
00087                 pcl17::transformPointCloud(cloud_transformed, cloud_aligned, rotation);
00088 
00089                 Eigen::Affine3f res = (rotation * view_transform);
00090 
00091                 cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1);
00092                 cloud_aligned.sensor_orientation_ = res.rotate(
00093                                 Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
00094 
00095                 seg.setInputCloud(cloud_aligned.makeShared());
00096                 seg.segment(*inliers, *coefficients);
00097 
00098                 std::cout << "Z vector: " << coefficients->values[0] << " "
00099                                 << coefficients->values[1] << " " << coefficients->values[2]
00100                                 << " " << coefficients->values[3] << std::endl;
00101 
00102                 pub.publish(cloud_aligned);
00103 
00104 
00105 
00106         }
00107 
00108         ros::Publisher pub;
00109         ros::Subscriber sub;
00110         int tilt;
00111         int skip;
00112         int iter;
00113 
00114 };
00115 
00116 int main(int argc, char **argv) {
00117 
00118         int tilt = 30;
00119 
00120         int skip = 30;
00121 
00122         pcl17::console::parse_argument(argc, argv, "-skip", skip);
00123         pcl17::console::parse_argument(argc, argv, "-tilt", tilt);
00124 
00125         ros::init(argc, argv, "convert_node");
00126         ConvertNode cn(tilt, skip);
00127         ros::spin();
00128 
00129         return 0;
00130 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Thu May 23 2013 18:32:28