Recommit

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