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)
{
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;
}
bool
loopDetection (int end, const CloudVector &clouds, double dist, int &first, int &last)
{
static double min_dist = -1;
int state = 0;
for (int i = end-1; i > 0; i--)
{
Eigen::Vector4f cstart, cend;
//TODO use pose of scan
pcl::compute3DCentroid (*(clouds[i].second), cstart);
pcl::compute3DCentroid (*(clouds[end].second), cend);
Eigen::Vector4f diff = cend - cstart;
double norm = diff.norm ();
//std::cout << "distance between " << i << " and " << end << " is " << norm << " state is " << state << std::endl;
if (state == 0 && norm > dist)
{
state = 1;
//std::cout << "state 1" << std::endl;
}
if (state > 0 && norm < dist)
{
state = 2;
//std::cout << "loop detected between scan " << i << " (" << clouds[i].first << ") and scan " << end << " (" << clouds[end].first << ")" << std::endl;
if (min_dist < 0 || norm < min_dist)
{
min_dist = norm;
first = i;
last = end;
}
}
}
//std::cout << "min_dist: " << min_dist << " state: " << state << " first: " << first << " end: " << end << std::endl;
if (min_dist > 0 && (state < 2 || end == int (clouds.size ()) - 1)) //TODO
{
min_dist = -1;
return true;
}
return false;
}
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;
}