Eigen::Matrix4f KinectFushionApp::initial_alignment(PointCloudRgbPtr src_points, PointCloudRgbPtr dst_points)
{ // Compute the intial alignment
PointCloudRgbPtr keypoints[2];
LocalDescriptorsPtr local_descriptors[2];
compute_features(src_points, keypoints[0], local_descriptors[0]);
compute_features(dst_points, keypoints[1], local_descriptors[1]);
//param
double min_sample_dist = 0.05, max_correspondence_dist=0.02, nr_iters=500;
// Find the transform that roughly aligns the points
Eigen::Matrix4f tform = computeInitialAlignment (keypoints[0], local_descriptors[0], keypoints[1], local_descriptors[1],
min_sample_dist, max_correspondence_dist, nr_iters);
PCL_INFO("Computed initial alignment\n");
return tform;
}
int
main (int argc, char ** argv)
{
if (argc < 3)
{
pcl::console::print_info ("Syntax is: %s source target <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" -i min_sample_dist,max_dist,nr_iters ................ Compute initial alignment\n");
pcl::console::print_info (" -r max_dist,rejection_thresh,tform_eps,max_iters ............. Refine alignment\n");
pcl::console::print_info (" -s output.pcd ........................... Save the registered and merged clouds\n");
pcl::console::print_info ("Note: The inputs (source and target) must be specified without the .pcd extension\n");
return (1);
}
// Load the points
PointCloudPtr src_points = loadPoints (argv[1]);
PointCloudPtr tgt_points = loadPoints (argv[2]);
Eigen::Matrix4f tform = Eigen::Matrix4f::Identity ();
// Compute the intial alignment
double min_sample_dist, max_correspondence_dist, nr_iters;
bool compute_intial_alignment =
pcl::console::parse_3x_arguments (argc, argv, "-i", min_sample_dist, max_correspondence_dist, nr_iters) > 0;
if (compute_intial_alignment)
{
// Load the keypoints and local descriptors
PointCloudPtr src_keypoints = loadKeypoints (argv[1]);
LocalDescriptorsPtr src_descriptors = loadLocalDescriptors (argv[1]);
PointCloudPtr tgt_keypoints = loadKeypoints (argv[2]);
LocalDescriptorsPtr tgt_descriptors = loadLocalDescriptors (argv[2]);
// Find the transform that roughly aligns the points
tform = computeInitialAlignment (src_keypoints, src_descriptors, tgt_keypoints, tgt_descriptors,
min_sample_dist, max_correspondence_dist, nr_iters);
pcl::console::print_info ("Computed initial alignment\n");
}
// Refine the initial alignment
std::string params_string;
bool refine_alignment = pcl::console::parse_argument (argc, argv, "-r", params_string) > 0;
if (refine_alignment)
{
std::vector<std::string> tokens;
boost::split (tokens, params_string, boost::is_any_of (","), boost::token_compress_on);
assert (tokens.size () == 4);
float max_correspondence_distance = atof(tokens[0].c_str ());
float outlier_rejection_threshold = atof(tokens[1].c_str ());
float transformation_epsilon = atoi(tokens[2].c_str ());
int max_iterations = atoi(tokens[3].c_str ());
tform = refineAlignment (src_points, tgt_points, tform, max_correspondence_distance,
outlier_rejection_threshold, transformation_epsilon, max_iterations);
pcl::console::print_info ("Refined alignment\n");
}
// Transform the source point to align them with the target points
pcl::transformPointCloud (*src_points, *src_points, tform);
// Save output
std::string filename;
bool save_output = pcl::console::parse_argument (argc, argv, "-s", filename) > 0;
if (save_output)
{
// Merge the two clouds
(*src_points) += (*tgt_points);
// Save the result
pcl::io::savePCDFile (filename, *src_points);
pcl::console::print_info ("Saved registered clouds as %s\n", 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::PointCloudColorHandlerCustom<PointT> red (src_points, 255, 0, 0);
vis.addPointCloud (src_points, red, "src_points");
pcl::visualization::PointCloudColorHandlerCustom<PointT> yellow (tgt_points, 255, 255, 0);
vis.addPointCloud (tgt_points, yellow, "tgt_points");
vis.resetCamera ();
vis.spin ();
}
return (0);
}
Eigen::Matrix4f twostepAlign (const PointCloud &src_points,
const PointCloud &tgt_points, const PointCloud &src_key,
const PointCloud &tgt_key, pcl::CorrespondencesPtr correspondences,
const Eigen::Matrix4f initial_matrix, PointCloud &output, bool initial,
double threshold)
{
Eigen::Matrix4f matrix1; // initial
Eigen::Matrix4f matrix2; //fine*initial
Eigen::Matrix4f matrix_final;
// smart pointer no need to delete it at the end
PointCloudPtr src (new PointCloud (src_points));
PointCloudPtr tgt (new PointCloud (tgt_points));
bool flag = 1;
// initial alignment
if (initial && flag == 1)
{
if (correspondences->size () < 3)
{
printf (
"The correspondence points are less than 3. The RANSAC based initial alignment will be swithched off. \n");
printf ("Please relax the threshold for SURF feature mtaching. \n");
flag = 0;
}
else
matrix1 = computeInitialAlignment (src_key, tgt_key, correspondences);
}
// Switch off the initial alignment
if (!initial || flag == 0)
{
matrix1 << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
}
PointCloudPtr points_transformed (new PointCloud);
pcl::transformPointCloud (*src, *points_transformed, matrix1);
// fine registration
if (FINE_FLAG)
{
matrix2 = fineAlign (points_transformed, tgt, output, threshold,initial) * matrix1; //pairwise alignment matrix
}
else // Switch off the fine alignment
{
matrix2 = matrix1;
output = *points_transformed;
}
// initial_matrix: the initial alignment refered to the first frame
// output is the transformed point cloud refered to the firs frame
matrix_final = initial_matrix * matrix2;
pcl::transformPointCloud (output, output, initial_matrix);
// print the transform for debugging
if (PRINT_TRANSFORM_PAIR)
{
cout << "Final Tranform: " << endl;
for (int k = 0; k < 3; k++)
{
for (int j = 0; j < 4; j++)
{
cout << matrix_final (k, j) << " ";
}
cout << endl;
}
}
// return the final ransfrom relative to the first frame
return matrix_final;
}
