New gtest for processor odom ICP

src/processor/processor_odom_icp.cpp

@@ -50,10 +50,10 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features)
     Eigen::Vector3s t_identity;
     t_identity << 0, 0, 0;
 
-    // XXX: Dynamic or static?
-    CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
-    CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_);
-    SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(this->getSensor());
+    // XXX: Dynamic or static? JS: static is OK.
+    CaptureLaser2DPtr   incoming_ptr    = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
+    CaptureLaser2DPtr   last_ptr        = std::static_pointer_cast<CaptureLaser2D>(last_ptr_);
+    SensorLaser2DPtr    laser_sen       = std::static_pointer_cast<SensorLaser2D >(this->getSensor());
 
     laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
 
@@ -133,26 +133,27 @@ void ProcessorOdomICP::resetDerived()
         // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
 
         // Rotation composition
-        Eigen::Rotation2D<Scalar> w_R_ro  = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
-        Eigen::Rotation2D<Scalar> r_R_s   = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
-        Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(origin_last_.res_transf(2));
+        Eigen::Rotation2D<Scalar>   w_R_ro  = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
+        Eigen::Rotation2D<Scalar>   r_R_s   = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
+        Eigen::Rotation2D<Scalar>& ro_R_so  = r_R_s;
+        Eigen::Rotation2D<Scalar>  so_R_sl  = Eigen::Rotation2D<Scalar>(origin_last_.res_transf(2));
 
         // Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*r_R_s*so_R_sl*r_R_s.inverse();
         // ro_R_so = rl_R_sl = r_R_s
         // Planar rotations are commutative
-        Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*so_R_sl;
+        Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro * so_R_sl;
 
         // Translation composition
-        Eigen::Vector2s w_p_w_ro = origin_ptr_->getFrame()->getState().head(2);
-        Eigen::Vector2s w_p_ro_so = w_R_ro*origin_ptr_->getSensorP()->getState();
-        Eigen::Vector2s w_p_so_sl = w_R_ro*r_R_s*origin_last_.res_transf.head(2);
-        Eigen::Vector2s w_p_sl_rl = w_R_rl*(-last_ptr_->getSensorP()->getState());
+        Eigen::Vector2s w_p_w_ro    = origin_ptr_->getFrame()->getState().head(2);
+        Eigen::Vector2s w_p_ro_so   = w_R_ro * origin_ptr_->getSensorP()->getState();
+        Eigen::Vector2s w_p_so_sl   = w_R_ro * ro_R_so * origin_last_.res_transf.head(2);
+        Eigen::Vector2s w_p_sl_rl   = w_R_rl * (-last_ptr_->getSensorP()->getState());
 
         Eigen::Vector2s w_p_w_rl = w_p_w_ro + w_p_ro_so + w_p_so_sl + w_p_sl_rl;
 
         Eigen::Vector3s curr_state;
-        curr_state.head(2) = w_p_w_rl;
-        curr_state(2) = origin_ptr_->getFrame()->getState()(2) + origin_last_.res_transf(2);
+        curr_state.head(2)  = w_p_w_rl;
+        curr_state(2)       = origin_ptr_->getFrame()->getState()(2) + origin_last_.res_transf(2);
 
         last_ptr_->getFrame()->setState(curr_state);
         std::cout << "[KEY FRAME DONE: ]" << '\n';

test/CMakeLists.txt

@@ -16,6 +16,9 @@ target_link_libraries(gtest_example ${PLUGIN_NAME})      #
 wolf_add_gtest(gtest_landmark_polyline gtest_landmark_polyline.cpp)
 target_link_libraries(gtest_landmark_polyline ${PLUGIN_NAME} ${wolf_LIBRARY})
 
+wolf_add_gtest(gtest_processor_odom_icp gtest_processor_odom_icp.cpp)
+target_link_libraries(gtest_processor_odom_icp ${PLUGIN_NAME} ${wolf_LIBRARY})
+
 wolf_add_gtest(gtest_polyline_2D gtest_polyline_2D.cpp)
 target_link_libraries(gtest_polyline_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
 

test/gtest_processor_odom_icp.cpp

+/**
+ * \file gtest_processor_odom_icp.cpp
+ *
+ *  Created on: Aug 6, 2019
+ *      \author: jsola
+ */
+
+#include "laser/processor/processor_odom_icp.h"
+
+#include "core/utils/utils_gtest.h"
+
+using namespace wolf;
+
+TEST(ProcessorParamsOdomICP, print)
+{
+    auto params = std::make_shared<ProcessorParamsOdomICP>();
+
+    ASSERT_TRUE(params); // not nullptr
+
+    params->use_corr_tricks = 15;
+
+    ASSERT_EQ(params->use_corr_tricks, 15);
+
+    WOLF_INFO("params: ", params->print());
+}
+
+TEST(ProcessorOdomIcp, Constructor)
+{
+    auto params = std::make_shared<ProcessorParamsOdomICP>();
+    auto prc    = std::make_shared<ProcessorOdomICP>(params);
+
+    ASSERT_TRUE(prc); // not nullptr
+    ASSERT_EQ(prc->getType(), "ODOM ICP");
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+//::testing::GTEST_FLAG(filter) = "TestGroup.DummyTestExample";
+  return RUN_ALL_TESTS();
+}
+