Function main

tools/icp.cpp:57–123

using CloudPtr = Cloud::Ptr;

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");

  pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp;
  if (nonLinear)
  {
    std::cout << "Using IterativeClosestPointNonLinear" << std::endl;
    icp.reset (new pcl::IterativeClosestPointNonLinear<PointType, PointType> ());
  }
  else
  {
    std::cout << "Using IterativeClosestPoint" << std::endl;
    icp.reset (new pcl::IterativeClosestPoint<PointType, PointType> ());
  }
  icp->setMaximumIterations (iter);
  icp->setMaxCorrespondenceDistance (dist);
  icp->setRANSACOutlierRejectionThreshold (rans);

  pcl::registration::IncrementalRegistration<PointType> iicp;
  iicp.setRegistration (icp);

  for (const int &pcd_index : pcd_indices)
  {
    CloudPtr data (new Cloud);
    if (pcl::io::loadPCDFile (argv[pcd_index], *data) == -1)
    {
      std::cout << "Could not read file" << std::endl;
      return -1;
    }
    pcl::Indices dummy_indices;
    pcl::removeNaNFromPointCloud(*data, *data, dummy_indices);

    if (!iicp.registerCloud (data))
    {
      std::cout << "Registration failed. Resetting transform" << std::endl;
      iicp.reset ();
      iicp.registerCloud (data);
    };

    CloudPtr tmp (new Cloud);
    pcl::transformPointCloud (*data, *tmp, iicp.getAbsoluteTransform ());

    std::cout << iicp.getAbsoluteTransform () << std::endl;