diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h index 388e3aa4eb3167b5064a456185c1bd1e030f6a2b..4936a1a7fd54e862b125645b397a5afded0761a7 100644 --- a/include/vision/processor/processor_visual_odometry.h +++ b/include/vision/processor/processor_visual_odometry.h @@ -240,7 +240,7 @@ class ProcessorVisualOdometry : public ProcessorTracker, public MotionProvider { public: ProcessorVisualOdometry(const YAML::Node& _params); - virtual ~ProcessorVisualOdometry() override {}; + virtual ~ProcessorVisualOdometry() override{}; TimeStamp getTimeStamp() const override; VectorComposite getState(StateKeys _structure = "") const override; diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index c63f98259cdffa53a9e0ab8108f9e3db350c1c4a..3e1f2de78264c3d8893bdd20f7c7ddedb85e70bc 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -386,7 +386,8 @@ void ProcessorVisualOdometry::establishFactors() // std::cout << "NEW valid track \\o/" << std::endl; auto lmk_ptr = emplaceLandmarkTriangulation(feat_pi, track_kf); - if (not lmk_ptr) lmk_ptr = emplaceLandmarkNaive(feat_pi, params_visual_odometry_.lmk_creation.distance_init); + if (not lmk_ptr) + lmk_ptr = emplaceLandmarkNaive(feat_pi, params_visual_odometry_.lmk_creation.distance_init); // Associate all features of the track to the landmark for (auto feat_track : track_matrix_.track(feat->trackId())) @@ -848,8 +849,7 @@ bool ProcessorVisualOdometry::tryTriangulationFromFeatures(FeaturePointImagePtr // Normalization necessary since homogeneous point stateblock is supposed to be unitary pt_w.normalize(); - if (pt_w(3) < 0) - pt_w *= -1; + if (pt_w(3) < 0) pt_w *= -1; // Cheriality check // transform to camera frame diff --git a/test/gtest_processor_visual_odometry.cpp b/test/gtest_processor_visual_odometry.cpp index d82e50bfaf66fd41e69312f861d99f529cec278a..61f4c2af5b4ad4b02d46aeaecaa6d284222c07e2 100644 --- a/test/gtest_processor_visual_odometry.cpp +++ b/test/gtest_processor_visual_odometry.cpp @@ -436,8 +436,7 @@ class ProcessorVOMultiView_class : public testing::Test X_c1.push_back((Eigen::Vector4d() << 0.75, 0.75, 2.0, 1.0).finished()); // Keep 3D points in the world frame - for(auto i = 0; i < X_c1.size(); i++) - X_w.push_back(X_c1.at(i)); + for (auto i = 0; i < X_c1.size(); i++) X_w.push_back(X_c1.at(i)); // Project pixels in the first view auto cam_dist_vec = camera->getDistortionVector(); @@ -446,7 +445,7 @@ class ProcessorVOMultiView_class : public testing::Test mwkps_c1.emplace(i, WKeyPoint(pinhole::projectPoint(k, cam_dist_vec, X_c1.at(i).head<3>()))); // Set the transformation between views 1 and 2 - t_12 = Vector3d(0.2, 0.1, 0.0); // t_12 = Vector3d(2, 0, 0.0); + t_12 = Vector3d(0.2, 0.1, 0.0); // t_12 = Vector3d(2, 0, 0.0); Eigen::Vector3d euler(0.2, 0.1, 0.3); q_12 = e2q(euler); @@ -549,7 +548,8 @@ TEST_F(ProcessorVOMultiView_class, tryTriangulationFromFeatures) track.emplace(1.0, f2); Vector4d pt_c; - EXPECT_TRUE(processor->tryTriangulationFromFeatures(std::static_pointer_cast<FeaturePointImage>(f2), track, pt_c)); + EXPECT_TRUE( + processor->tryTriangulationFromFeatures(std::static_pointer_cast<FeaturePointImage>(f2), track, pt_c)); EXPECT_QUATERNION_VECTOR_APPROX( X_c1.at(i).normalized(), pt_c, 1e-3); // Homogeneous (if normalized) is equivalent to quaternion