new gtest passing and removed debug print

src/processor/processor_motion.cpp

@@ -65,7 +65,6 @@ void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
     cap_target->getBuffer().insert(cap_target->getBuffer().begin(),
                                    cap_prev->getBuffer().begin(),
                                    cap_prev->getBuffer().end());
-    cap_target->getBuffer().print();
 
     // change origin
     cap_target->setOriginCapture(cap_prev->getOriginCapture());

src/tree_manager/tree_manager_sliding_window_dual_rate.cpp

@@ -25,6 +25,7 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
     // REMOVE FIRST RECENT FRAME: all recent frames except one of each rate_old_frames
     if (count_frames_ != 0)
     {
+        WOLF_DEBUG("TreeManagerSlidingWindow removing the oldest of recent frames");
         FrameBasePtr remove_recent_frame    = std::next(getProblem()->getTrajectory()->rbegin(),
                                                         params_swdr_->n_frames_recent)->second;
         FrameBasePtr keep_recent_frame      = std::next(getProblem()->getTrajectory()->rbegin(),
@@ -43,9 +44,11 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _key_frame)
             // merge captures (if exist)
             if (cap_prev and cap_next)
             {
+                WOLF_DEBUG("TreeManagerSlidingWindow merging capture ", cap_prev->id(), " with ", cap_next->id());
                 assert(cap_next->getOriginCapture() == cap_prev);
                 proc_motion->mergeCaptures(cap_prev, cap_next);
             }
+
         }
 
         // remove frame

test/CMakeLists.txt

@@ -267,6 +267,10 @@ ENDIF(Ceres_FOUND)
 wolf_add_gtest(gtest_tree_manager_sliding_window gtest_tree_manager_sliding_window.cpp)
 target_link_libraries(gtest_tree_manager_sliding_window ${PLUGIN_NAME})
 
+# TreeManagerSlidingWindowDualRate class test
+wolf_add_gtest(gtest_tree_manager_sliding_window_dual_rate gtest_tree_manager_sliding_window_dual_rate.cpp)
+target_link_libraries(gtest_tree_manager_sliding_window_dual_rate ${PLUGIN_NAME})
+
 # yaml conversions
 wolf_add_gtest(gtest_yaml_conversions gtest_yaml_conversions.cpp)
 target_link_libraries(gtest_yaml_conversions ${PLUGIN_NAME})

test/gtest_tree_manager_sliding_window.cpp

@@ -133,9 +133,9 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
 
     // Checks
     EXPECT_TRUE(F1->isRemoving());
-    EXPECT_TRUE(c12->isRemoving());
-    EXPECT_TRUE(C12->isRemoving()); //Virally removed
-    EXPECT_TRUE(f12->isRemoving()); //Virally removed
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C12->isRemoving()); // Virally removed
+    EXPECT_TRUE(f12->isRemoving()); // Virally removed
     EXPECT_TRUE(F2->isFixed()); //Fixed
 
     // FRAME 5 ----------------------------------------------------------
@@ -153,16 +153,17 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
 
     // Checks
     EXPECT_TRUE(F1->isRemoving());
-    EXPECT_TRUE(c12->isRemoving());
-    EXPECT_TRUE(C12->isRemoving()); //Virally removed
-    EXPECT_TRUE(f12->isRemoving()); //Virally removed
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C12->isRemoving()); // Virally removed
+    EXPECT_TRUE(f12->isRemoving()); // Virally removed
     EXPECT_TRUE(F2->isRemoving());
-    EXPECT_TRUE(c2->isRemoving());
-    EXPECT_TRUE(C2->isRemoving()); //Virally removed
-    EXPECT_TRUE(f2->isRemoving()); //Virally removed
-    EXPECT_TRUE(c23->isRemoving());
-    EXPECT_TRUE(C23->isRemoving()); //Virally removed
-    EXPECT_TRUE(f23->isRemoving()); //Virally removed
+    EXPECT_TRUE(c2->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C2->isRemoving()); // Virally removed
+    EXPECT_TRUE(f2->isRemoving()); // Virally removed
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
+    EXPECT_TRUE(C23->isRemoving()); // Virally removed
+    EXPECT_TRUE(f23->isRemoving()); // Virally removed
+
     EXPECT_TRUE(F3->isFixed()); //Fixed
 }
 
@@ -229,7 +230,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
 
     // Checks
     EXPECT_TRUE(F1->isRemoving());
-    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
     EXPECT_FALSE(C12->isRemoving()); //Not virally removed
     EXPECT_FALSE(f12->isRemoving()); //Not virally removed
     EXPECT_FALSE(F2->isFixed()); //Not fixed
@@ -249,14 +250,14 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
 
     // Checks
     EXPECT_TRUE(F1->isRemoving());
-    EXPECT_TRUE(c12->isRemoving());
+    EXPECT_TRUE(c12->isRemoving()); // Factor removed because involves removed frame
     EXPECT_TRUE(C12->isRemoving());
     EXPECT_TRUE(f12->isRemoving());
     EXPECT_TRUE(F2->isRemoving());
     EXPECT_TRUE(c2->isRemoving());
     EXPECT_TRUE(C2->isRemoving());
     EXPECT_TRUE(f2->isRemoving());
-    EXPECT_TRUE(c23->isRemoving());
+    EXPECT_TRUE(c23->isRemoving()); // Factor removed because involves removed frame
     EXPECT_FALSE(C23->isRemoving()); //Not virally removed
     EXPECT_FALSE(f23->isRemoving()); //Not virally removed
     EXPECT_FALSE(F3->isFixed()); //Not fixed

test/yaml/params_tree_manager_sliding_window1.yaml

@@ -4,8 +4,6 @@ config:
     dimension: 3
     prior:
       mode: "factor"
-      # state: [0,0,0,0,0,0,1]
-      # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
       $state:
         P: [0,0,0]
         O: [0,0,0,1]

test/yaml/params_tree_manager_sliding_window2.yaml

@@ -4,8 +4,6 @@ config:
     dimension: 3
     prior:
       mode: "factor"
-      # state: [0,0,0,0,0,0,1]
-      # cov: [[6,6],.1,0,0,0,0,0, 0,.1,0,0,0,0, 0,0,.1,0,0,0, 0,0,0,.1,0,0, 0,0,0,0,.1,0, 0,0,0,0,0,.1]
       $state:
         P: [0,0,0]
         O: [0,0,0,1]

test/yaml/params_tree_manager_sliding_window_dual_rate1.yaml

+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager:
+      type: "TreeManagerSlidingWindowDualRate"
+      n_frames: 5
+      n_frames_recent: 3
+      rate_old_frames: 2
+      fix_first_frame: true
+      viral_remove_empty_parent: true

test/yaml/params_tree_manager_sliding_window_dual_rate2.yaml

+config:
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager:
+      type: "TreeManagerSlidingWindowDualRate"
+      n_frames: 5
+      n_frames_recent: 3
+      rate_old_frames: 2
+      fix_first_frame: false
+      viral_remove_empty_parent: false

test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml

+config:
+  solver:
+    period: 1
+    verbose: 2
+    update_immediately: false
+    max_num_iterations: 10
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager:
+      type: "TreeManagerSlidingWindowDualRate"
+      n_frames: 5
+      n_frames_recent: 3
+      rate_old_frames: 2
+      fix_first_frame: true
+      viral_remove_empty_parent: true
+  sensors: 
+    -
+      type: "SensorOdom3d"
+      name: "odom"
+      plugin: "core"
+      k_disp_to_disp: 0.1
+      k_disp_to_rot: 0.1
+      k_rot_to_rot: 0.1 
+      min_disp_var: 0.1 
+      min_rot_var: 0.1
+      extrinsic:
+        pose: [1,2,3,0,0,0,1]
+  processors:
+    -
+      type: "ProcessorOdom3d"
+      name: "my_proc_odom3d"
+      sensor_name: "odom"
+      plugin: "core"
+      apply_loss_function: false
+      time_tolerance:         0.01  # seconds
+      keyframe_vote:
+        voting_active:        true
+        voting_aux_active:    false
+        max_time_span:          0.2   # seconds
+        max_buff_length:        10    # motion deltas
+        dist_traveled:          10   # meters
+        angle_turned:           3.1   # radians (1 rad approx 57 deg, approx 60 deg)
+      
+      unmeasured_perturbation_std: 0.00111

test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml

+config:
+  solver:
+    period: 1
+    verbose: 2
+    update_immediately: false
+    max_num_iterations: 10
+  problem:
+    frame_structure: "PO"
+    dimension: 3
+    prior:
+      mode: "factor"
+      $state:
+        P: [0,0,0]
+        O: [0,0,0,1]
+      $sigma:
+        P: [0.31, 0.31, 0.31]
+        O: [0.31, 0.31, 0.31]
+      time_tolerance: 0.1
+    tree_manager: 
+      type: "None"
+  sensors: 
+    -
+      type: "SensorOdom3d"
+      name: "odom"
+      plugin: "core"
+      k_disp_to_disp: 0.1
+      k_disp_to_rot: 0.1
+      k_rot_to_rot: 0.1 
+      min_disp_var: 0.1 
+      min_rot_var: 0.1
+      extrinsic:
+        pose: [1,2,3,0,0,0,1]
+  processors:
+    -
+      type: "ProcessorOdom3d"
+      name: "my_proc_odom3d"
+      sensor_name: "odom"
+      plugin: "core"
+      apply_loss_function: false
+      time_tolerance:         0.01  # seconds
+      keyframe_vote:
+        voting_active:        true
+        voting_aux_active:    false
+        max_time_span:          0.2   # seconds
+        max_buff_length:        10    # motion deltas
+        dist_traveled:          10   # meters
+        angle_turned:           3.1   # radians (1 rad approx 57 deg, approx 60 deg)
+      
+      unmeasured_perturbation_std: 0.00111