int
main (int argc, char **argv)
{
double dist = 0.1;
pcl::console::parse_argument (argc, argv, "-d", dist);
double rans = 0.1;
pcl::console::parse_argument (argc, argv, "-r", rans);
int iter = 100;
pcl::console::parse_argument (argc, argv, "-i", iter);
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (iter);
icp->setMaxCorrespondenceDistance (dist);
icp->setRANSACOutlierRejectionThreshold (rans);
elch.setReg (icp);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (const auto &cloud : clouds)
{
std::string result_filename (cloud.first);
result_filename = result_filename.substr (result_filename.rfind ('/') + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(cloud.second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
int
main (int argc, char **argv)
{
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (100);
icp->setMaxCorrespondenceDistance (0.1);
icp->setRANSACOutlierRejectionThreshold (0.1);
elch.setReg (icp);
CloudVector clouds;
for (int i = 1; i < argc; i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[i], *pc);
clouds.push_back (CloudPair (argv[i], pc));
std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i-1].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (size_t i = 0; i < clouds.size (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
int main (int argc, char **argv)
{
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sum_clouds (new pcl::PointCloud<pcl::PointXYZRGB>);
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
*sum_clouds += *pc;
}
std::string result_filename ("merged");
result_filename += getTimeAsString();
result_filename += ".pcd";
pcl::io::savePCDFileASCII (result_filename.c_str (), *sum_clouds);
std::cout << "saving result to " << result_filename << std::endl;
return 0;
}
int
main (int argc, char **argv)
{
double dist = 0.05;
pcl::console::parse_argument (argc, argv, "-d", dist);
double rans = 0.05;
pcl::console::parse_argument (argc, argv, "-r", rans);
int iter = 50;
pcl::console::parse_argument (argc, argv, "-i", iter);
bool nonLinear = false;
pcl::console::parse_argument (argc, argv, "-n", nonLinear);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudPtr model (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;
std::string result_filename (argv[pcd_indices[0]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFile (result_filename.c_str (), *model);
std::cout << "saving first model to " << result_filename << std::endl;
Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
for (size_t i = 1; i < pcd_indices.size (); i++)
{
CloudPtr data (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;
pcl::IterativeClosestPoint<PointType, PointType> *icp;
if (nonLinear)
{
std::cout << "Using IterativeClosestPointNonLinear" << std::endl;
icp = new pcl::IterativeClosestPointNonLinear<PointType, PointType>();
}
else
{
std::cout << "Using IterativeClosestPoint" << std::endl;
icp = new pcl::IterativeClosestPoint<PointType, PointType>();
}
icp->setMaximumIterations (iter);
icp->setMaxCorrespondenceDistance (dist);
icp->setRANSACOutlierRejectionThreshold (rans);
icp->setInputTarget (model);
icp->setInputSource (data);
CloudPtr tmp (new Cloud);
icp->align (*tmp);
t = icp->getFinalTransformation () * t;
pcl::transformPointCloud (*data, *tmp, t);
std::cout << icp->getFinalTransformation () << std::endl;
*model = *data;
std::string result_filename (argv[pcd_indices[i]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
int
main (int argc, char **argv)
{
double dist = 2.5;
pcl::console::parse_argument (argc, argv, "-d", dist);
int iter = 10;
pcl::console::parse_argument (argc, argv, "-i", iter);
int lumIter = 1;
pcl::console::parse_argument (argc, argv, "-l", lumIter);
double loopDist = 5.0;
pcl::console::parse_argument (argc, argv, "-D", loopDist);
int loopCount = 20;
pcl::console::parse_argument (argc, argv, "-c", loopCount);
pcl::registration::LUM<PointType> lum;
lum.setMaxIterations (lumIter);
lum.setConvergenceThreshold (0.001f);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
lum.addPointCloud (clouds[i].second);
}
for (int i = 0; i < iter; i++)
{
for (size_t i = 1; i < clouds.size (); i++)
for (size_t j = 0; j < i; j++)
{
Eigen::Vector4f ci, cj;
pcl::compute3DCentroid (*(clouds[i].second), ci);
pcl::compute3DCentroid (*(clouds[j].second), cj);
Eigen::Vector4f diff = ci - cj;
//std::cout << i << " " << j << " " << diff.norm () << std::endl;
if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount))
{
if(i - j > loopCount)
std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl;
pcl::registration::CorrespondenceEstimation<PointType, PointType> ce;
ce.setInputTarget (clouds[i].second);
ce.setInputSource (clouds[j].second);
pcl::CorrespondencesPtr corr (new pcl::Correspondences);
ce.determineCorrespondences (*corr, dist);
if (corr->size () > 2)
lum.setCorrespondences (j, i, corr);
}
}
lum.compute ();
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
//std::cout << i << ": " << lum.getTransformation (i) (0, 3) << " " << lum.getTransformation (i) (1, 3) << " " << lum.getTransformation (i) (2, 3) << std::endl;
clouds[i].second = lum.getTransformedCloud (i);
}
}
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
//std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
int
main (int argc, char **argv)
{
int iter = 35;
pcl::console::parse_argument (argc, argv, "-i", iter);
float ndt_res = 1.0f;
pcl::console::parse_argument (argc, argv, "-r", ndt_res);
double step_size = 0.1;
pcl::console::parse_argument (argc, argv, "-s", step_size);
double trans_eps = 0.01;
pcl::console::parse_argument (argc, argv, "-t", trans_eps);
float filter_res = 0.2f;
pcl::console::parse_argument (argc, argv, "-f", filter_res);
bool display_help = false;
pcl::console::parse_argument (argc, argv, "--help", display_help);
if (display_help || argc <= 1)
{
std::cout << "Usage: ndt3d [OPTION]... [FILE]..." << std::endl;
std::cout << "Registers PCD files using 3D Normal Distributions Transform algorithm" << std::endl << std::endl;
std::cout << " -i maximum number of iterations" << std::endl;
std::cout << " -r resolution (in meters) of NDT grid" << std::endl;
std::cout << " -s maximum step size (in meters) of newton optimizer" << std::endl;
std::cout << " -t transformation epsilon used for termination condition" << std::endl;
std::cout << " -f voxel filter resolution (in meters) used on source cloud" << std::endl;
std::cout << " --help display this help and exit" << std::endl;
return (0);
}
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudPtr model (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;
std::string result_filename (argv[pcd_indices[0]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFile (result_filename.c_str (), *model);
std::cout << "saving first model to " << result_filename << std::endl;
Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
pcl::ApproximateVoxelGrid<PointType> voxel_filter;
voxel_filter.setLeafSize (filter_res, filter_res, filter_res);
for (size_t i = 1; i < pcd_indices.size (); i++)
{
CloudPtr data (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;
pcl::NormalDistributionsTransform<PointType, PointType> * ndt = new pcl::NormalDistributionsTransform<PointType, PointType>();
ndt->setMaximumIterations (iter);
ndt->setResolution (ndt_res);
ndt->setStepSize (step_size);
ndt->setTransformationEpsilon (trans_eps);
ndt->setInputTarget (model);
CloudPtr filtered_data (new Cloud);
voxel_filter.setInputCloud (data);
voxel_filter.filter (*filtered_data);
ndt->setInputSource (filtered_data);
CloudPtr tmp (new Cloud);
ndt->align (*tmp);
t = ndt->getFinalTransformation () * t;
pcl::transformPointCloud (*data, *tmp, t);
std::cout << ndt->getFinalTransformation () << std::endl;
*model = *data;
std::string result_filename (argv[pcd_indices[i]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
std::cout << "saving result to " << result_filename << std::endl;
}
return (0);
}
int
main (int argc, char **argv)
{
int iter = 10;
double grid_step = 3.0;
double grid_extent = 25.0;
double optim_step = 1.0;
pcl::console::parse_argument (argc, argv, "-i", iter);
pcl::console::parse_argument (argc, argv, "-g", grid_step);
pcl::console::parse_argument (argc, argv, "-e", grid_extent);
pcl::console::parse_argument (argc, argv, "-s", optim_step);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudPtr model (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;
std::string result_filename (argv[pcd_indices[0]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
try
{
pcl::io::savePCDFile (result_filename.c_str (), *model);
std::cout << "saving first model to " << result_filename << std::endl;
}
catch(pcl::IOException& e)
{
std::cerr << e.what() << std::endl;
}
Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
for (size_t i = 1; i < pcd_indices.size (); i++)
{
CloudPtr data (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
pcl::NormalDistributionsTransform2D<PointType, PointType> ndt;
ndt.setMaximumIterations (iter);
ndt.setGridCentre (Eigen::Vector2f (15,0));
ndt.setGridExtent (Eigen::Vector2f (grid_extent,grid_extent));
ndt.setGridStep (Eigen::Vector2f (grid_step,grid_step));
ndt.setOptimizationStepSize (optim_step);
ndt.setTransformationEpsilon (1e-5);
ndt.setInputTarget (model);
ndt.setInputCloud (data);
CloudPtr tmp (new Cloud);
ndt.align (*tmp);
t = ndt.getFinalTransformation () * t;
pcl::transformPointCloud (*data, *tmp, t);
std::cout << ndt.getFinalTransformation () << std::endl;
*model = *data;
try
{
std::string result_filename (argv[pcd_indices[i]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
std::cout << "saving result to " << result_filename << std::endl;
}
catch(pcl::IOException& e)
{
std::cerr << e.what() << std::endl;
}
}
return 0;
}
int
main (int argc, char **argv)
{
double dist = 0.05;
pcl::console::parse_argument (argc, argv, "-d", dist);
double rans = 0.05;
pcl::console::parse_argument (argc, argv, "-r", rans);
int iter = 50;
pcl::console::parse_argument (argc, argv, "-i", iter);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudPtr model (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[0]], *model) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[0]] << " width: " << model->width << " height: " << model->height << std::endl;
std::string result_filename (argv[pcd_indices[0]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFile (result_filename.c_str (), *model);
std::cout << "saving first model to " << result_filename << std::endl;
Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
for (size_t i = 1; i < pcd_indices.size (); i++)
{
CloudPtr data (new Cloud);
if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
{
std::cout << "Could not read file" << std::endl;
return -1;
}
std::cout << argv[pcd_indices[i]] << " width: " << data->width << " height: " << data->height << std::endl;
pcl::IterativeClosestPointNonLinear<PointType, PointType> icp;
boost::shared_ptr<pcl::registration::WarpPointRigid3D<PointType, PointType> > warp_fcn
(new pcl::registration::WarpPointRigid3D<PointType, PointType>);
// Create a TransformationEstimationLM object, and set the warp to it
boost::shared_ptr<pcl::registration::TransformationEstimationLM<PointType, PointType> > te (new pcl::registration::TransformationEstimationLM<PointType, PointType>);
te->setWarpFunction (warp_fcn);
// Pass the TransformationEstimation objec to the ICP algorithm
icp.setTransformationEstimation (te);
icp.setMaximumIterations (iter);
icp.setMaxCorrespondenceDistance (dist);
icp.setRANSACOutlierRejectionThreshold (rans);
icp.setInputTarget (model);
icp.setInputCloud (data);
CloudPtr tmp (new Cloud);
icp.align (*tmp);
t = icp.getFinalTransformation () * t;
pcl::transformPointCloud (*data, *tmp, t);
std::cout << icp.getFinalTransformation () << std::endl;
*model = *data;
std::string result_filename (argv[pcd_indices[i]]);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *tmp);
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
