void cloud_cb( const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud) { iter++; if(iter != skip) return; iter = 0; pcl::PointCloud<pcl::PointXYZRGB> cloud_transformed, cloud_aligned, cloud_filtered; Eigen::Affine3f view_transform; view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; Eigen::AngleAxis<float> rot(tilt * M_PI / 180, Eigen::Vector3f(0, 1, 0)); view_transform.prerotate(rot); pcl::transformPointCloud(*cloud, cloud_transformed, view_transform); pcl::ModelCoefficients::Ptr coefficients( new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::SACSegmentation<pcl::PointXYZRGB> seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.05); seg.setProbability(0.99); seg.setInputCloud(cloud_transformed.makeShared()); seg.segment(*inliers, *coefficients); pcl::ExtractIndices<pcl::PointXYZRGB> extract; extract.setInputCloud(cloud_transformed.makeShared()); extract.setIndices(inliers); extract.setNegative(true); extract.filter(cloud_transformed); std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; Eigen::Vector3f z_current(coefficients->values[0], coefficients->values[1], coefficients->values[2]); Eigen::Vector3f y(0, 1, 0); Eigen::Affine3f rotation; rotation = pcl::getTransFromUnitVectorsZY(z_current, y); rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3])); pcl::transformPointCloud(cloud_transformed, cloud_aligned, rotation); Eigen::Affine3f res = (rotation * view_transform); cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1); cloud_aligned.sensor_orientation_ = res.rotate( Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation(); seg.setInputCloud(cloud_aligned.makeShared()); seg.segment(*inliers, *coefficients); std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; pub.publish(cloud_aligned); }
int main(int argc, char** argv) { if (argc < 5) { PCL17_INFO ("Usage %s -input_file /in_file -output_file /out_file [options]\n", argv[0]); PCL17_INFO (" * where options are:\n" " -tilt <X> : tilt. Default : 30\n" ""); return -1; } int tilt = 30; std::string input_file; std::string output_file; pcl17::console::parse_argument(argc, argv, "-input_file", input_file); pcl17::console::parse_argument(argc, argv, "-output_file", output_file); pcl17::console::parse_argument(argc, argv, "-tilt", tilt); pcl17::PointCloud<pcl17::PointXYZRGB> cloud, cloud_transformed, cloud_aligned, cloud_filtered; pcl17::io::loadPCDFile(input_file, cloud); Eigen::Affine3f view_transform; view_transform.matrix() << 0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; Eigen::AngleAxis<float> rot(tilt * M_PI / 180, Eigen::Vector3f(0, 1, 0)); view_transform.prerotate(rot); pcl17::transformPointCloud(cloud, cloud_transformed, view_transform); pcl17::ModelCoefficients::Ptr coefficients(new pcl17::ModelCoefficients); pcl17::PointIndices::Ptr inliers(new pcl17::PointIndices); pcl17::SACSegmentation<pcl17::PointXYZRGB> seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl17::SACMODEL_PLANE); seg.setMethodType(pcl17::SAC_RANSAC); seg.setDistanceThreshold(0.05); seg.setProbability(0.99); seg.setInputCloud(cloud_transformed.makeShared()); seg.segment(*inliers, *coefficients); pcl17::ExtractIndices<pcl17::PointXYZRGB> extract; extract.setInputCloud(cloud_transformed.makeShared()); extract.setIndices(inliers); extract.setNegative(true); extract.filter(cloud_transformed); std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; Eigen::Vector3f z_current(coefficients->values[0], coefficients->values[1], coefficients->values[2]); Eigen::Vector3f y(0, 1, 0); Eigen::Affine3f rotation; rotation = pcl17::getTransFromUnitVectorsZY(z_current, y); rotation.translate(Eigen::Vector3f(0, 0, coefficients->values[3])); pcl17::transformPointCloud(cloud_transformed, cloud_aligned, rotation); Eigen::Affine3f res = (rotation * view_transform); cloud_aligned.sensor_origin_ = res * Eigen::Vector4f(0, 0, 0, 1); cloud_aligned.sensor_orientation_ = res.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation(); seg.setInputCloud(cloud_aligned.makeShared()); seg.segment(*inliers, *coefficients); std::cout << "Z vector: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl; pcl17::io::savePCDFile(output_file, cloud_aligned); return 0; }