From 9da16cd7024ad9a54799d59284ac049511a555ce Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Michael=20G=C3=B6rner?= <mgoerner@uos.de>
Date: Wed, 1 Apr 2015 21:18:16 +0200
Subject: [PATCH] make icp tool use IncrementalICP

This simplifies the code and gives a good example for using the class
---
 tools/icp.cpp | 63 +++++++++++++++++++--------------------------------
 1 file changed, 23 insertions(+), 40 deletions(-)

diff --git a/tools/icp.cpp b/tools/icp.cpp
index 9f7504b278d..3d00eb192a1 100644
--- a/tools/icp.cpp
+++ b/tools/icp.cpp
@@ -42,6 +42,7 @@
 #include <pcl/point_types.h>
 #include <pcl/registration/icp.h>
 #include <pcl/registration/icp_nl.h>
+#include <pcl/registration/incremental_icp.h>
 
 #include <string>
 #include <iostream>
@@ -71,22 +72,25 @@ main (int argc, char **argv)
   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)
+  pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp;
+  if (nonLinear)
   {
-    std::cout << "Could not read file" << std::endl;
-    return -1;
+    std::cout << "Using IterativeClosestPointNonLinear" << std::endl;
+    icp.reset (new pcl::IterativeClosestPointNonLinear<PointType, PointType> ());
   }
-  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;
+  else
+  {
+    std::cout << "Using IterativeClosestPoint" << std::endl;
+    icp.reset (new pcl::IterativeClosestPoint<PointType, PointType> ());
+  }
+  icp->setMaximumIterations (iter);
+  icp->setMaxCorrespondenceDistance (dist);
+  icp->setRANSACOutlierRejectionThreshold (rans);
 
-  Eigen::Matrix4f t (Eigen::Matrix4f::Identity ());
+  pcl::registration::IncrementalICP<PointType> iicp;
+  iicp.setICP (icp);
 
-  for (size_t i = 1; i < pcd_indices.size (); i++)
+  for (size_t i = 0; i < pcd_indices.size (); i++)
   {
     CloudPtr data (new Cloud);
     if (pcl::io::loadPCDFile (argv[pcd_indices[i]], *data) == -1)
@@ -94,39 +98,18 @@ main (int argc, char **argv)
       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)
+    if (!iicp.registerCloud (data))
     {
-      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);
+      std::cout << "Registration failed. Resetting transform" << std::endl;
+      iicp.reset ();
+      iicp.registerCloud (data);
+    };
 
     CloudPtr tmp (new Cloud);
-    icp->align (*tmp);
-
-    t = t * icp->getFinalTransformation ();
-
-    pcl::transformPointCloud (*data, *tmp, t);
-
-    std::cout << icp->getFinalTransformation () << std::endl;
+    pcl::transformPointCloud (*data, *tmp, iicp.getAbsoluteTransform ());
 
-    *model = *data;
+    std::cout << iicp.getAbsoluteTransform () << std::endl;
 
     std::string result_filename (argv[pcd_indices[i]]);
     result_filename = result_filename.substr (result_filename.rfind ("/") + 1);