Removed some unnecessary code from Tracker

ganechean · May 7, 2014 · d25dccd · d25dccd
1 parent 556b619
commit d25dccd
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 7 deletions.
diff --git a/include/mcptam/Tracker.h b/include/mcptam/Tracker.h
@@ -421,7 +421,7 @@ class Tracker
   ros::Time mLastProcessTime;           ///< Time that the previous image processing step began
   ros::Duration mLastProcessDur;        ///< Time since the previous image processing step start
   TooN::Matrix<6>	mm6PoseCovariance;		///< covariance of current converged pose estimate
-  TooN::Matrix<6>	mm6PoseCovarianceNoOutliers;  ///< covariance of current pose estimate with outliers removed
+  //TooN::Matrix<6>	mm6PoseCovarianceNoOutliers;  ///< covariance of current pose estimate with outliers removed
   int mnTotalFound;                 ///< Number of features found by the tracker in the current frame
   int mnTotalAttempted;             ///< Number of features attempted to find by the tracker
   int mnNumInliers;                 ///< Number of measurement inliers for the tracking

diff --git a/launch/mcptam_client.launch b/launch/mcptam_client.launch
@@ -27,6 +27,7 @@
   <param name="mm_outlier_multiplier" type="double" value="1.1" />
 
   <param name="small_image_level" type="int" value="1" />
+  <param name="level_zero_points" type="bool" value="true" />
 
   <!-- For no GUI, set headless:=true -->
   <param name="headless" type="bool" value="$(arg headless)" />

diff --git a/src/Tracker.cc b/src/Tracker.cc
@@ -1436,9 +1436,9 @@ Vector<6> Tracker::CalcPoseUpdate(std::vector<TrackerDataPtrVector>& vIterationS
 
   // The TooN WLSCholesky class handles reweighted least squares.
   // It just needs errors and jacobians.
-  WLS<6> wls, wls_noweight;
+  WLS<6> wls; //, wls_noweight;
   wls.add_prior(100.0); // Stabilising prior
-  wls_noweight.add_prior(100);
+  //wls_noweight.add_prior(100);
   mnNumInliers = 0;
 
   for(unsigned i=0; i < mvCurrCamNames.size(); ++i)
@@ -1466,8 +1466,8 @@ Vector<6> Tracker::CalcPoseUpdate(std::vector<TrackerDataPtrVector>& vIterationS
       else 
         dWeight= Huber::Weight(dErrorSq, dSigmaSquared);
 
-      wls_noweight.add_mJ(v2Error[0], td.mdSqrtInvNoise * td.mm26Jacobian[0], 1); // These two lines are currently
-      wls_noweight.add_mJ(v2Error[1], td.mdSqrtInvNoise * td.mm26Jacobian[1], 1); // the slowest bit of poseits
+      //wls_noweight.add_mJ(v2Error[0], td.mdSqrtInvNoise * td.mm26Jacobian[0], 1); // These two lines are currently
+      //wls_noweight.add_mJ(v2Error[1], td.mdSqrtInvNoise * td.mm26Jacobian[1], 1); // the slowest bit of poseits
 
       // Inlier/outlier accounting, only really works for cut-off estimators such as Tukey.
       if(dWeight == 0.0)
@@ -1499,8 +1499,8 @@ Vector<6> Tracker::CalcPoseUpdate(std::vector<TrackerDataPtrVector>& vIterationS
   {
 		mm6PoseCovariance = TooN::SVD<6>(wls.get_C_inv()).get_pinv();   // from ethzasl_ptam
 
-    wls_noweight.compute();
-    mm6PoseCovarianceNoOutliers = TooN::SVD<6>(wls_noweight.get_C_inv()).get_pinv();   // from ethzasl_ptam
+    //wls_noweight.compute();
+    //mm6PoseCovarianceNoOutliers = TooN::SVD<6>(wls_noweight.get_C_inv()).get_pinv();   // from ethzasl_ptam
   }
 
   //std::cout<<"CalcPoseUpdate: "<<wls.get_mu()<<std::endl;