diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index 1d8238d83c98c5971df0f2c02a7e79c7309b9e7c..3025c32d3712e33f5cb4bac7d7a26310fc61ddab 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/src/processor/processor_visual_odometry.cpp @@ -486,16 +486,16 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat) Eigen::Vector3d point3d; point3d = pinhole::backprojectPoint( - getSensor()->getIntrinsic()->getState(), - (std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(), - point2d); + getSensor()->getIntrinsic()->getState(), + (std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(), + point2d); - //double distance = params_bundle_adjustment_->distance; // arbitrary value + // double distance = params_bundle_adjustment_->distance; // arbitrary value double distance = 1; Eigen::Vector4d vec_homogeneous_c; vec_homogeneous_c = {point3d(0),point3d(1),point3d(2),point3d.norm()/distance}; - //TODO: lmk from camera to world coordinate frame. + // lmk from camera to world coordinate frame. Transform<double,3,Isometry> T_w_r = Translation<double,3>(feat_pi->getFrame()->getP()->getState()) * Quaterniond(_feat->getFrame()->getO()->getState().data());