void
estimateNormals (const typename PointCloud<PointT>::ConstPtr &input, PointCloud<Normal> &normals)
{
if (input->isOrganized ())
{
IntegralImageNormalEstimation<PointT, Normal> ne;
// Set the parameters for normal estimation
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
// Estimate normals in the cloud
ne.setInputCloud (input);
ne.compute (normals);
// Save the distance map for the plane comparator
float *map=ne.getDistanceMap ();// This will be deallocated with the IntegralImageNormalEstimation object...
distance_map_.assign(map, map+input->size() ); //...so we must copy the data out
plane_comparator_->setDistanceMap(distance_map_.data());
}
else
{
NormalEstimation<PointT, Normal> ne;
ne.setInputCloud (input);
ne.setRadiusSearch (0.02f);
ne.setSearchMethod (search_);
ne.compute (normals);
}
}
void
segment (const PointT &picked_point,
int picked_idx,
PlanarRegion<PointT> ®ion,
typename PointCloud<PointT>::Ptr &object)
{
object.reset ();
// Segment out all planes
vector<ModelCoefficients> model_coefficients;
vector<PointIndices> inlier_indices, boundary_indices;
vector<PlanarRegion<PointT>, Eigen::aligned_allocator<PlanarRegion<PointT> > > regions;
// Prefer a faster method if the cloud is organized, over RANSAC
if (cloud_->isOrganized ())
{
print_highlight (stderr, "Estimating normals ");
TicToc tt; tt.tic ();
// Estimate normals
PointCloud<Normal>::Ptr normal_cloud (new PointCloud<Normal>);
estimateNormals (cloud_, *normal_cloud);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", normal_cloud->size ()); print_info (" points]\n");
OrganizedMultiPlaneSegmentation<PointT, Normal, Label> mps;
mps.setMinInliers (1000);
mps.setAngularThreshold (deg2rad (3.0)); // 3 degrees
mps.setDistanceThreshold (0.03); // 2 cm
mps.setMaximumCurvature (0.001); // a small curvature
mps.setProjectPoints (true);
mps.setComparator (plane_comparator_);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud_);
// Use one of the overloaded segmentAndRefine calls to get all the information that we want out
PointCloud<Label>::Ptr labels (new PointCloud<Label>);
vector<PointIndices> label_indices;
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
}
else
{
SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients (true);
seg.setModelType (SACMODEL_PLANE);
seg.setMethodType (SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.005);
// Copy XYZ and Normals to a new cloud
typename PointCloud<PointT>::Ptr cloud_segmented (new PointCloud<PointT> (*cloud_));
typename PointCloud<PointT>::Ptr cloud_remaining (new PointCloud<PointT>);
ModelCoefficients coefficients;
ExtractIndices<PointT> extract;
PointIndices::Ptr inliers (new PointIndices ());
// Up until 30% of the original cloud is left
int i = 1;
while (double (cloud_segmented->size ()) > 0.3 * double (cloud_->size ()))
{
seg.setInputCloud (cloud_segmented);
print_highlight (stderr, "Searching for the largest plane (%2.0d) ", i++);
TicToc tt; tt.tic ();
seg.segment (*inliers, coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", inliers->indices.size ()); print_info (" points]\n");
// No datasets could be found anymore
if (inliers->indices.empty ())
break;
// Save this plane
PlanarRegion<PointT> region;
region.setCoefficients (coefficients);
regions.push_back (region);
inlier_indices.push_back (*inliers);
model_coefficients.push_back (coefficients);
// Extract the outliers
extract.setInputCloud (cloud_segmented);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_remaining);
cloud_segmented.swap (cloud_remaining);
}
}
print_highlight ("Number of planar regions detected: %lu for a cloud of %lu points\n", regions.size (), cloud_->size ());
double max_dist = numeric_limits<double>::max ();
// Compute the distances from all the planar regions to the picked point, and select the closest region
int idx = -1;
for (size_t i = 0; i < regions.size (); ++i)
{
double dist = pointToPlaneDistance (picked_point, regions[i].getCoefficients ());
if (dist < max_dist)
{
max_dist = dist;
idx = static_cast<int> (i);
}
}
// Get the plane that holds the object of interest
if (idx != -1)
{
plane_indices_.reset (new PointIndices (inlier_indices[idx]));
if (cloud_->isOrganized ())
{
approximatePolygon (regions[idx], region, 0.01f, false, true);
print_highlight ("Planar region: %lu points initial, %lu points after refinement.\n", regions[idx].getContour ().size (), region.getContour ().size ());
}
else
{
// Save the current region
region = regions[idx];
print_highlight (stderr, "Obtaining the boundary points for the region ");
TicToc tt; tt.tic ();
// Project the inliers to obtain a better hull
typename PointCloud<PointT>::Ptr cloud_projected (new PointCloud<PointT>);
ModelCoefficients::Ptr coefficients (new ModelCoefficients (model_coefficients[idx]));
ProjectInliers<PointT> proj;
proj.setModelType (SACMODEL_PLANE);
proj.setInputCloud (cloud_);
proj.setIndices (plane_indices_);
proj.setModelCoefficients (coefficients);
proj.filter (*cloud_projected);
// Compute the boundary points as a ConvexHull
ConvexHull<PointT> chull;
chull.setDimension (2);
chull.setInputCloud (cloud_projected);
PointCloud<PointT> plane_hull;
chull.reconstruct (plane_hull);
region.setContour (plane_hull);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%lu", plane_hull.size ()); print_info (" points]\n");
}
}
// Segment the object of interest
if (plane_indices_ && !plane_indices_->indices.empty ())
{
plane_.reset (new PointCloud<PointT>);
copyPointCloud (*cloud_, plane_indices_->indices, *plane_);
object.reset (new PointCloud<PointT>);
segmentObject (picked_idx, cloud_, plane_indices_, *object);
}
}
typename PointCloud<PointT>::Ptr transform(
const typename PointCloud<PointT>::Ptr source,
const typename PointCloud<PointT>::Ptr target,boost::shared_ptr<Configuration> configuration)
{
source_cloud = *source;
target_cloud = *target;
cout << "[PFHTransformStrategy::transform] Setting source cloud: "
<< source->size() << " points" << endl;
cout << "[PFHTransformStrategy::transform] Setting target cloud: "
<< target->size() << " points" << endl;
typename PointCloud<PointT>::Ptr transformed(new PointCloud<PointT>);
typename PointCloud<PointXYZRGB>::Ptr source_points(source);
typename PointCloud<PointXYZRGB>::Ptr source_filtered(
new PointCloud<PointT>);
PointCloud<Normal>::Ptr source_normals(new PointCloud<Normal>);
PointCloud<PointWithScale>::Ptr source_keypoints(
new PointCloud<PointWithScale>);
PointCloud<PFHSignature125>::Ptr source_descriptors(
new PointCloud<PFHSignature125>);
typename PointCloud<PointT>::Ptr target_points(target);
typename PointCloud<PointT>::Ptr target_filtered(
new PointCloud<PointT>);
PointCloud<Normal>::Ptr target_normals(
new PointCloud<Normal>);
PointCloud<PointWithScale>::Ptr target_keypoints(
new PointCloud<PointWithScale>);
PointCloud<PFHSignature125>::Ptr target_descriptors(
new PointCloud<PFHSignature125>);
cout << "[PFHTransformStrategy::transform] Downsampling source and "
<< "target clouds..." << endl;
//filter(source_points, source_filtered, 0.01f);
//filter(target_points, target_filtered, 0.01f);
source_filtered=source_points;
target_filtered=target_points;
cout << "[PFHTransformStrategy::transform] Creating normals for "
<< "source and target cloud..." << endl;
create_normals<Normal>(source_filtered, source_normals);
create_normals<Normal>(target_filtered, target_normals);
cout << "[PFHTransformStrategy::transform] Finding keypoints in "
<< "source and target cloud..." << endl;
detect_keypoints(source_filtered, source_keypoints);
detect_keypoints(target_filtered, target_keypoints);
for(PointWithScale p: source_keypoints->points){
cout<<"keypoint "<<p;
}
vector<int> source_indices(source_keypoints->points.size());
vector<int> target_indices(target_keypoints->points.size());
cout << "[PFHTransformStrategy::transform] Computing PFH features "
<< "for source and target cloud..." << endl;
compute_PFH_features_at_keypoints(source_filtered, source_normals,source_keypoints,
source_descriptors, target_indices);
compute_PFH_features_at_keypoints(target_filtered, target_normals,target_keypoints,
target_descriptors, target_indices);
vector<int> correspondences;
vector<float> correspondence_scores;
find_feature_correspondence(source_descriptors, target_descriptors,
correspondences, correspondence_scores);
cout << "correspondences: " << correspondences.size() << endl;
cout << "c. scores: " << correspondence_scores.size() << endl;
cout << "First cloud: Found " << source_keypoints->size() << " keypoints "
<< "out of " << source_filtered->size() << " total points." << endl;
cout << "Second cloud: Found " << target_keypoints->size() << " keypoints"
<< " out of " << target_filtered->size() << " total points." << endl;
// Start with the actual transformation. Yeay :)
TransformationFromCorrespondences tfc;
tfc.reset();
vector<int> sorted_scores;
cv::sortIdx(correspondence_scores, sorted_scores, CV_SORT_EVERY_ROW);
vector<float> tmp(correspondence_scores);
sort(tmp.begin(), tmp.end());
float median_score = tmp[tmp.size() / 2];
vector<int> fidx;
vector<int> fidxt;
Eigen::Vector3f source_position(0, 0, 0);
Eigen::Vector3f target_position(0, 0, 0);
for (size_t i = 0; i < correspondence_scores.size(); i++) {
int index = sorted_scores[i];
if (median_score >= correspondence_scores[index]) {
source_position[0] = source_keypoints->points[index].x;
source_position[1] = source_keypoints->points[index].y;
source_position[2] = source_keypoints->points[index].z;
target_position[0] = target_keypoints->points[index].x;
target_position[1] = target_keypoints->points[index].y;
target_position[2] = target_keypoints->points[index].z;
if (abs(source_position[1] - target_position[1]) > 0.2) {
continue;
}
// cout<< "abs position difference:"<<abs(source_position[1] - target_position[1])<<"C-Score"<<correspondence_scores[index]<<endl;
// cout<<" Source Position " <<source_position<<endl;
// cout<<" target position "<<target_position<<endl;
tfc.add(source_position, target_position,
correspondence_scores[index]);
fidx.push_back(source_indices[index]);
fidxt.push_back(target_indices[correspondences[index]]);
}
}
cout << "TFC samples: "<<tfc.getNoOfSamples()<<" AccumulatedWeight "<<tfc.getAccumulatedWeight()<<endl;
Eigen::Affine3f tr;
tr = tfc.getTransformation();
cout << "TFC transformation: " << endl;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
cout << tr.rotation()(i, i) << "\t";
}
cout << endl;
}
transformPointCloud(*source_filtered, *transformed,
tfc.getTransformation());
cout << "transformation finished";
return transformed;
};
/** \brief does radius search and tests the results to be unique, valid and ordered. Additionally it test whether all test methods are returning the same results
* \param cloud the input point cloud
* \param search_methods vector of all search methods to be tested
* \param query_indices indices of query points in the point cloud (not necessarily in input_indices)
* \param input_indices indices defining a subset of the point cloud.
*/
template<typename PointT> void
testRadiusSearch (typename PointCloud<PointT>::ConstPtr point_cloud, vector<search::Search<PointT>*> search_methods,
const vector<int>& query_indices, const vector<int>& input_indices = vector<int> ())
{
vector< vector<int> >indices (search_methods.size ());
vector< vector<float> >distances (search_methods.size ());
vector <bool> passed (search_methods.size (), true);
vector<bool> indices_mask (point_cloud->size (), true);
vector<bool> nan_mask (point_cloud->size (), true);
if (input_indices.size () != 0)
{
indices_mask.assign (point_cloud->size (), false);
for (vector<int>::const_iterator iIt = input_indices.begin (); iIt != input_indices.end (); ++iIt)
indices_mask [*iIt] = true;
}
// remove also Nans
#pragma omp parallel for
for (int pIdx = 0; pIdx < int (point_cloud->size ()); ++pIdx)
{
if (!isFinite (point_cloud->points [pIdx]))
nan_mask [pIdx] = false;
}
boost::shared_ptr<vector<int> > input_indices_;
if (input_indices.size ())
input_indices_.reset (new vector<int> (input_indices));
#pragma omp parallel for
for (int sIdx = 0; sIdx < int (search_methods.size ()); ++sIdx)
search_methods [sIdx]->setInputCloud (point_cloud, input_indices_);
// test radii 0.01, 0.02, 0.04, 0.08
for (float radius = 0.01f; radius < 0.1f; radius *= 2.0f)
{
//cout << radius << endl;
// find nn for each point in the cloud
for (vector<int>::const_iterator qIt = query_indices.begin (); qIt != query_indices.end (); ++qIt)
{
#pragma omp parallel for
for (int sIdx = 0; sIdx < static_cast<int> (search_methods.size ()); ++sIdx)
{
search_methods [sIdx]->radiusSearch (point_cloud->points[*qIt], radius, indices [sIdx], distances [sIdx], 0);
passed [sIdx] = passed [sIdx] && testUniqueness (indices [sIdx], search_methods [sIdx]->getName ());
passed [sIdx] = passed [sIdx] && testOrder (distances [sIdx], search_methods [sIdx]->getName ());
passed [sIdx] = passed [sIdx] && testResultValidity<PointT>(point_cloud, indices_mask, nan_mask, indices [sIdx], input_indices, search_methods [sIdx]->getName ());
}
// compare results to each other
#pragma omp parallel for
for (int sIdx = 1; sIdx < static_cast<int> (search_methods.size ()); ++sIdx)
{
passed [sIdx] = passed [sIdx] && compareResults (indices [0], distances [0], search_methods [0]->getName (),
indices [sIdx], distances [sIdx], search_methods [sIdx]->getName (), 1e-6f);
}
}
}
for (unsigned sIdx = 0; sIdx < search_methods.size (); ++sIdx)
{
cout << search_methods [sIdx]->getName () << ": " << (passed[sIdx]?"passed":"failed") << endl;
EXPECT_TRUE (passed [sIdx]);
}
}
// Downsample a point cloud by using a voxel grid.
// See http://pointclouds.org/documentation/tutorials/voxel_grid.php
void filter (typename PointCloud<PointT>::Ptr cloud,
typename PointCloud<PointT>::Ptr cloud_filtered,
float leaf_size=0.01f)
{
cout << "[PFHTransformationStrategy::filter] Input cloud:"
<< endl << " " << cloud->points.size() << " points" << endl;
typename PointCloud<PointT>::Ptr tmp_ptr1(new PointCloud<PointT>);
VoxelGrid<PointT> vox_grid;
vox_grid.setInputCloud(cloud);
vox_grid.setSaveLeafLayout(true);
vox_grid.setLeafSize(leaf_size, leaf_size, leaf_size);
cout << "[PFHTransformationStrategy::filter] Creating a voxel grid:"
<< endl << " leaf size: [" << leaf_size << ", "
<< leaf_size << ", " << leaf_size << "]" << endl;
// Skip the rest...
// vox_grid.filter(*tmp_ptr1);
vox_grid.filter(*cloud_filtered);
cout << "[PFHTransformationStrategy::filter] Result of voxel grid"
<< " filtering:" << endl << " " << cloud_filtered->points.size()
<< " points remaining" << endl;
return;
typename PointCloud<PointT>::Ptr tmp_ptr2(new PointCloud<PointT>);
float pass1_limit_min = 0.0;
float pass1_limit_max = 3.0;
PassThrough<PointT> pass1;
pass1.setInputCloud(tmp_ptr1);
pass1.setFilterFieldName("z");
pass1.setFilterLimits(pass1_limit_min, pass1_limit_max);
cout << "[PFH Transformation : Downsampling] starting first filter"
<< " pass through:" << endl;
cout << " filter field name: " << pass1.getFilterFieldName() << endl;
cout << " filter limits: min = " << pass1_limit_min << ", max = "
<< pass1_limit_max << endl;
float avg;
for (PointT point : *tmp_ptr1) {
avg += point.z;
}
cout << " average field value: " << avg / tmp_ptr1->size() << endl;
pass1.filter(*tmp_ptr2);
cout << "[PFH Transformation : Downsampling] result of first pass:"******" " << tmp_ptr2->points.size() << " points remaining."
<< endl;
float pass2_limit_min = -2.0;
float pass2_limit_max = 1.0;
PassThrough<PointT> pass2;
pass2.setInputCloud(tmp_ptr2);
pass2.setFilterFieldName("x");
pass2.setFilterLimits(pass2_limit_min, pass2_limit_max);
cout << "[PFH Transformation : Downsampling] starting second filter"
<< " pass through:" << endl;
cout << " filter field name: " << pass2.getFilterFieldName() << endl;
cout << " filter limits: min = " << pass2_limit_min << ", max = "
<< pass2_limit_max << endl;
pass2.filter(*cloud_filtered);
cout << "[PFH Transformation : Downsampling] result of second pass:"******" " << cloud_filtered->points.size()
<< " points remaining" << endl;
cout << "[PFH Transformation : Downsampling] size of output cloud:"
<< endl << " " << cloud_filtered->points.size() << " points"
<< endl << endl;
};