From ea9d298e955b2992980e396a3ebcc085390fd841 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Sat, 23 Apr 2022 23:00:32 +0200
Subject: [PATCH] Normalize homogeneous landmark after transformation to world
frame
---
src/processor/processor_visual_odometry.cpp | 4 +++-
1 file changed, 3 insertions(+), 1 deletion(-)
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 0be8e83c3..ae13d5d32 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -492,7 +492,6 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
double distance = 1;
Eigen::Vector4d vec_homogeneous_c;
vec_homogeneous_c = {point3d(0),point3d(1),point3d(2),point3d.norm()/distance};
- vec_homogeneous_c.normalize();
//TODO: lmk from camera to world coordinate frame.
Transform<double,3,Isometry> T_w_r
@@ -505,6 +504,9 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
* T_r_c
* vec_homogeneous_c;
+ // normalize to make equivalent to a unit quaternion
+ vec_homogeneous_w.normalize();
+
auto lmk_hp_ptr = LandmarkBase::emplace<LandmarkHp>(getProblem()->getMap(),
vec_homogeneous_w,
feat_pi->getKeyPoint().getDescriptor());
--
GitLab