From fc8c3df931063ceefa7d04edbbabedb5c8301e54 Mon Sep 17 00:00:00 2001
From: Ji Zhang <zhangji@cmu.edu>
Date: Sat, 23 Aug 2014 20:19:29 -0400
Subject: [PATCH] Recommit

---
 src/visualOdometry.cpp | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/visualOdometry.cpp b/src/visualOdometry.cpp
index 6f3c639..40484e1 100644
--- a/src/visualOdometry.cpp
+++ b/src/visualOdometry.cpp
@@ -362,7 +362,7 @@ void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2)
     }
   }
 
-  int iterNum = 50;
+  int iterNum = 100;
   pcl::PointXYZHSV ipr2, ipr3, ipr4;
   int ipRelationsNum = ipRelations->points.size();
   int ptNumNoDepthRec = 0;
@@ -512,7 +512,7 @@ void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2)
         matA.at<float>(i, 3) = ipr2.h;
         matA.at<float>(i, 4) = ipr2.s;
         matA.at<float>(i, 5) = ipr2.v;
-        matB.at<float>(i, 0) = -0.1 * ipy2[i];
+        matB.at<float>(i, 0) = -0.2 * ipy2[i];
       }
       cv::transpose(matA, matAt);
       matAtA = matAt * matA;
@@ -528,6 +528,19 @@ void imagePointsHandler(const sensor_msgs::PointCloud2ConstPtr& imagePoints2)
         transform[4] += matX.at<float>(4, 0);
         transform[5] += matX.at<float>(5, 0);
       //}
+
+      float deltaR = sqrt(matX.at<float>(0, 0) * 180 / PI * matX.at<float>(0, 0) * 180 / PI
+                   + matX.at<float>(1, 0) * 180 / PI * matX.at<float>(1, 0) * 180 / PI
+                   + matX.at<float>(2, 0) * 180 / PI * matX.at<float>(2, 0) * 180 / PI);
+      float deltaT = sqrt(matX.at<float>(3, 0) * 100 * matX.at<float>(3, 0) * 100
+                   + matX.at<float>(4, 0) * 100 * matX.at<float>(4, 0) * 100
+                   + matX.at<float>(5, 0) * 100 * matX.at<float>(5, 0) * 100);
+
+      if (deltaR < 0.00001 && deltaT < 0.00001) {
+        break;
+      }
+
+      //ROS_INFO ("iter: %d, deltaR: %f, deltaT: %f", iterCount, deltaR, deltaT);
     }
   }