Apriltag: some improvements/changes

This MR has changes in:

• factor_autodiff_apriltag.h: The operator()was rewritten to remove some temporary variables and copies.
• gtest_factor_autodiff_apriltag.cpp: New test with non-zero values for extrinsics and frame pose. Also removed some wrong addConstrainedBy().
• processor_tracker_landmark_apriltag.cpp: After playing with the demo, the only change that remained is not providing features without orientation in detectNewFeatures() to avoid potential wrong frame and landmark initialization in future calls to reestimateLastFrame(). It could be implemented using a bool option in the processor parameters.
• processor_tracker.cpp a small bug fix also contained in !257 (merged).

TO DO: Ensure the proper frame coordinates for the error in FactorAutodiffApriltag::operator() to deal with covariance properly.

@mederic_fourmy, merge the current branch if you consider the contained changes are of your interest.

src/processor/processor_tracker_landmark_apriltag.cpp

@@ -762,33 +762,35 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
 
     // std::cout << "last_feature_list.size(): " << last_feature_list.size() << std::endl;
     // std::cout << "ori_feature_list.size(): " << ori_feature_list.size() << std::endl;
+    if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){
+        return;
+    }
     
     // Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence)
     Scalar lowest_ration = 1;  // rep_error1/rep_error2 cannot be higher than 1
     FeatureApriltagPtr best_feature;
     bool useable_feature = false;
-    if (last_feature_list.size() > 0 && ori_feature_list.size() > 0){
-        for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
-            FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
-            for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
-                FeatureApriltagPtr ori_feat_ptr =  std::static_pointer_cast<FeatureApriltag>(*it_ori);
-                if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
-                    Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2(); 
-                    //if (ratio < lowest_ration){
-                    if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
-                        useable_feature = true;
-                        lowest_ration = ratio;
-                        best_feature = last_feat_ptr;
-                        // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl;
-                    }
+    for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
+        FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
+        for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
+            FeatureApriltagPtr ori_feat_ptr =  std::static_pointer_cast<FeatureApriltag>(*it_ori);
+            if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
+                Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
+                //if (ratio < lowest_ration){
+                if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
+                    useable_feature = true;
+                    lowest_ration = ratio;
+                    best_feature = last_feat_ptr;
+                    // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl;
                 }
             }
         }
     }
     // If there is no common feature between the two images, the continuity is broken and 
     // nothing can be estimated. In the case of aprilslam alone, this result in a broken factor map
-    if (!useable_feature)
+    if (!useable_feature){
         return;
+    }
     
     // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
     // Retrieve cam to landmark transform
@@ -813,6 +815,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
             w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk;
         }
     }
+
     // Compute last frame estimate
     Eigen::Affine3ds w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();