pcl::CorrespondencesPtr CorrespondenceEstimation::estimateCorrespondences(pcl::PointCloud<PointXYZSIFT>::Ptr src_cloud_, pcl::PointCloud<PointXYZSIFT>::Ptr trg_cloud_) {
CLOG(LTRACE) << "estimateCorrespondences(src,trg)";
// Empty list of correspondences.
pcl::CorrespondencesPtr correspondences(new pcl::Correspondences());
// If pass_through - return empty list.
if (pass_through) {
CLOG(LDEBUG) << "Passthrough - returning no correspondences";
return correspondences;
}//: if passthrough
// Create object responsible for correspondence estimation.
pcl::registration::CorrespondenceEstimation<PointXYZSIFT, PointXYZSIFT>::Ptr correst(new pcl::registration::CorrespondenceEstimation<PointXYZSIFT, PointXYZSIFT>());
// Set feature representation.
SIFTFeatureRepresentation::Ptr point_representation(new SIFTFeatureRepresentation());
correst->setPointRepresentation(point_representation);
// Filter NaN points.
pcl::PointCloud<PointXYZSIFT>::Ptr filtered_src_cloud(new pcl::PointCloud<PointXYZSIFT>);
std::vector<int> src_indices;
pcl::removeNaNFromPointCloud(*src_cloud_, *filtered_src_cloud, src_indices);
//filtered_src_cloud->is_dense = false;
pcl::PointCloud<PointXYZSIFT>::Ptr filtered_trg_cloud(new pcl::PointCloud<PointXYZSIFT>);
std::vector<int> trg_indices;
pcl::removeNaNFromPointCloud(*trg_cloud_, *filtered_trg_cloud, trg_indices);
//filtered_trg_cloud->is_dense = false;
// Set input clouds.
correst->setInputSource(filtered_src_cloud);
correst->setInputTarget(filtered_trg_cloud);
// Find correspondences.
correst->determineReciprocalCorrespondences(*correspondences);
CLOG(LINFO) << "Found correspondences: " << correspondences->size();
out_corest.write(correst);
// Correspondence rejection.
if (prop_reject_correspondences) {
CLOG(LDEBUG) << "Correspondence rejection";
if (prop_use_RanSAC) {
CLOG(LDEBUG) << "Using RanSAC-based correspondence rejection";
// Use RANSAC to filter correspondences.
pcl::CorrespondencesPtr inliers(new pcl::Correspondences()) ;
pcl::registration::CorrespondenceRejectorSampleConsensus<PointXYZSIFT> crsac ;
crsac.setInputSource(filtered_src_cloud);
crsac.setInputTarget(filtered_trg_cloud);
crsac.setInlierThreshold(prop_RanSAC_inliers_threshold);
crsac.setMaximumIterations(prop_RanSAC_max_iterations);
crsac.setInputCorrespondences(correspondences);
// Reject correspondences.
crsac.getCorrespondences(*inliers);
CLOG(LINFO) << "Correspondences remaining after RANSAC-based rejection: " << inliers->size();
// Get computed transformation.
Types::HomogMatrix hm = crsac.getBestTransformation() ;
CLOG(LINFO) << "Found transformation:\n" << hm;
// Return correspondences - inliers.
return inliers;
} else {
// Use correspondence rejection distance.
CLOG(LDEBUG) << "Using correspondence rejection based on Euclidean distance";
// Use RANSAC to filter correspondences.
pcl::CorrespondencesPtr inliers(new pcl::Correspondences()) ;
pcl::registration:: CorrespondenceRejectorDistance cr ;
cr.setInputSource<PointXYZSIFT>(filtered_src_cloud);
cr.setInputTarget<PointXYZSIFT>(filtered_trg_cloud);
cr.setMaximumDistance(prop_max_distance);
cr.setInputCorrespondences(correspondences);
// Reject correspondences.
cr.getCorrespondences(*inliers);
CLOG(LINFO) << "Correspondences remaining after Euclidean based rejection: " << inliers->size();
// Return correspondences - inliers.
return inliers;
}//: else euclidean
} else {
CLOG(LDEBUG) << "Returning all correspondences";
// Return found correspondences.
return correspondences;
}//: if
}
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) {
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGBA>);
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> ("cloud_inliers.pcd", *cloud) == -1) {
PCL_ERROR ("Failed to read file cloud_inliers.pcd \n");
return (-1);
}
std::cout << "Loaded "
<< cloud->width * cloud->height
<< " data points from cloud_inliers.pcd."
<< std::endl;
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGBA> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i = 0;
int num_points = (int) cloud->points.size();
while (cloud->points.size() > 0.3 * num_points) {
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size() == 0) {
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
extract.setInputCloud (cloud);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
extract.setNegative (true);
extract.filter (*cloud_filtered);
*cloud = *cloud_filtered;
}
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;
ec.setClusterTolerance (0.02);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it) {
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZRGBA> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
}
int main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RGBcloud (new pcl::PointCloud<pcl::PointXYZRGB>);
reader.read (argv[1], *RGBcloud);
std::cout << "RGBPointCloud before filtering has: " << RGBcloud->points.size () << " data points." << std::endl; //*
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZRGB>);
reader.read(argv[1], *cloudF);
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
clouds.reserve(10);
labels.reserve(10);
//this is only for the old data sets
//for (size_t i=0; i < RGBcloud->points.size();++i)
//{
// if (!((RGBcloud->points[i].x > -.35 && RGBcloud->points[i].x < .35) && (RGBcloud->points[i].y > -.35 && RGBcloud->points[i].y < .35)))
// {
// RGBcloud->points[i].x = 0;
// RGBcloud->points[i].y = 0;
// RGBcloud->points[i].z = 0;
// }
//}
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZRGB> vg;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
vg.setInputCloud (RGBcloud);
vg.setLeafSize (0.001f, 0.001f, 0.001f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl;
//pcl::visualization::CloudViewer viewer("Cloud Viewer");
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.0085);//lesser the values, more points seep into the table
// Segment the largest planar component from the remaining cloud until 30% of the points remain
int i=0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size () > 0.30* nr_points)
{
seg.setInputCloud(cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
exit(-1);
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane); //*
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
//viewer.showCloud(cloud_plane, "cloud_name");
//std::cin.get();
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_filtered); //*
std::cerr <<" The Coefficients are: " << coefficients->values[0]<< " "<< coefficients->values[1]<< " "<< coefficients->values[2]<< " " << coefficients->values[3]<< " "<< std::endl;
}
// color segmentation
//
// define this vector so we can operate over all colored point clouds in a loop
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> coloredClouds(5);
// instantiate the colored clouds
for (int i = 0; i < 5; i++)
coloredClouds[i] = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
// alias the colored clouds with names
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Rcloud = coloredClouds[0];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Gcloud = coloredClouds[1];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Bcloud = coloredClouds[2];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ycloud = coloredClouds[3];
pcl::PointCloud<pcl::PointXYZRGB>::Ptr Ocloud = coloredClouds[4];
// color segment the cloud using min/max ranges
//colorSegment(cloud_filtered, Rcloud, Gcloud, Bcloud, Ycloud, Ocloud);
// color segment the cloud using a normal distrbuted color model
normalSegment(cloud_filtered, Rcloud, Gcloud, Bcloud, Ycloud, Ocloud);
// temp: display the colored clouds one after another
pcl::visualization::CloudViewer viewer("Cloud Viewer");
for (int i = 0; i < coloredClouds.size(); i++)
{
cout << "Showing cloud #" << i << endl;
viewer.showCloud(coloredClouds[i]);
cin.get();
}
// empty out the lists before re-filling them with colorful points
clouds.clear();
labels.clear();
// now find clusters inside each of the colored point clouds
int num_clusters = 0;
for (int i = 0; i < coloredClouds.size(); i++)
{
if (coloredClouds[i]->size() == 0)
continue;
cout << "Clustering colored cloud #" << i << " which has " << coloredClouds[i]->size() << " points" << endl;
num_clusters += clusterExtraction(coloredClouds[i], &clouds, &labels);
}
// display what we've found
std::cerr<<"Waiting 3 "<<std::endl;
viewer.runOnVisualizationThreadOnce(label);
char c_name[] = "Cloud No: ";
char cloud_name [20];
std::cerr << "Total Clusters: " << num_clusters << std::endl;
for (size_t k = 0; k < num_clusters; k++)
{
std::sprintf(cloud_name,"%s%d",c_name,k);
viewer.showCloud(clouds[k], cloud_name);
}
std::cin.get();
int xx, yy;
while(1)
{
cout << "Enter the label of the first cloud: ";
cin >> xx;
cout << "Enter the label of the second cloud: ";
cin >> yy;
cout << "Finding distance...";
cout << findDistance(clouds[xx], clouds[yy]) << endl;
viewer.runOnVisualizationThreadOnce(drawArrow);
cout << "Enter the number of the cloud you want to see alone: ";
cin >> xx;
}
while (!viewer.wasStopped ());
return 0;
}
TEX_NAMESPACE_BEGIN
/**
* Dampens the quality of all views in which the face's projection
* has a much different color than in the majority of views.
* Returns whether the outlier removal was successfull.
*
* @param infos contains information about one face seen from several views
* @param settings runtime configuration.
*/
bool
photometric_outlier_detection(std::vector<FaceProjectionInfo> * infos, Settings const & settings) {
if (infos->size() == 0) return true;
/* Configuration variables. */
double const gauss_rejection_threshold = 6e-3;
/* If all covariances drop below this we stop outlier detection. */
double const minimal_covariance = 5e-4;
int const outlier_detection_iterations = 10;
int const minimal_num_inliers = 4;
float outlier_removal_factor = std::numeric_limits<float>::signaling_NaN();
switch (settings.outlier_removal) {
case OUTLIER_REMOVAL_NONE: return true;
case OUTLIER_REMOVAL_GAUSS_CLAMPING:
outlier_removal_factor = 1.0f;
break;
case OUTLIER_REMOVAL_GAUSS_DAMPING:
outlier_removal_factor = 0.2f;
break;
}
Eigen::MatrixX3d inliers(infos->size(), 3);
std::vector<std::uint32_t> is_inlier(infos->size(), 1);
for (std::size_t row = 0; row < infos->size(); ++row) {
inliers.row(row) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
}
Eigen::RowVector3d var_mean;
Eigen::Matrix3d covariance;
Eigen::Matrix3d covariance_inv;
for (int i = 0; i < outlier_detection_iterations; ++i) {
if (inliers.rows() < minimal_num_inliers) {
return false;
}
/* Calculate the inliers' mean color and color covariance. */
var_mean = inliers.colwise().mean();
Eigen::MatrixX3d centered = inliers.rowwise() - var_mean;
covariance = (centered.adjoint() * centered) / double(inliers.rows() - 1);
/* If all covariances are very small we stop outlier detection
* and only keep the inliers (set quality of outliers to zero). */
if (covariance.array().abs().maxCoeff() < minimal_covariance) {
for (std::size_t row = 0; row < infos->size(); ++row) {
if (!is_inlier[row]) infos->at(row).quality = 0.0f;
}
return true;
}
/* Invert the covariance. FullPivLU is not the fastest way but
* it gives feedback about numerical stability during inversion. */
Eigen::FullPivLU<Eigen::Matrix3d> lu(covariance);
if (!lu.isInvertible()) {
return false;
}
covariance_inv = lu.inverse();
/* Compute new number of inliers (all views with a gauss value above a threshold). */
for (std::size_t row = 0; row < infos->size(); ++row) {
Eigen::RowVector3d color = mve_to_eigen(infos->at(row).mean_color).cast<double>();
double gauss_value = multi_gauss_unnormalized(color, var_mean, covariance_inv);
is_inlier[row] = (gauss_value >= gauss_rejection_threshold ? 1 : 0);
}
/* Resize Eigen matrix accordingly and fill with new inliers. */
inliers.resize(std::accumulate(is_inlier.begin(), is_inlier.end(), 0), Eigen::NoChange);
for (std::size_t row = 0, inlier_row = 0; row < infos->size(); ++row) {
if (is_inlier[row]) {
inliers.row(inlier_row++) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
}
}
}
covariance_inv *= outlier_removal_factor;
for (FaceProjectionInfo & info : *infos) {
Eigen::RowVector3d color = mve_to_eigen(info.mean_color).cast<double>();
double gauss_value = multi_gauss_unnormalized(color, var_mean, covariance_inv);
assert(0.0 <= gauss_value && gauss_value <= 1.0);
switch(settings.outlier_removal) {
case OUTLIER_REMOVAL_NONE: return true;
case OUTLIER_REMOVAL_GAUSS_DAMPING:
info.quality *= gauss_value;
break;
case OUTLIER_REMOVAL_GAUSS_CLAMPING:
if (gauss_value < gauss_rejection_threshold) info.quality = 0.0f;
break;
}
}
return true;
}
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;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
cloud->width = 15;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
// Generate the data
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1.0;
}
// Set a few outliers
cloud->points[0].z = 2.0;
cloud->points[3].z = -2.0;
cloud->points[6].z = 4.0;
std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> 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;
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i)
std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " "
<< cloud->points[inliers->indices[i]].y << " "
<< cloud->points[inliers->indices[i]].z << std::endl;
return (0);
}
void ClusterExtractor::computeClusters()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scenePCLPointCloudXYZ(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromPCLPointCloud2(*sceneCloud, *scenePCLPointCloudXYZ);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (scenePCLPointCloudXYZ);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (true)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
ROS_WARN("Could not estimate a planar model for the given dataset.");
//std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
ROS_INFO("%d",++i);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
if (cloud_plane->points.size() < .2*nr_points) {
break;
}
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
// Add euclidean plane to computer clusters
cloudClusters.push_back(cloud_plane);
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
cloudClusters.push_back(cloud_cluster);
}
}
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;
}
// Proccess the point clouds
void processPointCloud( const sensor_msgs::PointCloud2ConstPtr& pointcloud_msg )
{
ROS_INFO_NAMED("perception","\n\n\n");
ROS_INFO_STREAM_NAMED("perception","Processing new point cloud");
// ---------------------------------------------------------------------------------------------
// Start making result
result_.blocks.poses.clear(); // Clear last block perception result
result_.blocks.header.stamp = pointcloud_msg->header.stamp;
result_.blocks.header.frame_id = base_link;
// Basic point cloud conversions ---------------------------------------------------------------
// Convert from ROS to PCL
pcl::PointCloud<pcl::PointXYZRGB> cloud;
pcl::fromROSMsg(*pointcloud_msg, cloud);
// Make new point cloud that is in our working frame
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
// Transform to whatever frame we're working in, probably the arm's base frame, ie "base_link"
ROS_INFO_STREAM_NAMED("perception","Waiting for transform...");
ros::spinOnce();
tf_listener_.waitForTransform(base_link, cloud.header.frame_id, cloud.header.stamp, ros::Duration(2.0));
if(!pcl_ros::transformPointCloud(base_link, cloud, *cloud_transformed, tf_listener_))
{
if( process_count_ > 1 ) // the first time we can ignore it
ROS_ERROR_STREAM_NAMED("perception","Error converting to desired frame");
// Do this to speed up the next process attempt:
process_count_ = PROCESS_EVERY_NTH;
return;
}
// Limit to things we think are roughly at the table height ------------------------------------
// pcl::PointIndices::Ptr filtered_indices(new pcl::PointIndices); // hold things at table height
// std::vector<int>
boost::shared_ptr<std::vector<int> > filtered_indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud(cloud_transformed);
pass.setFilterFieldName("z");
pass.setFilterLimits(table_height - 0.05, table_height + block_size + 0.05);
//pass.setFilterLimits(table_height - 0.01, table_height + block_size + 0.02); // DTC
pass.filter(*filtered_indices);
/*
// Limit to things in front of the robot ---------------------------------------------------
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
//pass.setInputCloud(cloud_filteredZ);
pass.setIndices(filtered_indices);
pass.setFilterFieldName("x");
pass.setFilterLimits(.1,.5);
pass.filter(*cloud_filtered);
*/
/*
// Check if any points remain
if( cloud_filtered->points.size() == 0 )
{
ROS_ERROR_STREAM_NAMED("perception","0 points left");
return;
}
else
{
ROS_INFO_STREAM_NAMED("perception","Filtered, %d points left", (int) cloud_filtered->points.size());
}
*/
// Segment components --------------------------------------------------------------------------
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC); // robustness estimator - RANSAC is simple
seg.setMaxIterations(200); // the maximum number of iterations the sample consensus method will run
seg.setDistanceThreshold(0.005); // determines how close a point must be to the model in order to be considered an inlier
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr model_coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZRGB>());
/*
int nr_points = cloud_filtered->points.size();
// Segment cloud until there are less than 30% of points left? not sure why this is necessary
while(cloud_filtered->points.size() > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud (find the table)
seg.setInputCloud(cloud_filtered);
// seg.setIndices();
seg.segment(*inliers, *model_coefficients);
if(inliers->indices.size() == 0)
{
ROS_ERROR_STREAM_NAMED("perception","Could not estimate a planar model for the given dataset.");
return;
}
//std::cout << "Inliers: " << (inliers->indices.size()) << std::endl;
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Copy the extracted component (the table) to a seperate point cloud
extract.filter(*cloud_plane);
//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_filtered); // remove table from cloud_filtered
// Debug output - DTC
// Show the contents of the inlier set, together with the estimated plane parameters, in ax+by+cz+d=0 form (general equation of a plane)
ROS_INFO_STREAM_NAMED("perception", "Model coefficients: " << model_coefficients->values[0] << " "
<< model_coefficients->values[1] << " "
<< model_coefficients->values[2] << " "
<< model_coefficients->values[3] ); // TODO: turn this into an rviz marker somehow?
*/
// Show groups of recognized objects (the inliers)
/*std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i) {
std::cerr << inliers->indices[i] << " " << cloud.points[inliers->indices[i]].x << " "
<< cloud.points[inliers->indices[i]].y << " "
<< cloud.points[inliers->indices[i]].z << std::endl;
}*/
// }
// DTC: Removed to make compatible with PCL 1.5
// Creating the KdTree object for the search method of the extraction
// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTreeFLANN<pcl::PointXYZRGB>);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
// tree->setInputCloud(cloud_filtered);
tree->setInputCloud(cloud_transformed, filtered_indices);
// Find the clusters (objects) on the table
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> cluster_extract;
//cluster_extract.setClusterTolerance(0.005); // 5mm - If you take a very small value, it can happen that an actual object can be seen as multiple clusters. On the other hand, if you set the value too high, it could happen, that multiple objects are seen as one cluster. So our recommendation is to just test and try out which value suits your dataset.
cluster_extract.setClusterTolerance(0.02); // 2cm
cluster_extract.setMinClusterSize(100);
cluster_extract.setMaxClusterSize(25000);
// cluster_extract.setSearchMethod(tree);
// cluster_extract.setInputCloud(cloud_filtered);
cluster_extract.setInputCloud(cloud_transformed);
cluster_extract.setIndices(filtered_indices);
ROS_INFO_STREAM_NAMED("perception","Extracting...");
cluster_extract.extract(cluster_indices);
ROS_INFO_STREAM_NAMED("perception","after cluster extract");
// Publish point cloud data
// filtered_pub_.publish(cloud_filtered);
// plane_pub_.publish(cloud_plane);
ROS_WARN_STREAM_NAMED("perception","Number indicies/clusters: " << cluster_indices.size() );
// processClusters( cluster_indices, pointcloud_msg, cloud_filtered );
processClusters( cluster_indices, cloud_transformed, cloud_filtered, cloud );
// ---------------------------------------------------------------------------------------------
// Final results
if(result_.blocks.poses.size() > 0)
{
// Change action state, if we the action is currently active
if(action_server_.isActive())
{
action_server_.setSucceeded(result_);
}
// Publish block poses
block_pose_pub_.publish(result_.blocks);
// Publish rviz markers of the blocks
publishBlockLocation();
ROS_INFO_STREAM_NAMED("perception","Finished ---------------------------------------------- ");
}
else
{
ROS_INFO_STREAM_NAMED("perception","Couldn't find any blocks this iteration!");
}
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){
sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl;
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (10);
ec.setMaxClusterSize (2500);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit;
int j = 0;
for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for(pit = it->indices.begin(); pit != it->indices.end(); pit++) {
//push_back: add a point to the end of the existing vector
cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
/*cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;*/
}
//Merge current clusters to whole point cloud
*clustered_cloud += *cloud_cluster;
}
pcl::toROSMsg (*clustered_cloud , *clusters);
clusters->header.frame_id = "/camera_depth_frame";
clusters->header.stamp=ros::Time::now();
pub.publish (*clusters);
//sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2);
//pcl::toROSMsg(*cloud_filtered, *output);
//pub_plane.publish(*output);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl_conversions::toPCL(*input, *cloud_blob);
// Fill in the cloud data
//pcl::PCDReader reader;
//reader.read ("table_scene_lms400.pcd", *cloud_blob);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
//std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
//pcl::PCDWriter writer;
//writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
//while (cloud_filtered->points.size () > 0.3 * nr_points)
for (int i = 0; i < 2; i++)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
ROS_ERROR("Could not estimate a planar model for the given dataset.");
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// std::stringstream ss;
// ss << "table_scene_lms400_plane_" << i << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_f);
cloud_filtered.swap (cloud_f);
//i++;
}
//pcl::PCLPointCloud2 pre_output;
sensor_msgs::PointCloud2 output;
pcl::toROSMsg(*cloud_p, output);
//pcl_conversions::fromPCL(pre_output, output);
pub.publish(output);
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
// pcl::PCLPointCloud2 cloud_filtered;
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*input, *cloud_blob);
// Perform the actual filtering
pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.05, 0.05, 0.05);
sor.setFilterFieldName("z");
sor.setFilterLimits(0.01, 0.3);
sor.filter (*cloud_filtered_blob);
// // Remove outlier X
// pcl::PCLPointCloud2::Ptr cloud_filtered_blobx (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sorx;
// sorx.setInputCloud(cloud_filtered_blobz);
// sorx.setFilterFieldName("x");
// sorx.setFilterLimits(-1, 1);
// sorx.filter(*cloud_filtered_blobx);
// // Remove outlier Y
// pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
// pcl::VoxelGrid<pcl::PCLPointCloud2> sory;
// sory.setInputCloud(cloud_filtered_blobx);
// sory.setFilterFieldName("y");
// sory.setFilterLimits(-1, 1);
// sory.filter(*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromPCLPointCloud2 (*cloud_filtered_blob, *cloud_filtered);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
Eigen::Vector3f upVector(0, 0, 1);
seg.setAxis(upVector);
seg.setEpsAngle(1.5708);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
return;
}
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
// Publish inliers
// sensor_msgs::PointCloud2 inlierpc;
// pcl_conversions::fromPCL(cloud_p, inlierpc);
pub.publish (*cloud_p);
// Publish the model coefficients
pcl_msgs::ModelCoefficients ros_coefficients;
pcl_conversions::fromPCL(*coefficients, ros_coefficients);
pubCoef.publish (ros_coefficients);
// ==========================================
// // Convert to ROS data type
// sensor_msgs::PointCloud2 downpc;
// pcl_conversions::fromPCL(cloud_filtered, downpc);
// // Publish the data
// pub.publish (downpc);
// pcl::ModelCoefficients coefficients;
// pcl::PointIndices inliers;
// //.makeShared()
// // Create the segmentation object
// pcl::SACSegmentation<pcl::PointXYZ> seg;
// seg.setInputCloud (&cloudPtr);
// seg.setOptimizeCoefficients (true); // Optional
// seg.setModelType (pcl::SACMODEL_PLANE); // Mandatory
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setDistanceThreshold (0.1);
// seg.segment (inliers, coefficients);
// if (inliers.indices.size () == 0)
// {
// PCL_ERROR ("Could not estimate a planar model for the given dataset.");
// return;
// }
// std::cerr << "Model coefficients: " << coefficients.values[0] << " "
// << coefficients.values[1] << " "
// << coefficients.values[2] << " "
// << coefficients.values[3] << std::endl;
// // Publish the model coefficients
// pcl_msgs::ModelCoefficients ros_coefficients;
// pcl_conversions::fromPCL(coefficients, ros_coefficients);
// pubCoef.publish (ros_coefficients);
}
void environement_identification(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered_inliers (new pcl::PointCloud<pcl::PointXYZRGB> ());
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
//while (cloud_filtered->points.size () > 0.3 * nr_points)
//{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
//break;
}else{
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
//std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// }
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_filtered);
sor.setMeanK (50);
sor.setStddevMulThresh (1.0);
sor.filter (*cloud_filtered_inliers);
std::cout << "cloud filtred after plane extraction : "<<cloud_filtered_inliers->size()<< std::endl;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud_filtered_inliers);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.09); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered_inliers);
ec.extract (cluster_indices);
std::cout << "cluster number : "<<cluster_indices.size()<< std::endl;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
j=j%(sizeof(ColorB)/sizeof(*ColorB));
cloud_filtered_inliers->points[*pit].rgb = *reinterpret_cast<float*>(new uint32_t(static_cast<uint32_t>(ColorR[j]) << 16 |
static_cast<uint32_t>(ColorG[j]) << 8 |
static_cast<uint32_t>(ColorB[j]) ));
cloud_cluster->points.push_back (cloud_filtered_inliers->points[*pit]);
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud_cluster);
(viewer)->addPointCloud<pcl::PointXYZRGB> (cloud_cluster, rgb, "cloud identification"+j);
j++;
}
}
void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// sensor_msgs::PointCloud2 cloud_filtered;
pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_XYZ (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_p (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2::Ptr downsampled (new sensor_msgs::PointCloud2);
// sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2);
// std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
// << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (input);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*downsampled);
// std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Change from type sensor_msgs::PointCloud2 to pcl::PointXYZ
pcl::fromROSMsg (*downsampled, *downsampled_XYZ);
//Create the SACSegmentation object and set the model and method type
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);//For more info: wikipedia.org/wiki/RANSAC
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);//How close a point must be to the model to considered an inlier
int i = 0, nr_points = (int) downsampled_XYZ->points.size ();
//Contains the plane point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
// While 30% of the original cloud is still there
while (downsampled_XYZ->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (downsampled_XYZ);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (downsampled_XYZ);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// std::cerr << "PointCloud representing the planar component: "
// << output_p->width * output_p->height << " data points." << std::endl;
// std::stringstream ss;
// ss << "table_scene_lms400_plane_" << i << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
downsampled_XYZ.swap(cloud_f);
i++;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (downsampled_XYZ);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (downsampled_XYZ);
ec.extract (cluster_indices);
ros::NodeHandle nh;
//Create a publisher for each cluster
for (int i = 0; i < cluster_indices.size(); ++i)
{
std::string topicName = "/pcl_tut/cluster" + boost::lexical_cast<std::string>(i);
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2> (topicName, 1);
pub_vec.push_back(pub);
}
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (downsampled_XYZ->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
// std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
//Convert the pointcloud to be used in ROS
sensor_msgs::PointCloud2::Ptr output (new sensor_msgs::PointCloud2);
pcl::toROSMsg (*cloud_cluster, *output);
output->header.frame_id = input->header.frame_id;
// Publish the data
pub_vec[j].publish (output);
++j;
}
}
int main (int argc, char** argv)
{
sensor_msgs::PointCloud2::Ptr cloud_blob (new sensor_msgs::PointCloud2), cloud_filtered_blob (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
pcl::PCDReader reader;
reader.read ("table_scene_lms400.pcd", *cloud_blob);
std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
// Convert to the templated PointCloud
pcl::fromROSMsg (*cloud_filtered_blob, *cloud_filtered);
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
// Write the downsampled version to disk
pcl::PCDWriter writer;
writer.write<pcl::PointXYZ> ("table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.01);
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZ> extract;
int i = 0, nr_points = (int) cloud_filtered->points.size ();
// While 30% of the original cloud is still there
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
std::stringstream ss;
ss << "table_scene_lms400_plane_" << i << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_p, false);
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
i++;
}
return (0);
}
void FilterPlanes::filterPlanes(vector<BasicPlane> &plane_stack){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>), cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>);
if (verbose_text>0){
cout << "PointCloud before filtering: " << cloud->width * cloud->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
vector<int64_t> tic_toc;
tic_toc.push_back(bot_timestamp_now());
#endif
/* Ptcoll_cfg ptcoll_cfg;
ptcoll_cfg.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg.collection = pose_coll_id;
ptcoll_cfg.element_id = pose_element_id; */
//1. Downsample the dataset using a leaf size of 1cm
// this is 99% of the cpu time
pcl::VoxelGrid<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud);
// for table dataset: sor.setLeafSize (0.01, 0.01, 0.01);
sor.setLeafSize (0.05, 0.05, 0.05);
// sor.setLeafSize (0.1, 0.1, 0.1);
sor.filter (*cloud_filtered);
if (verbose_text>0){
cout << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
if (verbose_lcm>2){
/* ptcoll_cfg.id = 200;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_downsampled";
ptcoll_cfg.npoints = cloud_filtered->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);*/
}
// 2. Set up the Ransac Plane Fitting Object:
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100); // was 4000
seg.setDistanceThreshold (distance_threshold_); // 0.01 for table data set
// Create the filtering object
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
int n_major = 0, nr_points = cloud_filtered->points.size ();
//vector<BasicPlaneX> plane_stack;
BasicPlane one_plane;
// Extract the primary planes:
// major planes are the coarse planes extracted via ransac. they might contain disconnected points
// these are broken up into minor planes which are portions of major planes
if(verbose_lcm > 2){
for (int i=0;i<7;i++){
char n_major_char[10];
sprintf(n_major_char,"%d",i);
/* ptcoll_cfg.id =210+ i+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name =(char*) "cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = 0;
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_filtered);
*/
}
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
// TODO: this will rest this object. need to add publish of reset
// at start of this block and set rese ttofal
for (int i=0;i<7;i++){
/*
Ptcoll_cfg ptcoll_cfg2;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =pose_element_id; //bot_timestamp_now();
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
if (i==0){
ptcoll_cfg2.reset=true;
}else{
ptcoll_cfg2.reset=false;
}
ptcoll_cfg2.id =501;
ptcoll_cfg2.name.assign("Grown Stack Final Hull");
ptcoll_cfg2.npoints = 0;
ptcoll_cfg2.type =3;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, *cloud_filtered);
*/
}
}
#if DO_TIMING_PROFILE
tic_toc.push_back(bot_timestamp_now());
#endif
while (cloud_filtered->points.size () > stop_proportion_ * nr_points) {
// While XX% of the original cloud is still there
char n_major_char[10];
sprintf(n_major_char,"%d",n_major);
if (n_major >6) {
if (verbose_text >0){
std::cout << n_major << " is the max number of planes to find, quitting" << std::endl;
}
break;
}
//a Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
if (inliers->indices.size () < stop_cloud_size_) // stop when the plane is only a few points
{
//std::cerr << "No remaining planes in this set" << std::endl;
break;
}
//b Extract the inliers
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_p);
if (verbose_text>0){
std::cout << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
}
// Create the filtering object
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
sor.setInputCloud (cloud_p);
sor.setMeanK (30);
sor.setStddevMulThresh (0.5); //was 1.0
sor.filter (*cloud_p);
if(verbose_lcm > 2){
/*
ptcoll_cfg.id =210+ n_major+3;
ptcoll_cfg.reset=true;
ptcoll_cfg.name ="cloud_p ";
ptcoll_cfg.name.append(n_major_char);
ptcoll_cfg.npoints = cloud_p->points.size();
float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0};
ptcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
ptcoll_cfg.type=1;
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg, *cloud_p);
*/
}
vector<BasicPlane> grown_stack;
vector<BasicPlane> * grown_stack_ptr;
grown_stack_ptr = &grown_stack;
GrowCloud grow;
grow.setInputCloud(cloud_p);
grow.setMinCloudSize(stop_cloud_size_); // useing stop cloud size here too
grow.setLCM(publish_lcm);
grow.doGrowCloud(*grown_stack_ptr);
if (verbose_text>0){
cout << "grow_cloud new found " << grown_stack.size() << " seperate clouds\n";
}
// Spit raw clouds out to LCM:
if(verbose_lcm > 2){
for (int i=0;i<grown_stack.size();i++){
/*
Ptcoll_cfg ptcoll_cfg2;
ptcoll_cfg2.reset=false;
// the i below accounts for super planes in the same utime
ptcoll_cfg2.point_lists_id =10*n_major + i; //filterplanes->pose_element_id;
ptcoll_cfg2.collection = pose_coll_id;
ptcoll_cfg2.element_id = pose_element_id;
ptcoll_cfg2.id =500;
ptcoll_cfg2.name.assign("Grown Stack Final");
ptcoll_cfg2.npoints = grown_stack[i].cloud.points.size ();
ptcoll_cfg2.type =1;
int id = plane_stack.size() + i; // 10*n_major + i;
float colorm_temp0[] ={colors[3*(id%num_colors)],colors[3*(id%num_colors)+1],colors[3*(id%num_colors)+2] ,0.0};
ptcoll_cfg2.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float));
pcdXYZRGB_to_lcm(publish_lcm,ptcoll_cfg2, grown_stack[i].cloud);
*/
}
}
BasicPlane one_plane;
int n_minor=0;
BOOST_FOREACH( BasicPlane one_plane, grown_stack ){
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZRGB>);
// c Project the model inliers (seems to be necessary to fitting convex hull
pcl::ProjectInliers<pcl::PointXYZRGB> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp (new pcl::PointCloud<pcl::PointXYZRGB> ());
*temp = (one_plane.cloud);
proj.setInputCloud (temp);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
std::vector <pcl::Vertices> vertices;
if (1==1){ // convex:
pcl::ConvexHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setDimension(2);
chull.reconstruct (*cloud_hull,vertices);
}else { // concave
pcl::ConcaveHull<pcl::PointXYZRGB> chull;
chull.setInputCloud (cloud_projected);
chull.setKeepInformation(true);
chull.setAlpha(0.5);
// for arch way:
// 1.1 too few
// 0.7 a little to few but much better
chull.reconstruct (*cloud_hull,vertices);
}
//std::cout << "Hull has: " << cloud_hull->points.size () << " vertices." << std::endl;
if (cloud_hull->points.size () ==0){
cout <<"ERROR: CONVEX HULL HAS NO POINTS! - NEED TO RESOLVE THIS\n";
}
// d.2 Find the mean colour of the hull:
int rgba[]={0,0,0,0};
for(int i=0;i<temp->points.size();i++){
int rgba_one = *reinterpret_cast<int*>(&temp->points[i].rgba);
rgba[3] += ((rgba_one >> 24) & 0xff);
rgba[2] += ((rgba_one >> 16) & 0xff);
rgba[1] += ((rgba_one >> 8) & 0xff);
rgba[0] += (rgba_one & 0xff);
}
double scale = ((double) temp->points.size());
rgba[3] =(int) round(((double) rgba[3]) / scale);
rgba[2] =(int) round(((double) rgba[2]) / scale);
rgba[1] =(int) round(((double) rgba[1]) / scale);
rgba[0] =(int) round(((double) rgba[0]) / scale);
// Write the plane to file for experimentation:
//stringstream oss;
//oss << ptcoll_cfg.element_id <<"_" << ptcoll_cfg.name << ".pcd";
//writer.write (oss.str(), *this_hull, false);
for(int i=0;i<cloud_hull->points.size();i++){
unsigned char* rgba_ptr = (unsigned char*)&cloud_hull->points[i].rgba;
(*rgba_ptr) = rgba[0] ;
(*(rgba_ptr+1)) = rgba[1];
(*(rgba_ptr+2)) = rgba[2];
(*(rgba_ptr+3)) = rgba[3];
}
(one_plane.coeffs) = *coefficients;
one_plane.cloud = *cloud_hull;
one_plane.utime = pose_element_id;
one_plane.major = n_major;
one_plane.minor = n_minor;
one_plane.n_source_points = cloud_projected->points.size();
plane_stack.push_back(one_plane);
n_minor++;
// int stop;
// cout << "int to cont: ";
// cin >> stop;
}
// Create the filtering object
extract.setNegative (true);
extract.filter (*cloud_filtered);
n_major++;
}
void ImageProcessing::segEuclideanClusters(const char *filename) {
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
reader.read(filename, *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
int i = 0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points) {
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02); // 2cm
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr all_clusters(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB aPoint;
int j = 0;
Color myColor;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
myColor = Color::random(); //one color for each cluster.
//adding all points of one cluster
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) {
cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //*
aPoint.x = cloud_cluster->points.back().x;
aPoint.y = cloud_cluster->points.back().y;
aPoint.z = cloud_cluster->points.back().z;
aPoint.r = myColor.red;
aPoint.g = myColor.green;
aPoint.b = myColor.blue;
all_clusters->points.push_back(aPoint);
}
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
j++;
}
//writer.write<pcl::PointXYZRGB> ("clustered_cloud.pcd", *cloud_cluster, false); //*
pcl::visualization::CloudViewer viewer("Cluster viewer");
viewer.showCloud(all_clusters);
while (!viewer.wasStopped()) {
}
}
bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req,
jsk_pcl_ros::CallSnapIt::Response& res) {
pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
Eigen::Vector3f n, C_orig;
if (!extractPointsInsideCylinder(req.request.center,
req.request.direction,
req.request.radius,
req.request.height,
inliers, n, C_orig,
1.3)) {
return false;
}
if (inliers->indices.size() < 3) {
NODELET_ERROR("not enough points inside of the target_plane");
return false;
}
geometry_msgs::PointStamped centroid;
centroid.point.x = C_orig[0];
centroid.point.y = C_orig[1];
centroid.point.z = C_orig[2];
centroid.header = req.request.header;
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(input_);
extract.setIndices(inliers);
extract.filter(*candidate_points);
publishPointCloud(debug_candidate_points_pub_, candidate_points);
// first, to remove plane we estimate the plane
pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
extractPlanePoints(candidate_points, plane_inliers, plane_coefficients,
n, req.request.eps_angle);
if (plane_inliers->indices.size() == 0) {
NODELET_ERROR ("plane estimation failed");
return false;
}
// remove the points blonging to the plane
pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (candidate_points);
extract.setIndices (plane_inliers);
extract.setNegative (true);
extract.filter (*points_inside_pole_wo_plane);
publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane);
// normal estimation
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
ne.setInputCloud (points_inside_pole_wo_plane);
ne.setKSearch (50);
ne.compute (*cloud_normals);
// segmentation
pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_CYLINDER);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setRadiusLimits (0.01, req.request.radius * 1.2);
seg.setDistanceThreshold (0.05);
seg.setInputCloud(points_inside_pole_wo_plane);
seg.setInputNormals (cloud_normals);
seg.setMaxIterations (10000);
seg.setNormalDistanceWeight (0.1);
seg.setAxis(n);
if (req.request.eps_angle != 0.0) {
seg.setEpsAngle(req.request.eps_angle);
}
else {
seg.setEpsAngle(0.35);
}
seg.segment (*cylinder_inliers, *cylinder_coefficients);
if (cylinder_inliers->indices.size () == 0)
{
NODELET_ERROR ("Could not estimate a cylinder model for the given dataset.");
return false;
}
debug_centroid_pub_.publish(centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud (points_inside_pole_wo_plane);
extract.setIndices (cylinder_inliers);
extract.setNegative (false);
extract.filter (*cylinder_points);
publishPointCloud(debug_candidate_points_pub3_, cylinder_points);
Eigen::Vector3f n_prime;
Eigen::Vector3f C_new;
for (size_t i = 0; i < 3; i++) {
C_new[i] = cylinder_coefficients->values[i];
n_prime[i] = cylinder_coefficients->values[i + 3];
}
double radius = fabs(cylinder_coefficients->values[6]);
// inorder to compute centroid, we project all the points to the center line.
// and after that, get the minimum and maximum points in the coordinate system of the center line
double min_alpha = DBL_MAX;
double max_alpha = -DBL_MAX;
for (size_t i = 0; i < cylinder_points->points.size(); i++ ) {
pcl::PointXYZ q = cylinder_points->points[i];
double alpha = (q.getVector3fMap() - C_new).dot(n_prime);
if (alpha < min_alpha) {
min_alpha = alpha;
}
if (alpha > max_alpha) {
max_alpha = alpha;
}
}
// the center of cylinder
Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime;
Eigen::Vector3f n_cross = n.cross(n_prime);
if (n.dot(n_prime)) {
n_cross = - n_cross;
}
double theta = asin(n_cross.norm());
Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
Eigen::Vector3f C = trans * C_orig;
Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse();
tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);
geometry_msgs::PointStamped centroid_transformed;
centroid_transformed.point.x = C_new_prime[0];
centroid_transformed.point.y = C_new_prime[1];
centroid_transformed.point.z = C_new_prime[2];
centroid_transformed.header = req.request.header;
debug_centroid_after_trans_pub_.publish(centroid_transformed);
// publish marker
visualization_msgs::Marker marker;
marker.type = visualization_msgs::Marker::CYLINDER;
marker.scale.x = radius;
marker.scale.y = radius;
marker.scale.z = (max_alpha - min_alpha);
marker.pose.position.x = C_new_prime[0];
marker.pose.position.y = C_new_prime[1];
marker.pose.position.z = C_new_prime[2];
// n_prime -> z
// n_cross.normalized() -> x
Eigen::Vector3f z_axis = n_prime.normalized();
Eigen::Vector3f y_axis = n_cross.normalized();
Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized();
Eigen::Matrix3f M;
for (size_t i = 0; i < 3; i++) {
M(i, 0) = x_axis[i];
M(i, 1) = y_axis[i];
M(i, 2) = z_axis[i];
}
Eigen::Quaternionf q (M);
marker.pose.orientation.x = q.x();
marker.pose.orientation.y = q.y();
marker.pose.orientation.z = q.z();
marker.pose.orientation.w = q.w();
marker.color.g = 1.0;
marker.color.a = 1.0;
marker.header = input_header_;
marker_pub_.publish(marker);
return true;
}
void PointCloudProcessor::_doEuclideanClusterExtraction(const QList<pcl::PointCloud<PointType>::Ptr> & clouds)
{
QList<pcl::PointCloud<PointType>::Ptr> clusteredClouds;
foreach (pcl::PointCloud<PointType>::Ptr cloud, clouds)
{
// Create the filtering object: downsample the dataset using a leaf size of 10cm
pcl::PointCloud<PointType>::Ptr cloudFiltered (new pcl::PointCloud<PointType> ());
pcl::VoxelGrid<PointType> vg;
vg.setInputCloud (cloud);
vg.setLeafSize (0.1f, 0.1f, 0.1f);
vg.filter (*cloudFiltered);
int nr_points = static_cast<int>(cloudFiltered->points.size());
addStatus(QStringLiteral("PointCloud after filtering has: ") + QString::number(nr_points) + QStringLiteral(" data points."));
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointType> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<PointType>::Ptr cloud_plane (new pcl::PointCloud<PointType> ());
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
pcl::PointCloud<PointType>::Ptr cloud_f (new pcl::PointCloud<PointType>);
while (cloudFiltered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloudFiltered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
addStatus(QStringLiteral("Could not estimate a planar model for the given dataset."));
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointType> extract;
extract.setInputCloud (cloudFiltered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
addStatus(QStringLiteral("PointCloud representing the planar component: ") + QString::number(cloud_plane->points.size()) + QStringLiteral(" data points."));
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloudFiltered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointType>::Ptr tree (new pcl::search::KdTree<PointType>);
tree->setInputCloud (cloudFiltered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointType> ec;
ec.setClusterTolerance (0.2); // 20cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (250000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloudFiltered);
ec.extract (cluster_indices);
for (auto it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<PointType>::Ptr cloud_cluster (new pcl::PointCloud<PointType>);
for (auto pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cloud_cluster->points.push_back (cloudFiltered->points[*pit]); //*
}
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusteredClouds.push_back(cloud_cluster);
}
}
void UAV_Segmentation::cloudCallback (const sensor_msgs::PointCloud2::ConstPtr& cloud_in)
{
ROS_INFO("Processing!");
/************************ CENTER AND LEFT BOXES ***************************************/
//Creating point cloud and convert ROS Message
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_in, *pcl_cloud);
//Create and define filter parameters
pcl::PassThrough<pcl::PointXYZ> pass3;
pass3.setFilterFieldName("x");
pass3.setFilterLimits(-2, 2); //-0.5 0.5
pass3.setInputCloud(pcl_cloud);
pass3.filter(*pcl_cloud);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setFilterFieldName("y");
pass.setFilterLimits(-2, 2); //-0.5 0.5
pass.setInputCloud(pcl_cloud);
pass.filter(*pcl_cloud);
pcl::PassThrough<pcl::PointXYZ> pass2;
pass2.setFilterFieldName("z");
pass2.setFilterLimits(0, 3.0); //-0.5 0.5
pass2.setInputCloud(pcl_cloud);
pass2.filter(*pcl_cloud);
//Model fitting process ->RANSAC
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PARALLEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.03); //0.03
seg.setAxis (Eigen::Vector3f(1, 0, 0));
seg.setEpsAngle (0.1); //0.02
seg.setInputCloud (pcl_cloud);
seg.segment (*inliers, *coefficients);
//Verify if inliers is not empty
if (inliers->indices.size () == 0)
return;
pcl::PointCloud<pcl::PointXYZ>::Ptr treated_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (std::vector<int>::const_iterator it = inliers->indices.begin(); it != inliers->indices.end (); ++it)
treated_cloud->push_back(pcl_cloud->points[*it]);
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(pcl_cloud);
extract.setIndices(inliers);
extract.setNegative(true);
extract.filter(*seg_cloud);
//Apply clustering algorithm
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ>);
kdtree->setInputCloud (seg_cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.5);
ec.setMinClusterSize (2);
ec.setMaxClusterSize (150);
ec.setSearchMethod (kdtree);
ec.setInputCloud (seg_cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
pcl::PointXYZI cluster_point;
double cluster_final_average;
int cluster_id=0;
float x1 = 0.0, y1 = 0.0, z1 = 0.0;
float x2 = 0.0, y2 = 0.0, z2 = 0.0;
float x3 = 0.0, y3 = 0.0, z3 = 0.0;
int total1 = 0, total2 = 0, total3 = 0;
bool hasUAV1, hasCup2, hasCup3;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end ();
++it, cluster_id+=1)
{
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
{
cluster_point.x = seg_cloud->points[*pit].x;
cluster_point.y = seg_cloud->points[*pit].y;
cluster_point.z = seg_cloud->points[*pit].z;
cluster_point.intensity = cluster_id;
cluster_cloud->push_back(cluster_point);
if(cluster_id == 0){
x1 += seg_cloud->points[*pit].x;
y1 += seg_cloud->points[*pit].y;
z1 += seg_cloud->points[*pit].z;
total1++;
}
}
}
if(total1 != 0 ){
x1 = x1/total1;
y1 = y1/total1;
z1 = z1/total1;
hasUAV1=true;
}else{
hasUAV1 = false;
}
//Publish message
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(*cluster_cloud, cloud);
cloud.header.stamp = ros::Time::now();
cloud.header.frame_id = cloud_in->header.frame_id;
treated_cloud_pub_.publish(cloud);
//Geometry
geometry_msgs::PointStamped UAVPoint1;
tf::Quaternion q;
geometry_msgs::PointStamped UAVSensorPoint1;
static tf::TransformBroadcaster tfBc1;
tf::Transform tfCup1;
UAVPoint1.header.frame_id = "base_footprint";
UAVPoint1.header.stamp = cloud_in->header.stamp;
UAVPoint1.point.x = x1;
UAVPoint1.point.y = y1;
UAVPoint1.point.z = z1;
ROS_INFO("Cup 1: X=%f Y=%f Z=%f", UAVPoint1.point.x, UAVPoint1.point.y, UAVPoint1.point.z);
trabfinal::gpsXY uavPos;
uavPos.x=UAVPoint1.point.x;
uavPos.y=UAVPoint1.point.y;
uavPos.z=UAVPoint1.point.z;
uav_control.publish(uavPos);
tf_listener.transformPoint("base_footprint", UAVPoint1, UAVSensorPoint1);
tfCup1.setOrigin( tf::Vector3(UAVSensorPoint1.point.x, UAVSensorPoint1.point.y, UAVSensorPoint1.point.z) );
ROS_INFO("Sensor Frame UAV 1: X=%f Y=%f Z=%f", UAVSensorPoint1.point.x, UAVSensorPoint1.point.y, UAVSensorPoint1.point.z);
q.setRPY(0, 0, 0);
tfCup1.setRotation(q);
tfBc1.sendTransform(tf::StampedTransform(tfCup1, cloud_in->header.stamp, "base_footprint", "uav1"));
ROS_INFO("All Sent!");
}
void TorusFinder::segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
if (!done_initialization_) {
return;
}
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloud_msg, *cloud);
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
if (voxel_grid_sampling_) {
pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
sor.filter (*downsampled_cloud);
cloud = downsampled_cloud;
}
pcl::SACSegmentation<pcl::PointNormal> seg;
if (use_normal_) {
pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
seg_normal.setInputNormals(cloud);
seg = seg_normal;
}
seg.setOptimizeCoefficients (true);
seg.setInputCloud(cloud);
seg.setRadiusLimits(min_radius_, max_radius_);
if (algorithm_ == "RANSAC") {
seg.setMethodType(pcl::SAC_RANSAC);
}
else if (algorithm_ == "LMEDS") {
seg.setMethodType(pcl::SAC_LMEDS);
}
else if (algorithm_ == "MSAC") {
seg.setMethodType(pcl::SAC_MSAC);
}
else if (algorithm_ == "RRANSAC") {
seg.setMethodType(pcl::SAC_RRANSAC);
}
else if (algorithm_ == "RMSAC") {
seg.setMethodType(pcl::SAC_RMSAC);
}
else if (algorithm_ == "MLESAC") {
seg.setMethodType(pcl::SAC_MLESAC);
}
else if (algorithm_ == "PROSAC") {
seg.setMethodType(pcl::SAC_PROSAC);
}
seg.setDistanceThreshold (outlier_threshold_);
seg.setMaxIterations (max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
if (use_hint_) {
seg.setAxis(hint_axis_);
seg.setEpsAngle(eps_hint_angle_);
}
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.segment(*inliers, *coefficients);
JSK_NODELET_INFO("input points: %lu", cloud->points.size());
if (inliers->indices.size() > min_size_) {
// check direction. Torus should direct to origin of pointcloud
// always.
Eigen::Vector3f dir(coefficients->values[4],
coefficients->values[5],
coefficients->values[6]);
if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
dir = - dir;
}
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
coefficients->values[1],
coefficients->values[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
PCLIndicesMsg ros_inliers;
ros_inliers.indices = inliers->indices;
ros_inliers.header = cloud_msg->header;
pub_inliers_.publish(ros_inliers);
PCLModelCoefficientMsg ros_coefficients;
ros_coefficients.values = coefficients->values;
ros_coefficients.header = cloud_msg->header;
pub_coefficients_.publish(ros_coefficients);
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
tf::poseEigenToMsg(pose, torus_msg.pose);
torus_msg.small_radius = 0.01;
torus_msg.large_radius = coefficients->values[3];
pub_torus_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_.publish(torus_array_msg);
// publish pose stamped
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = torus_msg.header;
pose_stamped.pose = torus_msg.pose;
pub_pose_stamped_.publish(pose_stamped);
pub_torus_array_with_failure_.publish(torus_array_msg);
pub_torus_with_failure_.publish(torus_msg);
}
else {
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
torus_msg.failure = true;
pub_torus_with_failure_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_with_failure_.publish(torus_array_msg);
JSK_NODELET_INFO("failed to find torus");
}
}
void ClusterExtraction::extract() {
/*
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read();
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (clusterTolerance); // 2cm
ec.setMinClusterSize (minClusterSize);
ec.setMaxClusterSize (maxClusterSize);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters;
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
clusters.push_back(cloud_cluster);
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
// if(j==0)
// {
// ss << "cloud_cluster_0" << ".pcd";
// writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
// j++;
// }
// if(j==0)
//{
std::cin.ignore();
out_the_biggest_cluster.write(cloud_cluster);
j++;
std::cout<<"Zapis!!!\n";
// }
}
out_indices.write(cluster_indices);
out_clusters.write(clusters);
//std::cout<<"j=="<<j<<endl;
//std::cout<<clusters.size()<<endl;
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
// Read in the cloud data
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_pcl.read();
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
//pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
/*
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
*/
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
//std::stringstream ss;
//ss << "cloud_cluster_" << j << ".pcd";
//writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
//std::cin.ignore();
// check if cluster contain interesting us plane, if so, return cluster, next remove plane and we have object :)
//if (include_plane){
out_the_biggest_cluster.write(cloud_cluster);
break;
//}
j++;
}
}
void FloorAxisAlignment::alignCloudPrincipleAxis (const PointCloudConstPtr input_cloud_ptr,
const PointCloudPtr output_cloud_ptr,
Eigen::Matrix4f& transform_output)
{
// Visualization visualizer;
Eigen::Matrix4f inital_guess;
inital_guess.block<3,3>(0,0) = Eigen::AngleAxisf(angle_, axis_).toRotationMatrix();
// inital_guess = Eigen::Matrix4f::Zero(4,4);
// inital_guess(0,0) = 1.0;
// inital_guess(1,2) = 1.0;
// inital_guess(2,1) = -1.0;
// inital_guess(3,3) = 1.0;
PointCloudPtr rotated_cloud_ptr (new PointCloud);
transformPointCloud (*input_cloud_ptr, *rotated_cloud_ptr, inital_guess);
// std::vector<PointCloudConstPtr> vis_cloud_ptrs;
// vis_cloud_ptrs.push_back(rotated_cloud_ptr);
//transformPointCloud (*input_cloud_ptr, *output_cloud_ptr, inital_guess);
//assume the model is approx correct, i.e. z is height
// find the largest plane perpendicular to z axis and use this to align cloud
pcl17::ModelCoefficients::Ptr coefficients (new pcl17::ModelCoefficients);
pcl17::PointIndices::Ptr inliers (new pcl17::PointIndices);
pcl17::SACSegmentation<PointType> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl17::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl17::SAC_RANSAC);
seg.setDistanceThreshold (ransac_threshold_);
seg.setMaxIterations(max_iterations_);
//seg.setProbability(0.1);
seg.setAxis (Eigen::Vector3f (0, 0, 1));
seg.setEpsAngle (30.0 * (M_PI / 180));//radians
seg.setInputCloud (rotated_cloud_ptr);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL17_ERROR("Could not estimate a planar model for the given dataset.");
// I had some issues with pcl on a different computer to where I normally dev. This is a workaround to be removed
coefficients->values.push_back(-0.0370391);
coefficients->values.push_back(0.0777064);
coefficients->values.push_back(0.996288);
coefficients->values.push_back(2.63374);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1]
<< " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl;
// calculate angle and axis from dot product
double rotate = acos (coefficients->values[2]);
Eigen::Vector3f axis_to_rotate_about (-coefficients->values[1], coefficients->values[0], 0.0);
axis_to_rotate_about.normalize ();
//convert axis and angle to rotation matrix and apply to pointcloud
Eigen::AngleAxis<float> angle_axis (rotate, axis_to_rotate_about);
transform_output = Eigen::Matrix4f::Identity (4, 4);
transform_output.block<3, 3> (0, 0) = angle_axis.toRotationMatrix ().inverse ();
// apply translation to bring the floor to z = 0
transform_output(2, 3) = coefficients->values[3];
transformPointCloud (*rotated_cloud_ptr, *output_cloud_ptr, transform_output);
// pcl17::ExtractIndices<PointType> eifilter;
// eifilter.setInputCloud (rotated_cloud_ptr);
// eifilter.setIndices (inliers);
// eifilter.filter (*output_cloud_ptr);
// vis_cloud_ptrs.push_back(output_cloud_ptr);
// visualizer.visualizeCloud(vis_cloud_ptrs);
}
/*!
* @brief メソッドPointCloudMethod::extractPlane().平面を検出するメソッド
* @param pcl::PointCloud<pcl::PointXYZ>::Ptr inputPointCloud
* @return pcl::PointCloud<pcl::PointXYZ>::Ptr outputPointCloud
*/
pcl::PointCloud<pcl::PointXYZRGB>::Ptr PointCloudMethod::extractPlane(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &inputPointCloud, bool optimize, double threshold, bool negative)
{
cout << "before Extract Plane => " << inputPointCloud->size() << endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
//セグメンテーションオブジェクトの生成
pcl::SACSegmentation<pcl::PointXYZRGB> seg;
//オプション
seg.setOptimizeCoefficients(optimize);
//Mandatory
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(threshold);
seg.setInputCloud(inputPointCloud->makeShared());
seg.segment(*inliers, *coefficients);
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f(new pcl::PointCloud<pcl::PointXYZRGB>());
//int i = 0, nr_points = (int)inputPointCloud->points.size();
//while (inputPointCloud->points.size() > 0.3*nr_points)
//{
// seg.setInputCloud(inputPointCloud);
// seg.segment(*inliers, *coefficients);
// if (inliers->indices.size() == 0)
// {
// cout << "Could not estimate a planar model for the given dataset." << endl;
// break;
// }
// pcl::ExtractIndices<pcl::PointXYZRGB> extract;
// extract.setInputCloud(inputPointCloud);
// extract.setIndices(inliers);
// extract.setNegative(false);
// extract.filter(*filtered);
// extract.setNegative(true);
// extract.filter(*cloud_f);
// *inputPointCloud = *cloud_f;
//}
//pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);
//tree->setInputCloud(filtered);
//vector<pcl::PointIndices> cluster_indices;
//pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
//ec.setClusterTolerance(0.02); //cm
//ec.setMinClusterSize(100);
//ec.setMaxClusterSize(25000);
//ec.setSearchMethod(tree);
//ec.setInputCloud(filtered);
//ec.extract(cluster_indices);
//int j = 0;
//for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
//{
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
// for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
// {
// cloud_cluster->points.push_back(filtered->points[*pit]);
// }
// cloud_cluster->width = cloud_cluster->points.size();
// cloud_cluster->height = 1;
// cloud_cluster->is_dense = true;
// filtered = cloud_cluster;
// j++;
//}
/*for (size_t i = 0; i < inliers->indices.size(); ++i){
inputPointCloud->points[inliers->indices[i]].r = 255;
inputPointCloud->points[inliers->indices[i]].g = 255;
inputPointCloud->points[inliers->indices[i]].b = 255;
}*/
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(inputPointCloud);
extract.setIndices(inliers);
extract.setNegative(negative);
extract.filter(*filtered);
cout << "after Extract Plane => " << filtered->size() << endl;
return filtered;
}
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("table_scene_lms400.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
}
/*****************************************************************************
// Identify good matches using RANSAC
// Return fundemental matrix
*/
cv::Mat ransacTest( const std::vector<cv::DMatch>& matches,
const std::vector<cv::KeyPoint>& keypoints1,
const std::vector<cv::KeyPoint>& keypoints2,
std::vector<cv::DMatch>& outMatches)
{
// Convert keypoints into Point2f
std::vector<cv::Point2f> points1, points2;
for ( std::vector<cv::DMatch>::
const_iterator it= matches.begin();
it!= matches.end(); ++it)
{
// Get the position of left keypoints
float x= keypoints1[it->queryIdx].pt.x;
float y= keypoints1[it->queryIdx].pt.y;
points1.push_back(cv::Point2f(x,y));
// Get the position of right keypoints
x= keypoints2[it->trainIdx].pt.x;
y= keypoints2[it->trainIdx].pt.y;
points2.push_back(cv::Point2f(x,y));
}
// Compute F matrix using RANSAC
std::vector<uchar> inliers(points1.size(),0);
cv::Mat fundemental= cv::findFundamentalMat(cv::Mat(points1),cv::Mat(points2), // matching points
inliers, // match status (inlier or outlier)
CV_FM_RANSAC, // RANSAC method
_distance, // distance to epipolar line
_confidence); // confidence probability
// extract the surviving (inliers) matches
std::vector<uchar>::const_iterator
itIn= inliers.begin();
std::vector<cv::DMatch>::const_iterator
itM= matches.begin();
// for all matches
for ( ;itIn!= inliers.end(); ++itIn, ++itM)
{
if (*itIn)
{ // it is a valid match
outMatches.push_back(*itM);
}
}
if (_refine)
{
// The F matrix will be recomputed with
// all accepted matches
// Convert keypoints into Point2f
// for final F computation
points1.clear();
points2.clear();
for (std::vector<cv::DMatch>::
const_iterator it= outMatches.begin();
it!= outMatches.end(); ++it)
{
// Get the position of left keypoints
float x= keypoints1[it->queryIdx].pt.x;
float y= keypoints1[it->queryIdx].pt.y;
points1.push_back(cv::Point2f(x,y));
// Get the position of right keypoints
x= keypoints2[it->trainIdx].pt.x;
y= keypoints2[it->trainIdx].pt.y;
points2.push_back(cv::Point2f(x,y));
}
// Compute 8-point F from all accepted matches
//fundemental = cv::findFundamentalMat(cv::Mat(points1),cv::Mat(points2), CV_FM_8POINT); // 8-point method
fundemental = cv::findHomography(cv::Mat(points1),cv::Mat(points2), cv::RANSAC/*CV_LMEDS*/, 1);
}
return fundemental;
}
bool seg_cb(bwi_perception::ButtonDetection::Request &req, bwi_perception::ButtonDetection::Response &res)
{
//get the point cloud by aggregating k successive input clouds
waitForCloudK(15);
cloud = cloud_aggregated;
//**Step 1: z-filter and voxel filter**//
// Create the filtering object
pcl::PassThrough<PointT> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.0, 1.15);
pass.filter (*cloud);
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<PointT> vg;
pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.0025f, 0.0025f, 0.0025f);
vg.filter (*cloud_filtered);
//publish point cloud for debugging
ROS_INFO("Publishing point cloud...");
/*pcl::toROSMsg(*cloud_filtered,cloud_ros);
cloud_ros.header.frame_id = cloud->header.frame_id;
cloud_pub.publish(cloud_ros);*/
ROS_INFO("After voxel grid filter: %i points",(int)cloud_filtered->points.size());
//**Step 2: plane fitting**//
//find palne
//one cloud contains plane other cloud contains other objects
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
// Create the segmentation object
pcl::SACSegmentation<PointT> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (0.02);
// Create the filtering object
pcl::ExtractIndices<PointT> extract;
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
// Extract the plane
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_plane);
//for everything else, cluster extraction; segment extraction
//extract everything else
extract.setNegative (true);
extract.filter (*cloud_blobs);
//get the plane coefficients
Eigen::Vector4f plane_coefficients;
plane_coefficients(0)=coefficients->values[0];
plane_coefficients(1)=coefficients->values[1];
plane_coefficients(2)=coefficients->values[2];
plane_coefficients(3)=coefficients->values[3];
//**Step 3: Eucledian Cluster Extraction**//
std::vector<PointCloudT::Ptr > clusters = computeClusters(cloud_blobs,0.04);
ROS_INFO("clustes found: %i", (int)clusters.size());
clusters_on_plane.clear();
//if clusters are touching the table put them in a vector
for (unsigned int i = 0; i < clusters.size(); i++){
Eigen::Vector4f centroid_i;
pcl::compute3DCentroid(*clusters.at(i), centroid_i);
pcl::PointXYZ center;
center.x=centroid_i(0);center.y=centroid_i(1);center.z=centroid_i(2);
double distance = pcl::pointToPlaneDistance(center, plane_coefficients);
if (distance < 0.1 /*&& clusters.at(i).get()->points.size() >*/ ){
clusters_on_plane.push_back(clusters.at(i));
}
}
ROS_INFO("clustes_on_plane found: %i", (int)clusters_on_plane.size());
// if the clousters size == 0 return false
if(clusters_on_plane.size() == 0) {
cloud_mutex.unlock ();
res.button_found = false;
return true;
}
//**Step 4: detect the button among the remaining clusters**//
int max_index = -1;
double max_red = 0.0;
// Find the max red value
for (unsigned int i = 0; i < clusters_on_plane.size(); i++){
double red_i = computeAvgRedValue(clusters_on_plane.at(i));
//ROS_INFO("Cluster %i: %i points, red_value = %f",i,(int)clusters_on_plane.at(i)->points.size(),red_i);
if (red_i > max_red){
max_red = red_i;
max_index = i;
}
}
//publish cloud if we think it's a button
/*max_red > 170 && max_red < 250 && */
ROS_INFO("max_red=%f", max_red);
if (max_index >= 0 && max_red > red_min) {
ROS_INFO("Button_found");
pcl::toROSMsg(*clusters_on_plane.at(max_index),cloud_ros);
cloud_ros.header.frame_id = cloud->header.frame_id;
cloud_pub.publish(cloud_ros);
//fill in response
res.button_found = true;
res.cloud_button = cloud_ros;
/*Eigen::Vector4f centroid;
pcl::compute3DCentroid(*clusters_on_plane.at(max_index), centroid);
//transforms the pose into /map frame
geometry_msgs::Pose pose_i;
pose_i.position.x=centroid(0);
pose_i.position.y=centroid(1);
pose_i.position.z=centroid(2);
pose_i.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,-3.14/2);
geometry_msgs::PoseStamped stampedPose;
stampedPose.header.frame_id = cloud->header.frame_id;
stampedPose.header.stamp = ros::Time(0);
stampedPose.pose = pose_i;*/
//geometry_msgs::PoseStamped stampOut;
//listener.waitForTransform(cloud->header.frame_id, "m1n6s200_link_base", ros::Time(0), ros::Duration(3.0));
//listener.transformPose("m1n6s200_link_base", stampedPose, stampOut);
//stampOut.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,-3.14/2,0);
//pose_pub.publish(stampOut);
}
else {
res.button_found = false;
}
cloud_mutex.unlock ();
return true;
}
int main (int argc, char** argv)
{
PointCloudT::Ptr cloud (new PointCloudT);
PointCloudT::Ptr new_cloud (new PointCloudT);
bool new_cloud_available_flag = false;
//pcl::Grabber* grab = new pcl::OpenNIGrabber ();
PointCloudT::Ptr ddd;
boost::function<void (const PointCloudT::ConstPtr&)> f =
boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag);
grab->registerCallback (f);
viewer->registerKeyboardCallback(keyboardEventOccurred);
grab->start ();
bool first_time = true;
FILE* objects;
objects = fopen ("objects.txt","a");
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag=false;
////////////////////
// invert correction
////////////////////
Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity();
transMat (1,1) = -1;
////////////////////
// pass filter
////////////////////
PointCloudT::Ptr passed_cloud;
pcl::PassThrough<PointT> pass;
passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
PointCloudT::Ptr voxelized_cloud;
voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::VoxelGrid<PointT> vg;
vg.setLeafSize (0.001, 0.001, 0.001);
////////////////////
// sac segmentation
////////////////////
PointCloudT::Ptr cloud_f;
PointCloudT::Ptr cloud_plane;
PointCloudT::Ptr cloud_filtered;
cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
pcl::SACSegmentation<PointT> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
////////////////////
// euclidean clustering
////////////////////
std::vector<pcl::PointIndices> cluster_indices;
std::vector<PointCloudT::Ptr> extracted_clusters;
pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>);
pcl::EuclideanClusterExtraction<PointT> ec;
ec.setClusterTolerance (0.04);
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (eucl_tree);
PointCloudT::Ptr cloud_cluster;
////////////////////
// vfh estimate
////////////////////
pcl::NormalEstimation<PointT, pcl::Normal> ne;
pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ());
pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ());
std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs;
ne.setSearchMethod (vfh_tree1);
ne.setRadiusSearch (0.05);
vfh.setSearchMethod (vfh_tree2);
vfh.setRadiusSearch (0.05);
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs;
////////////////////
// nearest neighbour
////////////////////
int k = 6;
std::string kdtree_idx_file_name = "kdtree.idx";
std::string training_data_h5_file_name = "training_data.h5";
std::string training_data_list_file_name = "training_data.list";
// std::vector<vfh_model> models;
// flann::Matrix<float> data;
// loadFileList (models, training_data_list_file_name);
// flann::load_from_file (data,
// training_data_h5_file_name,
// "training_data");
//
// flann::Index<flann::ChiSquareDistance<float> > index (data,
// flann::SavedIndexParams
// ("kdtree.idx"));
////////////////////
// process nearest neighbour
////////////////////
std::vector<hypothesis> final_hypothesis;
final_hypothesis.clear();
double last = pcl::getTime();
while (! viewer->wasStopped())
{
if (new_cloud_available_flag)
{
new_cloud_available_flag = false;
double now = pcl::getTime();
////////////////////
// pass filter
////////////////////
//passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// voxel grid
////////////////////
//voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
////////////////////
// sac segmentation
////////////////////
//cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);
//cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);
//cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);
////////////////////
// euclidean clustering
////////////////////
extracted_clusters.clear();
cluster_indices.clear();
////////////////////
// vfh estimate
////////////////////
computed_vfhs.clear();
////////////////////
// nearest neighbour
////////////////////
cloud_mutex.lock();
//displayCloud(viewer,cloud);
boost::thread displayCloud_(displayCloud,viewer,cloud);
if(now-last > 13 || first_time)
{
first_time = false;
last=now;
////////////////////
// invert correction
////////////////////
pcl::transformPointCloud(*cloud,*new_cloud,transMat);
////////////////////
// pass filter
////////////////////
pass.setInputCloud (new_cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-0.5, 0.5);
//pass.setFilterLimitsNegative (true);
pass.filter (*passed_cloud);
////////////////////
// voxel grid
////////////////////
vg.setInputCloud (passed_cloud);
vg.filter (*voxelized_cloud);
////////////////////
// sac segmentation
////////////////////
*cloud_filtered = *voxelized_cloud;
int i=0, nr_points = (int) voxelized_cloud->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Couldnt estimate a planar model for the dataset.\n";
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Get the points associated with the planar surface
extract.filter (*cloud_plane);
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
////////////////////
// euclidean clustering
////////////////////
// Creating the KdTree object for the search method of the extraction
eucl_tree->setInputCloud (cloud_filtered);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
//PointCloudT::Ptr cloud_cluster (new PointCloudT);
cloud_cluster = boost::shared_ptr<PointCloudT>(new PointCloudT);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points [*pit]);
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
extracted_clusters.push_back(cloud_cluster);
}
cloud_cluster.reset();
////////////////////
// vfh estimate
////////////////////
for (int z=0; z<extracted_clusters.size(); ++z)
{
vfhs = boost::shared_ptr<pcl::PointCloud<pcl::VFHSignature308> > (new pcl::PointCloud<pcl::VFHSignature308>);
normals = boost::shared_ptr<pcl::PointCloud<pcl::Normal> > (new pcl::PointCloud<pcl::Normal>);
ne.setInputCloud (extracted_clusters.at(z));
ne.compute (*normals);
vfh.setInputCloud (extracted_clusters.at(z));
vfh.setInputNormals (normals);
vfh.compute (*vfhs);
computed_vfhs.push_back(vfhs);
}
vfhs.reset();
normals.reset();
////////////////////
// nearest neighbour
////////////////////
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
loadFileList (models, training_data_list_file_name);
flann::load_from_file (data,
training_data_h5_file_name,
"training_data");
flann::Index<flann::ChiSquareDistance<float> > index
(data,
flann::SavedIndexParams
("kdtree.idx"));
for(int z=0; z<computed_vfhs.size(); ++z)
{
vfh_model histogram;
histogram.second.resize(308);
for (size_t i = 0; i < 308; ++i)
{
histogram.second[i] = computed_vfhs.at(z)->points[0].histogram[i];
}
index.buildIndex ();
nearestKSearch (index, histogram, k, k_indices, k_distances);
hypothesis x;
x.distance = k_distances[0][0];
size_t index = models.at(k_indices[0][0]).first.find("_v",5);
x.object_name = models.at(k_indices[0][0]).first.substr(5,index-5);
ddd = boost::shared_ptr<PointCloudT>(new PointCloudT);
pcl::transformPointCloud(*extracted_clusters.at(z),*ddd,transMat);
x.cluster = ddd;
ddd.reset();
std::string filename ="";
filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
x.cluster_name = filename.c_str();
final_hypothesis.push_back(x);
x.cluster.reset();
//delete x;
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j+1);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// const char* filen = filename.c_str();
// fprintf(objects,"%s",filen);
// fprintf(objects,"::");
// fprintf(objects,models.at (k_indices[0][0]).first.c_str());
// fprintf(objects,"::");
// fprintf(objects,"%f",k_distances[0][0]);
// fprintf(objects,"\n");
}
delete[] k_indices.ptr ();
delete[] k_distances.ptr ();
delete[] data.ptr ();
std::cout << final_hypothesis.size() << std::endl;
viewer->removeAllShapes();
for(int z=0; z<final_hypothesis.size();++z)
{
if(final_hypothesis.at(z).distance < 100)
{
fprintf(objects,"%s",final_hypothesis.at(z).cluster_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%s",final_hypothesis.at(z).object_name.c_str());
fprintf(objects,"::");
fprintf(objects,"%f",final_hypothesis.at(z).distance);
fprintf(objects,"\n");
std::stringstream ddd;
ddd << final_hypothesis.at(z).object_name;
ddd << "\n" << "(";
ddd << final_hypothesis.at(z).distance;
ddd << ")";
viewer->addText3D(ddd.str().c_str(),
final_hypothesis.at(z).cluster->points[0],
0.02,1,1,1,
boost::lexical_cast<std::string>(z));
drawBoundingBox(final_hypothesis.at(z).cluster,viewer,z);
}
}
//boost::thread allBoxes_(allBoxes,viewer,final_hypothesis);
//allBoxes_.join();
viewer->spinOnce();
final_hypothesis.clear();
j++;
}
// for(int z=0; z<extracted_clusters.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + ".pcd";
// pcl::io::savePCDFile(filename,*extracted_clusters.at(z),false);
// }
// for(int z=0; z<computed_vfhs.size(); ++z)
// {
// //viewer->addPointCloud<PointT>(extracted_clusters.at(z),
// // boost::lexical_cast<std::string>(z));
//
// std::string filename ="";
// filename += "inputcloud_" + boost::lexical_cast<std::string>(j);
// filename += "_" + boost::lexical_cast<std::string>(z) + "_vfhs.pcd";
// pcl::io::savePCDFileASCII<pcl::VFHSignature308> (filename, *computed_vfhs.at(z));
// }
//viewer->removeAllShapes();
// viewer->removeAllPointClouds();
// viewer->setCameraPosition(0, 0, 0, 0, 0, 1, 0, -1, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
// viewer->addPointCloud<PointT>(cloud,rgb,"input_cloud");
// viewer->spinOnce();
//std::cout << final_hypothesis.at(0).cluster->points[0];
//boost::this_thread::sleep(boost::posix_time::milliseconds(10));
displayCloud_.join();
cloud_mutex.unlock();
}
}
grab->stop();
return 0;
}
// Identify good matches using RANSAC
// Return number of matched points
cv::Mat RobustMatcher::ransacTest(const std::vector<cv::DMatch>& matches,
const std::vector<cv::KeyPoint>& keypoints1,
const std::vector<cv::KeyPoint>& keypoints2, std::vector<cv::DMatch>& outMatches) {
// Convert keypoints into Point2f
std::vector<cv::Point2f> points1, points2;
for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it!= matches.end(); ++it) {
// Get the position of left keypoints
float x = keypoints1[it->queryIdx].pt.x;
float y = keypoints1[it->queryIdx].pt.y;
points1.push_back(cv::Point2f(x,y));
// Get the position of right keypoints
x = keypoints2[it->trainIdx].pt.x;
y = keypoints2[it->trainIdx].pt.y;
points2.push_back(cv::Point2f(x,y));
}
if(points1.empty() || points2.empty()) return cv::Mat();
// Compute F matrix using RANSAC
std::vector<uchar> inliers(points1.size(),0);
cv::Mat fundamental = cv::findFundamentalMat(
cv::Mat(points1),cv::Mat(points2), // matching points
inliers, // match status (inlier ou outlier)
CV_FM_RANSAC, // RANSAC method
this->_distance, // distance to epipolar line
this->_confidence); // confidence probability
// Extract the surviving (inliers) matches
std::vector<uchar>::const_iterator itIn = inliers.begin();
std::vector<cv::DMatch>::const_iterator itM = matches.begin();
// For all matches
for ( ;itIn!= inliers.end(); ++itIn, ++itM) {
if (*itIn) { // it is a valid match
outMatches.push_back(*itM);
}
}
std::cout << "Number of matched points (after cleaning): " << outMatches.size() << std::endl;
if (outMatches.size()>13){
//std::cout << "true";
this->_reconized = true;
}
if (outMatches.empty()) return fundamental;
if (this->_refineF) {
// The F matrix will be recomputed with all accepted matches
// Convert keypoints into Point2f for final F computation
points1.clear();
points2.clear();
for (std::vector<cv::DMatch>::const_iterator it= outMatches.begin();
it!= outMatches.end(); ++it) {
// Get the position of left keypoints
float x= keypoints1[it->queryIdx].pt.x;
float y= keypoints1[it->queryIdx].pt.y;
points1.push_back(cv::Point2f(x,y));
// Get the position of right keypoints
x = keypoints2[it->trainIdx].pt.x;
y = keypoints2[it->trainIdx].pt.y;
points2.push_back(cv::Point2f(x,y));
}
// Compute 8-point F from all accepted matches
fundamental= cv::findFundamentalMat(
cv::Mat(points1),cv::Mat(points2), // matching points
CV_FM_8POINT); // 8-point method
}
return fundamental;
}
