Commit ea9d298e authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Normalize homogeneous landmark after transformation to world frame

parent 3a61ab9e
......@@ -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());
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment