TEST (GraphCommon, ComputeSignedCurvature)
{
const size_t ROWS = 5;
const size_t COLS = 12;
const float STEP = 1.0;
// Create a "staircase" cloud
//
// top view side view
//
// * * * * * * * * * * * * *
// * * * * * * * * *
// * * * * * * * * *
// * * * * * * * * *
// * * * * * * * * * * * *
//
PointCloudPtr cloud (new PointCloud (ROWS * COLS, 1));
size_t idx = 0;
for (size_t y = 0; y < ROWS; ++y)
{
for (size_t x = 0; x < COLS; ++x, ++idx)
{
cloud->at (idx).y = y * STEP;
if (x < COLS / 3)
{
cloud->at (idx).x = x * STEP;
cloud->at (idx).z = -5.0f;
}
else if (x < 2 * COLS / 3)
{
cloud->at (idx).x = (COLS / 3 - 1) * STEP;
cloud->at (idx).z = (x - COLS / 3 + 1) * STEP - 5.0f;
}
else
{
cloud->at (idx).x = (x - COLS / 3) * STEP;
cloud->at (idx).z = (COLS / 3) * STEP - 5.0f;
}
}
}
pcl::graph::NearestNeighborsGraphBuilder<PointT, Graph> gb (6);
gb.setInputCloud (cloud);
Graph graph;
gb.compute (graph);
pcl::graph::computeNormalsAndCurvatures (graph);
// Expect that curvatures are all non-negative
for (Graph::vertex_descriptor i = 0; i < boost::num_vertices (graph); ++i)
EXPECT_LE (0.0f, graph[i].curvature);
pcl::graph::computeSignedCurvatures (graph);
// Expect curvatures of the points on the concave corner to be less than zero,
// and on the convex corner the other way round.
for (Graph::vertex_descriptor i = 0; i < boost::num_vertices (graph); ++i)
if (i % COLS == 3)
EXPECT_GT (0.0f, graph[i].curvature);
else if (i % COLS == 7)
EXPECT_LT (0.0f, graph[i].curvature);
}
//if a point is in a cloud
bool isPointInCloud(Point pt, PointCloudPtr cloud){
for(int i=0;i<cloud->size();i++){
if(pt.x==cloud->at(i).x&&pt.y==cloud->at(i).y&&pt.z==cloud->at(i).z){
return true;
}
}
return false;
}
void removeOutlier(
PointCloudPtr cloud,
PointCloudPtr cloud_output,
int K,
float distanceThreshold
)
{
int size = cloud->width * cloud->height;
pcl::KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud(cloud);
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
std::vector<int> chosenIndices;
for (int i=0; i<size; ++i)
{
PointT point = cloud->points[i];
float avgKDis = averageKDistance(kdtree, pointIdxNKNSearch, pointNKNSquaredDistance, point, K);
if (avgKDis<distanceThreshold) chosenIndices.push_back(i);
}
cloud_output->width = chosenIndices.size();
cloud_output->height = 1;
cloud_output->resize(chosenIndices.size());
for (int i=0; i<chosenIndices.size(); ++i) cloud_output->points[i]=cloud->points[chosenIndices[i]];
}
template<typename PointInT> void
pcl::nurbs::NurbsFitter<PointInT>::setInputCloud (PointCloudPtr &pcl_cloud)
{
if (pcl_cloud.get () == 0 || pcl_cloud->points.size () == 0)
throw std::runtime_error ("[NurbsFitter::setInputCloud] Error: Empty or invalid pcl-point-cloud.\n");
cloud_ = pcl_cloud;
have_cloud_ = true;
}
template<typename PointT, typename LeafT, typename BranchT, typename OctreeT> void
pcl::octree::OctreePointCloud<PointT, LeafT, BranchT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg)
{
assert (cloud_arg==input_);
cloud_arg->push_back (point_arg);
this->addPointIdx (static_cast<const int> (cloud_arg->points.size ()) - 1);
}
TEST (GraphCommon, CreateSubgraphsFromIndicesSingle)
{
const size_t N = 100;
PointCloudPtr cloud = generateRandomPlanarCloud (N);
Subgraph graph (cloud);
pcl::PointIndices indices;
for (size_t i = 0; i < N; ++i)
if (i % 10)
indices.indices.push_back (i);
std::vector<SubgraphRef> subgraphs;
pcl::graph::createSubgraphsFromIndices (graph, indices, subgraphs);
ASSERT_EQ (2, subgraphs.size ());
ASSERT_EQ (indices.indices.size (), boost::num_vertices (subgraphs[0].get ()));
ASSERT_EQ (N - indices.indices.size (), boost::num_vertices (subgraphs[1].get ()));
for (size_t i = 0; i < boost::num_vertices (subgraphs[0].get ()); ++i)
EXPECT_XYZ_EQ (cloud->at (indices.indices[i]), subgraphs[0].get ()[i]);
}
template<typename PointT, typename LeafContainerT, typename BranchContainerT, typename OctreeT> void
pcl::octree::OctreePointCloud<PointT, LeafContainerT, BranchContainerT, OctreeT>::addPointToCloud (const PointT& point_arg, PointCloudPtr cloud_arg,
IndicesPtr indices_arg)
{
assert (cloud_arg==input_);
assert (indices_arg==indices_);
cloud_arg->push_back (point_arg);
this->addPointFromCloud (static_cast<const int> (cloud_arg->points.size ()) - 1, indices_arg);
}
//find nearest neighbor
bool findNearestNeighbor(PointCloudPtr cloud, PointCloudPtr except_cloud, Point search_pt, Point& finded_pt){
int min=10;
bool flag=false;
for(int i=0;i<cloud->size();i++){
int dis=sqrt(pow(search_pt.x-cloud->at(i).x, 2)+pow(search_pt.y-cloud->at(i).y, 2)+pow(search_pt.z-cloud->at(i).z, 2));
if(dis<min){
min=dis;
finded_pt.x=cloud->at(i).x;
finded_pt.y=cloud->at(i).y;
finded_pt.z=cloud->at(i).z;
if(!isPointInCloud(finded_pt, except_cloud)){
flag=true;
}
}
}
return flag;
}
TEST (GraphCommon, CreateSubgraphsFromIndicesVector)
{
const size_t N = 100;
PointCloudPtr cloud = generateRandomPlanarCloud (N);
Subgraph graph (cloud);
std::vector<pcl::PointIndices> indices (3);
for (size_t i = 0; i < N; ++i)
indices[i % 3].indices.push_back (i);
std::vector<SubgraphRef> subgraphs;
pcl::graph::createSubgraphsFromIndices (graph, indices, subgraphs);
ASSERT_EQ (indices.size () + 1, subgraphs.size ());
ASSERT_EQ (0, boost::num_vertices (subgraphs[3].get ()));
for (size_t i = 0; i < indices.size (); ++i)
{
ASSERT_EQ (indices[i].indices.size (), boost::num_vertices (subgraphs[i].get ()));
for (size_t j = 0; j < boost::num_vertices (subgraphs[i].get ()); ++j)
EXPECT_XYZ_EQ (cloud->at (indices[i].indices[j]), subgraphs[i].get ()[j]);
}
}
void ppfmap::CudaPPFMatch<PointT, NormalT>::detect(
const PointCloudPtr cloud,
const NormalsPtr normals,
std::vector<Pose>& poses) {
float affine_s[12];
std::vector<Pose> pose_vector;
const float radius = map->getCloudDiameter() * neighborhood_percentage;
std::vector<int> indices;
std::vector<float> distances;
pcl::KdTreeFLANN<PointT> kdtree;
kdtree.setInputCloud(cloud);
if (!use_indices) {
ref_point_indices->resize(cloud->size());
for (int i = 0; i < cloud->size(); i++) {
(*ref_point_indices)[i] = i;
}
}
poses.clear();
for (const auto index : *ref_point_indices) {
const auto& point = cloud->at(index);
const auto& normal = normals->at(index);
if (!pcl::isFinite(point)) continue;
getAlignmentToX(point, normal, &affine_s);
kdtree.radiusSearch(point, radius, indices, distances);
poses.push_back(getPose(index, indices, cloud, normals, affine_s));
}
sort(poses.begin(), poses.end(),
[](const Pose& a, const Pose& b) -> bool {
return a.votes > b.votes;
});
}
template<typename PointInT> std::size_t
pcl::nurbs::NurbsFitter<PointInT>::PCL2Eigen (PointCloudPtr &pcl_cloud, const std::vector<int> &indices, vector_vec3 &on_cloud)
{
std::size_t numPoints (0);
for (unsigned i = 0; i < indices.size (); i++)
{
const PointInT &pt = pcl_cloud->at (indices[i]);
if (!pcl_isnan (pt.x) && !pcl_isnan (pt.y) && !pcl_isnan (pt.z))
{
on_cloud.push_back (vec3 (pt.x, pt.y, pt.z));
++numPoints;
}
}
return (numPoints);
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" -n radius ...................................... Estimate surface normals\n");
pcl::console::print_info (" -k min_scale,nr_octaves,nr_scales,min_contrast... Detect keypoints\n");
pcl::console::print_info (" -l radius ....................................... Compute local descriptors\n");
pcl::console::print_info (" -s output_name (without .pcd extension).......... Save outputs\n");
pcl::console::print_info ("Note: \n");
pcl::console::print_info (" Each of the first four options depends on the options above it.\n");
pcl::console::print_info (" Saving (-s) will output individual files for each option used (-n,-k,-f,-g).\n");
return (1);
}
// Load the input file
PointCloudPtr cloud (new PointCloud);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], cloud->size ());
// Estimate surface normals
SurfaceNormalsPtr normals;
double surface_radius;
bool estimate_surface_normals = pcl::console::parse_argument (argc, argv, "-n", surface_radius) > 0;
if (estimate_surface_normals)
{
normals = estimateSurfaceNormals (cloud, surface_radius);
pcl::console::print_info ("Estimated surface normals\n");
}
// Detect keypoints
PointCloudPtr keypoints;
std::string params_string;
bool detect_keypoints = pcl::console::parse_argument (argc, argv, "-k", params_string) > 0;
if (detect_keypoints)
{
assert (normals);
std::vector<std::string> tokens;
boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
assert (tokens.size () == 4);
float min_scale = atof(tokens[0].c_str ());
int nr_octaves = atoi(tokens[1].c_str ());
int nr_scales = atoi(tokens[2].c_str ());
float min_contrast = atof(tokens[3].c_str ());
keypoints = detectKeypoints (cloud, normals, min_scale, nr_octaves, nr_scales, min_contrast);
pcl::console::print_info ("Detected %lu keypoints\n", keypoints->size ());
}
// Compute local descriptors
LocalDescriptorsPtr local_descriptors;
double feature_radius;
bool compute_local_descriptors = pcl::console::parse_argument (argc, argv, "-l", feature_radius) > 0;
if (compute_local_descriptors)
{
assert (normals && keypoints);
local_descriptors = computeLocalDescriptors (cloud, normals, keypoints, feature_radius);
pcl::console::print_info ("Computed local descriptors\n");
}
// Compute global descriptor
GlobalDescriptorsPtr global_descriptor;
if (normals)
{
global_descriptor = computeGlobalDescriptor (cloud, normals);
pcl::console::print_info ("Computed global descriptor\n");
}
// Save output
std::string base_filename, output_filename;
bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", base_filename) > 0;
if (save_cloud)
{
if (normals)
{
output_filename = base_filename;
output_filename.append ("_normals.pcd");
pcl::io::savePCDFile (output_filename, *normals);
pcl::console::print_info ("Saved surface normals as %s\n", output_filename.c_str ());
}
if (keypoints)
{
output_filename = base_filename;
output_filename.append ("_keypoints.pcd");
pcl::io::savePCDFile (output_filename, *keypoints);
pcl::console::print_info ("Saved keypoints as %s\n", output_filename.c_str ());
}
if (local_descriptors)
{
output_filename = base_filename;
output_filename.append ("_localdesc.pcd");
pcl::io::savePCDFile (output_filename, *local_descriptors);
pcl::console::print_info ("Saved local descriptors as %s\n", output_filename.c_str ());
}
if (global_descriptor)
{
output_filename = base_filename;
output_filename.append ("_globaldesc.pcd");
pcl::io::savePCDFile (output_filename, *global_descriptor);
pcl::console::print_info ("Saved global descriptor as %s\n", output_filename.c_str ());
}
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit\n");
pcl::visualization::PCLVisualizer vis;
pcl::visualization::PCLHistogramVisualizer hist_vis;
vis.addPointCloud (cloud);
if (normals)
{
vis.addPointCloudNormals<PointT,NormalT> (cloud, normals, 4, 0.02, "normals");
}
if (keypoints)
{
pcl::visualization::PointCloudColorHandlerCustom<PointT> red (keypoints, 255, 0, 0);
vis.addPointCloud (keypoints, red, "keypoints");
vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "keypoints");
}
if (global_descriptor)
{
hist_vis.addFeatureHistogram (*global_descriptor, 308, "Global descriptor");
}
vis.resetCamera ();
vis.spin ();
}
return (0);
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" -p dist_threshold,max_iters ..... Subtract the dominant plane\n");
pcl::console::print_info (" -c tolerance,min_size,max_size ... Cluster points\n");
pcl::console::print_info (" -s output.pcd .................... Save the largest cluster\n");
return (1);
}
// Load the input file
PointCloudPtr cloud (new PointCloud);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());
// Subtract the dominant plane
double dist_threshold, max_iters;
bool subtract_plane = pcl::console::parse_2x_arguments (argc, argv, "-p", dist_threshold, max_iters) > 0;
if (subtract_plane)
{
size_t n = cloud->size ();
cloud = findAndSubtractPlane (cloud, dist_threshold, (int)max_iters);
pcl::console::print_info ("Subtracted %zu points along the detected plane\n", n - cloud->size ());
}
// Cluster points
double tolerance, min_size, max_size;
std::vector<pcl::PointIndices> cluster_indices;
bool cluster_points = pcl::console::parse_3x_arguments (argc, argv, "-c", tolerance, min_size, max_size) > 0;
if (cluster_points)
{
clusterObjects (cloud, tolerance, (int)min_size, (int)max_size, cluster_indices);
pcl::console::print_info ("Found %zu clusters\n", cluster_indices.size ());
}
// Save output
std::string output_filename;
bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
if (save_cloud)
{
// If clustering was performed, save only the first (i.e., largest) cluster
if (cluster_points)
{
PointCloudPtr temp_cloud (new PointCloud);
pcl::copyPointCloud (*cloud, cluster_indices[0], *temp_cloud);
cloud = temp_cloud;
}
pcl::console::print_info ("Saving result as %s...\n", output_filename.c_str ());
pcl::io::savePCDFile (output_filename, *cloud);
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
pcl::visualization::PCLVisualizer vis;
// If clustering was performed, display each cluster with a random color
if (cluster_points)
{
for (size_t i = 0; i < cluster_indices.size (); ++i)
{
// Extract the i_th cluster into a new cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_i (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud (*cloud, cluster_indices[i], *cluster_i);
// Create a random color
pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_color (cluster_i);
// Create a unique identifier
std::stringstream cluster_id ("cluster");
cluster_id << i;
// Add the i_th cluster to the visualizer with a random color and a unique identifier
vis.addPointCloud<pcl::PointXYZ> (cluster_i, random_color, cluster_id.str ());
}
}
else
{
// If clustering wasn't performed, just display the cloud
vis.addPointCloud (cloud);
}
vis.resetCamera ();
vis.spin ();
}
return (0);
}
int
main (int argc, char ** argv)
{
if (argc < 3)
{
pcl::console::print_info ("Syntax is: %s query.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" --objects_root_path root_path ......... Root path to append to files in object_files.txt \n");
pcl::console::print_info (" --objects object_files.txt ......... A list of the files to use as exemplars\n");
pcl::console::print_info (" --filter parameters.txt ............ Threshold, downsample, and denoise\n");
pcl::console::print_info (" --segment parameters.txt .......... Remove dominant plane and cluster\n");
pcl::console::print_info (" --feature parameters.txt ........... Compute normals, keypoints, and descriptors\n");
pcl::console::print_info (" --registration parameters.txt ...... Align best fitting model to query\n");
pcl::console::print_info (" and where the parameters files must contain the following values (one per line):\n");
pcl::console::print_info (" filter parameters:\n");
pcl::console::print_info (" * min_depth\n");
pcl::console::print_info (" * max_depth\n");
pcl::console::print_info (" * downsample_leaf_size\n");
pcl::console::print_info (" * outlier_rejection_radius\n");
pcl::console::print_info (" * outlier_rejection_min_neighbors\n");
pcl::console::print_info (" segmentation parameters:\n");
pcl::console::print_info (" * plane_inlier_distance_threshold\n");
pcl::console::print_info (" * max_ransac_iterations\n");
pcl::console::print_info (" * cluster_tolerance\n");
pcl::console::print_info (" * min_cluster_size\n");
pcl::console::print_info (" * max_cluster_size\n");
pcl::console::print_info (" feature estimation parameters:\n");
pcl::console::print_info (" * surface_normal_radius\n");
pcl::console::print_info (" * keypoints_min_scale\n");
pcl::console::print_info (" * keypoints_nr_octaves\n");
pcl::console::print_info (" * keypoints_nr_scales_per_octave\n");
pcl::console::print_info (" * keypoints_min_contrast\n");
pcl::console::print_info (" * local_descriptor_radius\n");
pcl::console::print_info (" registration parameters:\n");
pcl::console::print_info (" * initial_alignment_min_sample_distance\n");
pcl::console::print_info (" * initial_alignment_max_correspondence_distance\n");
pcl::console::print_info (" * initial_alignment_nr_iterations\n");
pcl::console::print_info (" * icp_max_correspondence_distance\n");
pcl::console::print_info (" * icp_rejection_threshold\n");
pcl::console::print_info (" * icp_transformation_epsilon\n");
pcl::console::print_info (" * icp_max_iterations\n");
return (1);
}
// Load input file
PointCloudPtr query (new PointCloud);
pcl::io::loadPCDFile (argv[1], *query);
pcl::console::print_info ("Loaded %s (%lu points)\n", argv[1], query->size ());
ifstream input_stream;
ObjectRecognitionParameters params;
// Parse the exemplar files
std::string objects_root_path;
pcl::console::parse_argument (argc, argv, "--objects_root_path", objects_root_path);
std::string objects_file;
pcl::console::parse_argument (argc, argv, "--objects", objects_file);
std::vector<std::string> exemplar_filenames (0);
input_stream.open (objects_file.c_str ());
if (input_stream.is_open ())
{
while (input_stream.good ())
{
std::string filename;
input_stream >> filename;
if (filename.size () > 0)
{
exemplar_filenames.push_back (objects_root_path + "/" + filename);
}
}
input_stream.close ();
}
//Parse filter parameters
std::string filter_parameters_file;
pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;
input_stream.open (filter_parameters_file.c_str ());
if (input_stream.is_open())
{
input_stream >> params.min_depth;
input_stream >> params.max_depth;
input_stream >> params.downsample_leaf_size;
input_stream >> params.outlier_rejection_radius;
input_stream >> params.outlier_rejection_min_neighbors;
input_stream.close ();
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" -t min_depth,max_depth ............... Threshold depth\n");
pcl::console::print_info (" -d leaf_size ......................... Downsample\n");
pcl::console::print_info (" -r radius,min_neighbors ............... Radius outlier removal\n");
pcl::console::print_info (" -s output.pcd ......................... Save output\n");
return (1);
}
// Load the input file
PointCloudPtr cloud (new PointCloud);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());
// Threshold depth
double min_depth, max_depth;
bool threshold_depth = pcl::console::parse_2x_arguments (argc, argv, "-t", min_depth, max_depth) > 0;
if (threshold_depth)
{
size_t n = cloud->size ();
cloud = thresholdDepth (cloud, min_depth, max_depth);
pcl::console::print_info ("Eliminated %zu points outside depth limits\n", n - cloud->size ());
}
// Downsample and threshold depth
double leaf_size;
bool downsample_cloud = pcl::console::parse_argument (argc, argv, "-d", leaf_size) > 0;
if (downsample_cloud)
{
size_t n = cloud->size ();
cloud = downsample (cloud, leaf_size);
pcl::console::print_info ("Downsampled from %zu to %zu points\n", n, cloud->size ());
}
// Remove outliers
double radius, min_neighbors;
bool remove_outliers = pcl::console::parse_2x_arguments (argc, argv, "-r", radius, min_neighbors) > 0;
if (remove_outliers)
{
size_t n = cloud->size ();
cloud = removeOutliers (cloud, radius, (int)min_neighbors);
pcl::console::print_info ("Removed %zu outliers\n", n - cloud->size ());
}
// Save output
std::string output_filename;
bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
if (save_cloud)
{
pcl::io::savePCDFile (output_filename, *cloud);
pcl::console::print_info ("Saved result as %s\n", output_filename.c_str ());
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
pcl::visualization::PCLVisualizer vis;
vis.addPointCloud (cloud);
vis.resetCamera ();
vis.spin ();
}
return (0);
}
ppfmap::Pose ppfmap::CudaPPFMatch<PointT, NormalT>::getPose(
const int reference_index,
const std::vector<int>& indices,
const PointCloudPtr cloud,
const NormalsPtr normals,
const float affine_s[12]) {
Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tsg_map(affine_s);
float affine_m[12];
const std::size_t n = indices.size();
const auto& ref_point = cloud->at(reference_index);
const auto& ref_normal = normals->at(reference_index);
thrust::host_vector<uint32_t> hash_list(n);
thrust::host_vector<float> alpha_s_list(n);
// Compute the PPF feature for all the pairs in the neighborhood
for (int i = 0; i < n; i++) {
const int index = indices[i];
const auto& point = cloud->at(index);
const auto& normal = normals->at(index);
// Compute the PPF between reference_point and the i-th neighbor
hash_list[i] = computePPFFeatureHash(ref_point, ref_normal,
point, normal,
distance_step,
angle_step);
// Compute the alpha_s angle
const Eigen::Vector3f transformed(Tsg_map * point.getVector4fMap());
alpha_s_list[i] = atan2f(-transformed(2), transformed(1));
}
int index;
float alpha;
int votes;
map->searchBestMatch(hash_list, alpha_s_list, index, alpha, votes);
const auto& model_point = model_->at(index);
const auto& model_normal = normals_->at(index);
getAlignmentToX(model_point, model_normal, &affine_m);
Eigen::Map<const Eigen::Matrix<float, 3, 4, Eigen::RowMajor> > Tmg_map(affine_m);
Eigen::Affine3f Tsg, Tmg;
Tsg.matrix().block<3, 4>(0, 0) = Tsg_map.matrix();
Tmg.matrix().block<3, 4>(0, 0) = Tmg_map.matrix();
// Set final pose
Pose final_pose;
final_pose.c = pcl::Correspondence(reference_index, index, 0.0f);
final_pose.t = Tsg.inverse() * Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitX()) * Tmg;
final_pose.votes = votes;
return final_pose;
}
void Skeletonization3D::translate_points_to_inside(PointCloudPtr& cloud,
int cam_num) const {
for (unsigned int i = 0; i < cloud->size(); i++) {
cloud->set_z(i, cloud->get_z(i) + move_distance);
}
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" --surfel radius,order............... Compute surface elements\n");
pcl::console::print_info (" --convex ........................... Compute convex hull\n");
pcl::console::print_info (" --concave alpha .................... Compute concave hull\n");
pcl::console::print_info (" --greedy radius,max_nn,mu,surf_angle,min_angle,max_angle ... Compute greedy triangulation\n");
pcl::console::print_info (" -s output.pcd ...................... Save the output cloud\n");
return (1);
}
// Load the points
PointCloudPtr cloud (new PointCloud);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());
// Compute surface elements
SurfaceElementsPtr surfels (new SurfaceElements);
double mls_radius, polynomial_order;
bool compute_surface_elements =
pcl::console::parse_2x_arguments (argc, argv, "--surfel", mls_radius, polynomial_order) > 0;
if (compute_surface_elements)
{
surfels = computeSurfaceElements (cloud, mls_radius, polynomial_order);
pcl::console::print_info ("Computed surface elements\n");
}
// Find the convex hull
MeshPtr convex_hull;
bool compute_convex_hull = pcl::console::find_argument (argc, argv, "--convex") > 0;
if (compute_convex_hull)
{
convex_hull = computeConvexHull (cloud);
pcl::console::print_info ("Computed convex hull\n");
}
// Find the concave hull
MeshPtr concave_hull;
double alpha;
bool compute_concave_hull = pcl::console::parse_argument (argc, argv, "--concave", alpha) > 0;
if (compute_concave_hull)
{
concave_hull = computeConcaveHull (cloud, alpha);
pcl::console::print_info ("Computed concave hull\n");
}
// Compute a greedy surface triangulation
pcl::PolygonMesh::Ptr greedy_mesh;
std::string params_string;
bool perform_greedy_triangulation = pcl::console::parse_argument (argc, argv, "--greedy", params_string) > 0;
if (perform_greedy_triangulation)
{
assert (surfels);
std::vector<std::string> tokens;
boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
assert (tokens.size () == 6);
float radius = atof(tokens[0].c_str ());
int max_nearest_neighbors = atoi(tokens[1].c_str ());
float mu = atof(tokens[2].c_str ());
float max_surface_angle = atof(tokens[3].c_str ());
float min_angle = atof(tokens[4].c_str ());
float max_angle = atof(tokens[5].c_str ());
greedy_mesh = greedyTriangulation (surfels, radius, max_nearest_neighbors, mu,
max_surface_angle, min_angle, max_angle);
pcl::console::print_info ("Performed greedy surface triangulation\n");
}
// Compute a greedy surface triangulation
pcl::PolygonMesh::Ptr marching_cubes_mesh;
double leaf_size, iso_level;
bool perform_marching_cubes = pcl::console::parse_2x_arguments (argc, argv, "--mc", leaf_size, iso_level) > 0;
if (perform_marching_cubes)
{
assert (surfels);
marching_cubes_mesh = marchingCubesTriangulation (surfels, leaf_size, iso_level);
pcl::console::print_info ("Performed marching cubes surface triangulation\n");
}
// Save output
std::string output_filename;
bool save_output = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
if (save_output)
{
// Save the result
pcl::io::savePCDFile (output_filename, *cloud);
pcl::console::print_info ("Saved result as %s\n", output_filename.c_str ());
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
pcl::visualization::PCLVisualizer vis;
vis.addPointCloud (cloud);
vis.resetCamera ();
if (compute_convex_hull)
{
vis.addPolygonMesh<PointT> (convex_hull->points, convex_hull->faces, "convex_hull");
}
if (compute_concave_hull)
{
vis.addPolygonMesh<PointT> (concave_hull->points, concave_hull->faces, "concave_hull");
}
if (perform_greedy_triangulation)
{
vis.addPolygonMesh(*greedy_mesh, "greedy_mesh");
}
if (perform_marching_cubes)
{
vis.addPolygonMesh(*marching_cubes_mesh, "marching_cubes_mesh");
}
vis.spin ();
}
}
int
main (int argc, char ** argv)
{
if (argc < 3)
{
pcl::console::print_info ("Syntax is: %s input.pcd output <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" --filter parameters.txt ............ Threshold, downsample, and denoise\n");
pcl::console::print_info (" --segment parameters.txt .......... Remove dominant plane and cluster\n");
pcl::console::print_info (" --feature parameters.txt ........... Compute normals, keypoints, and descriptors\n");
pcl::console::print_info (" --registration parameters.txt ...... Align best fitting model to query\n");
pcl::console::print_info (" and where the parameters files must contain the following values (one per line):\n");
pcl::console::print_info (" filter parameters:\n");
pcl::console::print_info (" * min_depth\n");
pcl::console::print_info (" * max_depth\n");
pcl::console::print_info (" * downsample_leaf_size\n");
pcl::console::print_info (" * outlier_rejection_radius\n");
pcl::console::print_info (" * outlier_rejection_min_neighbors\n");
pcl::console::print_info (" segmentation parameters:\n");
pcl::console::print_info (" * plane_inlier_distance_threshold\n");
pcl::console::print_info (" * max_ransac_iterations\n");
pcl::console::print_info (" * cluster_tolerance\n");
pcl::console::print_info (" * min_cluster_size\n");
pcl::console::print_info (" * max_cluster_size\n");
pcl::console::print_info (" feature estimation parameters:\n");
pcl::console::print_info (" * surface_normal_radius\n");
pcl::console::print_info (" * keypoints_min_scale\n");
pcl::console::print_info (" * keypoints_nr_octaves\n");
pcl::console::print_info (" * keypoints_nr_scales_per_octave\n");
pcl::console::print_info (" * keypoints_min_contrast\n");
pcl::console::print_info (" * local_descriptor_radius\n");
pcl::console::print_info (" registration parameters:\n");
pcl::console::print_info (" * initial_alignment_min_sample_distance\n");
pcl::console::print_info (" * initial_alignment_max_correspondence_distance\n");
pcl::console::print_info (" * initial_alignment_nr_iterations\n");
pcl::console::print_info (" * icp_max_correspondence_distance\n");
pcl::console::print_info (" * icp_rejection_threshold\n");
pcl::console::print_info (" * icp_transformation_epsilon\n");
pcl::console::print_info (" * icp_max_iterations\n");
pcl::console::print_info ("Note: The output's base filename must be specified without the .pcd extension\n");
pcl::console::print_info (" Four output files will be created with the following suffixes:\n");
pcl::console::print_info (" * '_points.pcd'\n");
pcl::console::print_info (" * '_keypoints.pcd'\n");
pcl::console::print_info (" * '_localdesc.pcd'\n");
pcl::console::print_info (" * '_globaldesc.pcd'\n");
return (1);
}
// Load input file
PointCloudPtr input (new PointCloud);
pcl::io::loadPCDFile (argv[1], *input);
pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], input->size ());
ObjectRecognitionParameters params;
ifstream params_stream;
//Parse filter parameters
std::string filter_parameters_file;
pcl::console::parse_argument (argc, argv, "--filter", filter_parameters_file) > 0;
params_stream.open (filter_parameters_file.c_str ());
if (params_stream.is_open())
{
params_stream >> params.min_depth;
params_stream >> params.max_depth;
params_stream >> params.downsample_leaf_size;
params_stream >> params.outlier_rejection_radius;
params_stream >> params.outlier_rejection_min_neighbors;
params_stream.close ();
}
bool SQ_fitter_b<PointT>::minimize( const int &_type,
const PointCloudPtr &_cloud,
const SQ_parameters &_in,
SQ_parameters &_out,
double &_error ) {
// Parameters initially _in:
_out = _in;
// Set necessary parameters
int n = _cloud->points.size();
int m = 13;
double p[m]; // Parameters of SQ
double y[n]; // Values we want to achieve
double opts[LM_OPTS_SZ];
double info[LM_INFO_SZ];
opts[0] = LM_INIT_MU;
opts[1] = 1E-15;
opts[2] = 1E-15;
opts[3] = 1E-20;
opts[4] = LM_DIFF_DELTA;
struct levmar_data data;
data.x = new double[n];
data.y = new double[n];
data.z = new double[n];
data.num = n;
int i; int ret;
typename pcl::PointCloud<PointT>::iterator pit;
for( pit = _cloud->begin(), i = 0; pit != _cloud->end(); ++pit, ++i ) {
data.x[i] = (*pit).x;
data.y[i] = (*pit).y;
data.z[i] = (*pit).z;
}
// Set minimizer value to zero (could be 1, depending of what equation you are minimizing)
for( i = 0; i < n; ++i ) { y[i] = 0.0; }
// Initialize values for parameters p
for( i = 0; i < 3; ++i ) { p[i] = _in.dim[i]; }
for( i = 0; i < 2; ++i ) { p[i+3] = _in.e[i]; }
for( i = 0; i < 3; ++i ) { p[i+5] = _in.trans[i]; }
for( i = 0; i < 3; ++i ) { p[i+8] = _in.rot[i]; }
p[11] = _in.alpha;
p[12] = _in.k;
printf("Initial p: %f %f %f %f %f %f %f %f %f %f %f alpha: %f k: %f \n",
p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12] );
// Set limits
double ub[m], lb[m];
for( i = 0; i < 3; ++i ) { lb[i] = this->mLowerLim_dim[i]; ub[i] = this->mUpperLim_dim[i]; }
for( i = 0; i < 2; ++i ) { lb[i+3] = this->mLowerLim_e; ub[i+3] = this->mUpperLim_e; }
for( i = 0; i < 3; ++i ) { lb[i+5] = this->mLowerLim_trans[i]; ub[i+5] = this->mUpperLim_trans[i]; }
for( i = 0; i < 3; ++i ) { lb[i+8] = this->mLowerLim_rot[i]; ub[i+8] = this->mUpperLim_rot[i]; }
lb[11] = mLowerLim_alpha; ub[11] = mUpperLim_alpha;
lb[12] = mLowerLim_k; ub[12] = mUpperLim_k;
switch( _type ) {
case SQ_FX_RADIAL: {
ret = dlevmar_bc_der( fr_add_b,
Jr_add_b,
p, y, m, n,
lb, ub,
NULL,
5000,
opts, info,
NULL, NULL, (void*)&data );
} break;
case SQ_FX_ICHIM: {
ret = dlevmar_bc_der( fi_add_b,
Ji_add_b,
p, y, m, n,
lb, ub,
NULL,
1000,
opts, info,
NULL, NULL, (void*)&data );
} break;
case SQ_FX_SOLINA: {
ret = dlevmar_bc_der( fs_add_b,
Js_add_b,
p, y, m, n,
lb, ub,
NULL,
1000,
opts, info,
NULL, NULL, (void*)&data );
} break;
case SQ_FX_CHEVALIER: {
ret = dlevmar_bc_der( fc_add_b,
Jc_add_b,
p, y, m, n,
lb, ub,
NULL,
1000,
opts, info,
NULL, NULL, (void*)&data );
} break;
case SQ_FX_5: {
ret = dlevmar_bc_der( f5_add_b,
J5_add_b,
p, y, m, n,
lb, ub,
NULL,
1000,
opts, info,
NULL, NULL, (void*)&data );
} break;
case SQ_FX_6: {
ret = dlevmar_bc_der( f6_add_b,
J6_add_b,
p, y, m, n,
lb, ub,
NULL,
1000,
opts, info,
NULL, NULL, (void*)&data );
} break;
} // end switch
// Fill _out
for( i = 0; i < 3; ++i ) { _out.dim[i] = p[i]; }
for( i = 0; i < 2; ++i ) { _out.e[i] = p[i+3]; }
for( i = 0; i < 3; ++i ) { _out.trans[i] = p[i+5]; }
for( i = 0; i < 3; ++i ) { _out.rot[i] = p[i+8]; }
_out.alpha = p[11];
_out.k = p[12];
_out.type = BENT;
// Return status and error
double eg, er;
get_error( _out, this->cloud_, eg, er, _error );
// If stopped by invalid (TODO: Add other reasons)
if( info[6] == 7 ) {
return false;
} else {
return true;
}
}
//append a cloud to another cloud
void appendCloud(PointCloudPtr sourceCloud,PointCloudPtr targetCloud){
for(int i=0;i<sourceCloud->size();i++){
targetCloud->push_back(sourceCloud->at(i));
}
}
