int
main(int, char** argv)
{
std::string filename = argv[1];
std::cout << "Reading " << filename << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) // load the file
{
PCL_ERROR ("Couldn't read file");
return -1;
}
std::cout << "points: " << cloud_xyz->points.size () <<std::endl;
// Parameters for sift computation
const float min_scale = 0.005f;
const int n_octaves = 6;
const int n_scales_per_octave = 4;
const float min_contrast = 0.005f;
// Estimate the sift interest points using z values from xyz as the Intensity variants
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;
pcl::PointCloud<pcl::PointWithScale> result;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
sift.setSearchMethod(tree);
sift.setScales(min_scale, n_octaves, n_scales_per_octave);
sift.setMinimumContrast(min_contrast);
sift.setInputCloud(cloud_xyz);
sift.compute(result);
std::cout << "No of SIFT points in the result are " << result.points.size () << std::endl;
/*
// Copying the pointwithscale to pointxyz so as visualize the cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_temp (new pcl::PointCloud<pcl::PointXYZ>);
copyPointCloud(result, *cloud_temp);
std::cout << "SIFT points in the result are " << cloud_temp->points.size () << std::endl;
// Visualization of keypoints along with the original cloud
pcl::visualization::PCLVisualizer viewer("PCL Viewer");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> keypoints_color_handler (cloud_temp, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color_handler (cloud_xyz, 255, 0, 0);
viewer.setBackgroundColor( 0.0, 0.0, 0.0 );
viewer.addPointCloud(cloud_xyz, cloud_color_handler, "cloud");
viewer.addPointCloud(cloud_temp, keypoints_color_handler, "keypoints");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");
while(!viewer.wasStopped ())
{
viewer.spinOnce ();
}
*/
return 0;
}
int
main (int argc, char** argv)
{
bool refine = pcl::console::find_switch (argc, argv, "-refine");
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile (argv[1], *cloud_xyz);
PCDOrganizedMultiPlaneSegmentation<pcl::PointXYZ> multi_plane_app (cloud_xyz, refine);
multi_plane_app.run ();
return 0;
}
void CloudViewer::on_cloud_xyzsift() {
CLOG(LTRACE) << "CloudViewer::on_cloud_xyzsift";
pcl::PointCloud<PointXYZSIFT>::Ptr cloud = in_cloud_xyzsift.read();
// Filter the NaN points.
std::vector<int> indices;
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
// Transform to XYZ cloud.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *cloud_xyz);
// Update cloud.
viewer->updatePointCloud<pcl::PointXYZ> (cloud_xyz,"xyzsift");
// Update cloud properties.
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, prop_sift_size,"xyzsift");
}
void PCDReader::Read() {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyz.write(cloud_xyz);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud_xyzrgb) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyzrgb.write(cloud_xyzrgb);
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift (new pcl::PointCloud<PointXYZSIFT>);
if (pcl::io::loadPCDFile<PointXYZSIFT> (filename, *cloud_xyzsift) == -1) //* load the file
{
cout <<"Błąd"<<endl;
}
out_cloud_xyzsift.write(cloud_xyzsift);
}
void PCDReader::Read() {
CLOG(LTRACE) << "PCDReader::Read\n";
// Try to read the cloud of XYZ points.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
/*
if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud_xyz) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZ cloud from "<<filename;
}else{
out_cloud_xyz.write(cloud_xyz);
CLOG(LINFO) <<"PointXYZ size: "<<cloud_xyz->size();
CLOG(LINFO) <<"PointXYZ cloud loaded properly from "<<filename;
//return;
}// else
*/
// Try to read the cloud of XYZRGB points.
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb (new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (filename, *cloud_xyzrgb) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZRGB cloud from "<<filename;
}else{
out_cloud_xyzrgb.write(cloud_xyzrgb);
CLOG(LINFO) <<"PointXYZRGB cloud loaded properly from "<<filename;
//return;
}// else
// Try to read the cloud of XYZSIFT points.
pcl::PointCloud<PointXYZSIFT>::Ptr cloud_xyzsift (new pcl::PointCloud<PointXYZSIFT>);
if (pcl::io::loadPCDFile<PointXYZSIFT> (filename, *cloud_xyzsift) == -1){
CLOG(LWARNING) <<"Cannot read PointXYZSIFT cloud from "<<filename;
}else{
out_cloud_xyzsift.write(cloud_xyzsift);
CLOG(LINFO) <<"PointXYZSIFT cloud loaded properly from "<<filename;
//return;
}// else
}
linkerList linking(CloudPtr input_cloud, const std::vector< pcl::PointIndices >& input_clusters, CloudPtr target_cloud, const std::vector< pcl::PointIndices >& target_clusters)
{
linkerList result;
// compute centroids of input_cloud
std::vector<Eigen::Vector4f,Eigen::aligned_allocator<Eigen::Vector4f> > centroid;
computeObjCentroid(input_cloud, input_clusters, centroid);
// cloud_xyz: convert target_cloud to pointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
// kdtree initialization: Neighbors within radius search
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
for(int i=0;i<input_clusters.size();i++)
{
Eigen::Vector4f centroid_i=centroid[i];
pcl::PointXYZ searchPoint;
searchPoint.x = centroid_i[0];
searchPoint.y = centroid_i[1];
searchPoint.z = centroid_i[2];
float overlap_ratio=0;
float overlap_within_new_cluster;
std::vector<int> link_index_i;
std::vector<float> overlap_ratio_i;
for(int j=0; j<target_clusters.size(); j++)
{
float radius = 0.05;
cloud_xyz.reset(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*target_cloud, target_clusters[j], *cloud_xyz);
kdtree.setInputCloud(cloud_xyz);
kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);
overlap_ratio = (float)pointIdxRadiusSearch.size()/input_clusters[i].indices.size();
overlap_within_new_cluster = (float)pointIdxRadiusSearch.size()/target_clusters[j].indices.size();
if( overlap_ratio > 0.0f)
{
link_index_i.push_back(j);
overlap_ratio_i.push_back(overlap_ratio);
}
if(VERBOSE)
{
std::cout << "pre_cluster[" << i << "] = " << input_clusters[i].indices.size() << " | ";
std::cout << "cur_cluster[" << j << "] = " << target_clusters[j].indices.size() << " | ";
std::cout << "radius search got " << pointIdxRadiusSearch.size() << " points | ";
std::cout << "overlap ratio [" << i << "," << j <<"] = " << overlap_ratio << " | ";
std::cout << "overlap ratio with new cluster [" << i << "," << j <<"] = " << overlap_within_new_cluster << std::endl;
}
}
if(!link_index_i.empty())
{
linker linker_i;
linker_i.link_index = link_index_i;
linker_i.overlap_ratio = overlap_ratio_i;
result.push_back(linker_i);
}
}
return result;
}
void CloudConverter::convert_xyzshot() {
pcl::PointCloud<PointXYZSHOT>::Ptr cloud_xyzshot = in_cloud_xyzshot.read();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*cloud_xyzshot, *cloud_xyz);
out_cloud_xyz.write(cloud_xyz);
}
void CloudConverter::convert_xyzrgb() {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_xyzrgb = in_cloud_xyzrgb.read();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>());
pcl::copyPointCloud(*cloud_xyzrgb, *cloud_xyz);
out_cloud_xyz.write(cloud_xyz);
}
int
main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("../learn15.pcd", *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read file test_pcd.pcd \n");
return (-1);
}
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.01);
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(true);
//Removes part_of_cloud but retain the original full_cloud
//extract.setNegative(true); // Removes part_of_cloud from full cloud and keep the rest
extract.filter(*cloud_plane);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud(cloud_plane);
sor.setMeanK(50);
sor.setStddevMulThresh(1.0);
sor.filter(*cloud_filtered);
//cloud.swap(cloud_plane);
/*
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud_plane);
while (!viewer.wasStopped())
{
}
*/
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
Eigen::Matrix<float, 1, 3> fitted_plane_norm, xyaxis_plane_norm, rotation_axis;
fitted_plane_norm[0] = coefficients->values[0];
fitted_plane_norm[1] = coefficients->values[1];
fitted_plane_norm[2] = coefficients->values[2];
xyaxis_plane_norm[0] = 0.0;
xyaxis_plane_norm[1] = 0.0;
xyaxis_plane_norm[2] = 1.0;
rotation_axis = xyaxis_plane_norm.cross(fitted_plane_norm);
float theta = acos(xyaxis_plane_norm.dot(fitted_plane_norm));
//float theta = -atan2(rotation_axis.norm(), xyaxis_plane_norm.dot(fitted_plane_norm));
transform_2.rotate(Eigen::AngleAxisf(theta, rotation_axis));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_recentered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::transformPointCloud(*cloud_filtered, *cloud_recentered, transform_2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud_recentered, *cloud_xyz);
///////////////////////////////////////////////////////////////////
Eigen::Vector4f pcaCentroid;
pcl::compute3DCentroid(*cloud_xyz, pcaCentroid);
Eigen::Matrix3f covariance;
computeCovarianceMatrixNormalized(*cloud_xyz, pcaCentroid, covariance);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
std::cout << eigenVectorsPCA.size() << std::endl;
if(eigenVectorsPCA.size()!=9)
eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
projectionTransform.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();
projectionTransform.block<3, 1>(0, 3) = -1.f * (projectionTransform.block<3, 3>(0, 0) * pcaCentroid.head<3>());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudPointsProjected(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud_xyz, *cloudPointsProjected, projectionTransform);
// Get the minimum and maximum points of the transformed cloud.
pcl::PointXYZ minPoint, maxPoint;
pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = rgbVis(cloud);
// viewer->addPointCloud<pcl::PointXYZ>(cloudPointsProjected, "Simple Cloud");
// viewer->addPointCloud<pcl::PointXYZRGB>(cloud, "Simple Cloud2");
viewer->addCube(minPoint.x, maxPoint.x, minPoint.y, maxPoint.y, minPoint.z , maxPoint.z);
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
/*
pcl::PCA< pcl::PointXYZ > pca;
pcl::PointCloud< pcl::PointXYZ >::ConstPtr cloud;
pcl::PointCloud< pcl::PointXYZ > proj;
pca.setInputCloud(cloud);
pca.project(*cloud, proj);
Eigen::Vector4f proj_min;
Eigen::Vector4f proj_max;
pcl::getMinMax3D(proj, proj_min, proj_max);
pcl::PointXYZ min;
pcl::PointXYZ max;
pca.reconstruct(proj_min, min);
pca.reconstruct(proj_max, max);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer->addCube(min.x, max.x, min.y, max.y, min.z, max.z);
*/
return (0);
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input) {
// std::cerr << "in cloud_cb" << std::endl;
/* 0. Importing input cloud */
std_msgs::Header header = input->header;
// std::string frame_id = input->header.frame_id;
// sensor_msgs::PointCloud2 input_cloud = *input;
pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2; // initialize object
pcl_conversions::toPCL(*input, *cloud); // from input, generate content of cloud
/* 1. Downsampling and Publishing voxel */
// LeafSize: should small enough to caputure a leaf of plants
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); // imutable pointer
pcl::PCLPointCloud2 cloud_voxel; // cloud_filtered to cloud_voxel
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr); // set input
sor.setLeafSize(0.02f, 0.02f, 0.02f); // 2cm, model equation
sor.filter(cloud_voxel); // set output
sensor_msgs::PointCloud2 output_voxel;
pcl_conversions::fromPCL(cloud_voxel, output_voxel);
pub_voxel.publish(output_voxel);
/* 2. Filtering with RANSAC */
// RANSAC initialization
pcl::SACSegmentation<pcl::PointXYZ> seg; // Create the segmentation object
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
seg.setOptimizeCoefficients(true); // Optional
// seg.setModelType(pcl::SACMODEL_PLANE);
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); // Use SACMODEL_PERPENDICULAR_PLANE instead
seg.setMethodType(pcl::SAC_RANSAC);
// minimum number of points calculated from N and distanceThres
seg.setMaxIterations (1000); // N in RANSAC
// seg.setDistanceThreshold(0.025); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
seg.setDistanceThreshold(0.050); // 0.01 - 0.05 default: 0.02 // 閾値（しきい値）
// param for perpendicular
seg.setAxis(Eigen::Vector3f (0.0, 0.0, 1.0));
seg.setEpsAngle(pcl::deg2rad (10.0f)); // 5.0 -> 10.0
// convert from PointCloud2 to PointXYZ
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_voxel_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_voxel, *cloud_voxel_xyz);
// RANSAC application
seg.setInputCloud(cloud_voxel_xyz);
seg.segment(*inliers, *coefficients); // values are empty at beginning
// inliers.indices have array index of the points which are included as inliers
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_xyz (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = inliers->indices.begin ();
pit != inliers->indices.end (); pit++) {
cloud_plane_xyz->points.push_back(cloud_voxel_xyz->points[*pit]);
}
// Organized as an image-structure
cloud_plane_xyz->width = cloud_plane_xyz->points.size ();
cloud_plane_xyz->height = 1;
/* insert code to set arbitary frame_id setting
such as frame_id ="/assemble_cloud_1"
with respect to "/odom or /base_link" */
// Conversions: PointCloud<T>, PCLPointCloud2, sensor_msgs::PointCloud2
pcl::PCLPointCloud2 cloud_plane_pcl;
pcl::toPCLPointCloud2(*cloud_plane_xyz, cloud_plane_pcl);
sensor_msgs::PointCloud2 cloud_plane_ros;
pcl_conversions::fromPCL(cloud_plane_pcl, cloud_plane_ros);
// cloud_plane_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_plane_ros.header.frame_id = header.frame_id;
cloud_plane_ros.header.stamp = header.stamp; // ros::Time::now() -> header.stamp
pub_plane.publish(cloud_plane_ros);
/* 3. PCA application to get eigen */
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(cloud_plane_xyz);
Eigen::Matrix3f eigen_vectors = pca.getEigenVectors(); // 3x3 eigen_vectors(n,m)
/* 4. PCA Visualization */
visualization_msgs::Marker points;
// points.header.frame_id = "/base_link"; // odom -> /base_link
points.header.frame_id = header.frame_id;
points.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
points.ns = "pca"; // namespace + id
points.id = 0; // pca/0
points.action = visualization_msgs::Marker::ADD;
points.type = visualization_msgs::Marker::ARROW;
points.pose.orientation.w = 1.0;
points.scale.x = 0.05;
points.scale.y = 0.05;
points.scale.z = 0.05;
points.color.g = 1.0f;
points.color.r = 0.0f;
points.color.b = 0.0f;
points.color.a = 1.0;
geometry_msgs::Point p_0, p_1;
p_0.x = 0; p_0.y = 0; p_0.z = 0; // get from tf
p_1.x = eigen_vectors(0,0);
p_1.y = eigen_vectors(0,1); // always negative
std::cerr << "y = " << eigen_vectors(0,1) << std::endl;
p_1.z = eigen_vectors(0,2);
points.points.push_back(p_0);
points.points.push_back(p_1);
pub_marker.publish(points);
/* 5. Point Cloud Rotation */
eigen_vectors(0,2) = 0; // ignore very small z-value
double norm = pow((pow(eigen_vectors(0,0), 2) + pow(eigen_vectors(0,1), 2)), 0.5);
double nx = eigen_vectors(0,0) / norm;
double ny = eigen_vectors(0,1) / norm;
Eigen::Matrix4d rot_z; // rotation inversed, convert to Matrix4f
rot_z(0,0) = nx; rot_z(0,1) = ny; rot_z(0,2) = 0; rot_z(0,3) = 0; // ny: +/-
rot_z(1,0) = -ny; rot_z(1,1) = nx; rot_z(1,2) = 0; rot_z(1,3) = 0; // ny: +/-
rot_z(2,0) = 0; rot_z(2,1) = 0; rot_z(2,2) = 1; rot_z(2,3) = 0;
rot_z(3,0) = 0; rot_z(3,1) = 0; rot_z(3,2) = 0; rot_z(3,3) = 1;
// Transformation: Rotation, Translation
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_rot (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(*cloudPtr, *cloud_xyz); // from PointCloud2 to PointXYZ
pcl::transformPointCloud(*cloud_xyz, *cloud_xyz_rot, rot_z); // original, transformed, transformation
pcl::PCLPointCloud2 cloud_rot_pcl;
sensor_msgs::PointCloud2 cloud_rot_ros;
pcl::toPCLPointCloud2(*cloud_xyz_rot, cloud_rot_pcl);
pcl_conversions::fromPCL(cloud_rot_pcl, cloud_rot_ros);
pub_rot.publish(cloud_rot_ros);
/* 6. Point Cloud Reduction */
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr >
vector_cloud_separated_xyz = separate(cloud_xyz_rot, header);
pcl::PCLPointCloud2 cloud_separated_pcl;
sensor_msgs::PointCloud2 cloud_separated_ros;
pcl::toPCLPointCloud2(*vector_cloud_separated_xyz.at(1), cloud_separated_pcl);
pcl_conversions::fromPCL(cloud_separated_pcl, cloud_separated_ros);
// cloud_separated_ros.header.frame_id = "/base_link"; // odom -> /base_link
cloud_separated_ros.header.frame_id = header.frame_id;
cloud_separated_ros.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
pub_red.publish(cloud_separated_ros);
// setMarker
// visualization_msgs::Marker width_min_line;
// width_min_line.header.frame_id = "/base_link";
// width_min_line.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// width_min_line.ns = "width_min";
// width_min_line.action = visualization_msgs::Marker::ADD;
// width_min_line.type = visualization_msgs::Marker::LINE_STRIP;
// width_min_line.pose.orientation.w = 1.0;
// width_min_line.id = 0;
// width_min_line.scale.x = 0.025;
// width_min_line.color.r = 0.0f;
// width_min_line.color.g = 1.0f;
// width_min_line.color.b = 0.0f;
// width_min_line.color.a = 1.0;
// width_min_line.points.push_back(point_vector.at(0));
// width_min_line.points.push_back(point_vector.at(2));
// pub_marker.publish(width_min_line);
// /* 6. Visualize center line */
// visualization_msgs::Marker line_strip;
// line_strip.header.frame_id = "/base_link"; // odom -> /base_link
// line_strip.header.stamp = input->header.stamp; // ros::Time::now() -> header.stamp
// line_strip.ns = "center";
// line_strip.action = visualization_msgs::Marker::ADD;
// line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// line_strip.pose.orientation.w = 1.0;
// line_strip.id = 0; // set id
// line_strip.scale.x = 0.05;
// line_strip.color.r = 1.0f;
// line_strip.color.g = 0.0f;
// line_strip.color.b = 0.0f;
// line_strip.color.a = 1.0;
// // geometry_msgs::Point p_stitch, p_min;
// p_s.x = 0; p_s.y = 0; p_s.z = 0;
// p_e.x = p_m.x; p_e.y = p_m.y; p_e.z = 0;
// line_strip.points.push_back(p_s);
// line_strip.points.push_back(p_e);
// pub_marker.publish(line_strip);
/* PCA Visualization */
// geometry_msgs::Pose pose; tf::poseEigenToMsg(pca.getEigenVectors, pose);
/* to use Pose marker in rviz */
/* Automatic Measurement */
// 0-a. stitch measurement: -0.5 < z < -0.3
// 0-b. min width measurement: 0.3 < z < 5m
// 1. iterate
// 2. pick point if y < 0
// 3. compare point with all points if 0 < y
// 4. pick point-pare recording shortest distance
// 5. compare the point with previous point
// 6. update min
// 7. display value in text in between 2 points
}
