Ejemplo n.º 1
0
Archivo: elch.cpp Proyecto: Bardo91/pcl
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;
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
0
Archivo: elch.cpp Proyecto: Bardo91/pcl
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;
}
Ejemplo n.º 4
0
Archivo: lum.cpp Proyecto: 2php/pcl
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;
}