From eba2a8162c2a3be30e9fda374c2ed0d53ed84609 Mon Sep 17 00:00:00 2001
From: David Seaward <dseaward925@gmail.com>
Date: Sat, 10 Oct 2015 11:39:17 +0200
Subject: [PATCH]  * calculate and store squared distance from final ICP
 iteration (calls matches.getDistsQuantile(1.0))  * make this value accessible
 from the icp object  * initialise to -1 (i.e. invalid before ICP is called) 
 * see https://github.com/ethz-asl/libpointmatcher/issues/125

---
 pointmatcher/ICP.cpp | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/pointmatcher/ICP.cpp b/pointmatcher/ICP.cpp
index c6eb1186..ba319765 100644
--- a/pointmatcher/ICP.cpp
+++ b/pointmatcher/ICP.cpp
@@ -64,7 +64,8 @@ InvalidModuleType::InvalidModuleType(const std::string& reason):
 template<typename T>
 PointMatcher<T>::ICPChainBase::ICPChainBase():
 	prefilteredReadingPtsCount(0),
-	prefilteredReferencePtsCount(0)
+	prefilteredReferencePtsCount(0),
+	finalSquaredDistance(-1)
 {}
 
 //! virtual desctructor
@@ -184,6 +185,14 @@ unsigned PointMatcher<T>::ICPChainBase::getPrefilteredReferencePtsCount() const
 	return prefilteredReferencePtsCount;
 }
 
+//! Return the squared distance between closest points
+// FIXME: rename and return the value and make it all floats (T)
+template<typename T>
+T PointMatcher<T>::ICPChainBase::getFinalSquaredDistance() const
+{
+	return finalSquaredDistance;
+}
+
 //! Instantiate modules if their names are in the YAML file
 template<typename T>
 template<typename R>
@@ -391,8 +400,13 @@ typename PointMatcher<T>::TransformationParameters PointMatcher<T>::ICP::compute
 		// in test
 		
 		this->transformationCheckers.check(T_iter, iterate);
-	
+
 		++iterationCount;
+
+		// store the final squared distance between closest points
+		// (the value that has been minimised by ICP)
+		if (!iterate)
+			finalSquaredDistance = matches.getDistsQuantile(1.0);
 	}
 	
 	this->inspector->addStat("IterationsCount", iterationCount);