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 }
