From a6999ba27dedf222934e7940c202a7f307f14e2c Mon Sep 17 00:00:00 2001
From: mtlazaro <maritelag@gmail.com>
Date: Tue, 4 Aug 2015 18:09:19 +0200
Subject: [PATCH] Tracker now accepts an initial_guess

---
 src/tsm_core/tracker.cpp | 14 +++++++++-----
 src/tsm_core/tracker.h   |  2 +-
 2 files changed, 10 insertions(+), 6 deletions(-)

diff --git a/src/tsm_core/tracker.cpp b/src/tsm_core/tracker.cpp
index bc7a480..7cd47d6 100644
--- a/src/tsm_core/tracker.cpp
+++ b/src/tsm_core/tracker.cpp
@@ -1,6 +1,11 @@
+#include <cstdio>
+#include <fstream>
+
 #include "tracker.h"
 
 namespace tsm {
+  using namespace std;
+
   inline double tv2sec(struct timeval& tv) {
     return (double) tv.tv_sec + 1e-6 * (double) tv.tv_usec;
   }
@@ -27,7 +32,7 @@ namespace tsm {
     _correspondence_finder = 0;
     _inlier_distance = 0.1;
     _min_correspondences_ratio = 0.5;
-    _local_map_clipping_range = 10;
+    _local_map_clipping_range = 5;
     _clip_translation_threshold = 5;
   }
 
@@ -42,7 +47,7 @@ namespace tsm {
   }
 
 
-  void Tracker::update(Cloud2D* cloud_) {
+  void Tracker::update(Cloud2D* cloud_, const Eigen::Isometry2f& initial_guess) {
     if (cloud_)
       setCurrent(cloud_);
     double init = getTime();
@@ -65,10 +70,9 @@ namespace tsm {
       throw std::runtime_error("Cannot track without a projector and a solver");
     }
 
-    Eigen::Isometry2f global_t_inverse = _global_t.inverse();
     _solver->setReference(_reference);
     _solver->setCurrent(_current);
-    _solver->setT(Eigen::Isometry2f::Identity());
+    _solver->setT(initial_guess);
     _correspondence_finder.init();
     _solver->setReferencePointsHint(_correspondence_finder.indicesReference());
 
@@ -104,6 +108,7 @@ namespace tsm {
 	  ++outliers;
       }
     }
+
     float correspondences_ratio  = (float)num_correspondences/(float)_current->size();
     if (correspondences_ratio < _min_correspondences_ratio) {
       if(_current && _current != _reference) {
@@ -132,7 +137,6 @@ namespace tsm {
       //_reference->voxelize(*_reference, 2);
       _reference->transformInPlace(_solver->T().inverse());
 
-
       if (_current && _reference != _current) {
 	delete _current;
 	_current = 0;
diff --git a/src/tsm_core/tracker.h b/src/tsm_core/tracker.h
index 650b9b8..aee94e6 100644
--- a/src/tsm_core/tracker.h
+++ b/src/tsm_core/tracker.h
@@ -16,7 +16,7 @@ namespace tsm {
   public:
     Tracker();
     virtual ~Tracker();
-    void update(Cloud2D* cloud=0);
+    void update(Cloud2D* cloud=0, const Eigen::Isometry2f& initial_guess=Eigen::Isometry2f::Identity());
     inline void reset();
 
     // setter & getter