diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 8538777e55096d3f633e726d825c3a0dd98b860d..388e3aa4eb3167b5064a456185c1bd1e030f6a2b 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -118,6 +118,7 @@ struct ParamsProcessorVisualOdometry
unsigned int min_track_length;
double min_pixel_dist;
bool use_homogeneous;
+ double distance_init;
};
DebugParams debug;
@@ -229,6 +230,7 @@ struct ParamsProcessorVisualOdometry
lmk_creation.min_track_length = _params["landmark_creation"]["min_track_length"].as<unsigned int>();
lmk_creation.min_pixel_dist = _params["landmark_creation"]["min_pixel_dist"].as<unsigned int>();
lmk_creation.use_homogeneous = _params["landmark_creation"]["use_homogeneous"].as<bool>();
+ lmk_creation.distance_init = _params["landmark_creation"]["distance_init"].as<double>();
}
};
@@ -238,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;
@@ -396,11 +398,12 @@ class ProcessorVisualOdometry : public ProcessorTracker, public MotionProvider
/**
* \brief Emplace a landmark corresponding to a track and initialize it with triangulation.
* \param _feature_ptr a pointer to the feature used to create the new landmark
+ * \param _distance distance to the camera in meters
* \return a pointer to the created landmark
*
- * Implementation: Use rays of features detections in last frame and create a landmark at 1 meter (arbitrary)
+ * Implementation: Use rays of features detections in last frame and create a landmark at '_distance' meters
*/
- LandmarkBasePtr emplaceLandmarkNaive(FeaturePointImagePtr _feature_ptr);
+ LandmarkBasePtr emplaceLandmarkNaive(FeaturePointImagePtr _feature_ptr, double _distance);
/**
* \brief Emplace a landmark corresponding to a track and initialize it with triangulation.
diff --git a/schema/processor/ProcessorVisualOdometry.schema b/schema/processor/ProcessorVisualOdometry.schema
index b10cb351959a4fb0480fae8f464877b7fd44b594..9c886abf7c71e2c4313f5b0a2dd63ff008d3fdbb 100644
--- a/schema/processor/ProcessorVisualOdometry.schema
+++ b/schema/processor/ProcessorVisualOdometry.schema
@@ -148,4 +148,10 @@ landmark_creation:
use_homogeneous:
_type: bool
_mandatory: true
- _doc: The created landmarks will use homogeneous coordinates (recommended), if false, it will create euclidean 3D points.
\ No newline at end of file
+ _doc: The created landmarks will use homogeneous coordinates (recommended), if false, it will create euclidean 3D points.
+
+ distance_init:
+ _type: double
+ _mandatory: false
+ _default: 10.0
+ _doc: "The distance used to initialize the landmarks in meters projecting the feature direction, only done when triangulation method fails."
\ No newline at end of file
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 82582470cb80c9042d5859a296059a6d55943990..c63f98259cdffa53a9e0ab8108f9e3db350c1c4a 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -386,7 +386,7 @@ 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);
+ 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()))
@@ -563,9 +563,6 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
cv::Mat& _E,
VectorComposite& _pose_prev_curr) const
{
- // TODO: Check new RANSAC methods introduced in opencv 4.5.0
- // https://opencv.org/evaluating-opencvs-new-ransacs/
-
// We need at least five tracks
// TODO: this case is NOT handled by the rest of the algorithm currently
if (_tracks_prev_curr.size() < 5) return false;
@@ -812,28 +809,26 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkTriangulation(FeaturePoi
// at least 2 KF needed to triangulate
if (_track_kf.size() < 2) return nullptr;
- Vector4d pt_c;
- if (not tryTriangulationFromFeatures(_feat, _track_kf, pt_c)) return nullptr;
+ Vector4d pt_w;
+ if (not tryTriangulationFromFeatures(_feat, _track_kf, pt_w)) return nullptr;
if (params_visual_odometry_.lmk_creation.use_homogeneous)
- return LandmarkBase::emplace<LandmarkHp>(getProblem()->getMap(), pt_c, _feat->getKeyPoint().getDescriptor());
+ return LandmarkBase::emplace<LandmarkHp>(getProblem()->getMap(), pt_w, _feat->getKeyPoint().getDescriptor());
else
return LandmarkBase::emplace<LandmarkVisualPoint3d>(
- getProblem()->getMap(), pt_c.head<3>() / pt_c(3), _feat->getKeyPoint().getDescriptor());
+ getProblem()->getMap(), pt_w.head<3>() / pt_w(3), _feat->getKeyPoint().getDescriptor());
}
bool ProcessorVisualOdometry::tryTriangulationFromFeatures(FeaturePointImagePtr _feat,
Track _track_kf,
- Vector4d& pt_c) const
+ Vector4d& pt_w) const
{
// heuristic: use oldest feature/KF to triangulate with respect to current time
FeaturePointImagePtr feat_pi1 = std::static_pointer_cast<FeaturePointImage>(_track_kf.begin()->second);
cv::Vec2d pix1, pix2;
- // WOLF_INFO("TOTO")
cv::eigen2cv(feat_pi1->getMeasurement(), pix1);
cv::eigen2cv(_feat->getMeasurement(), pix2);
- // WOLF_INFO(pix1, pix2);
// create 3x4 projection matrices [K|0] * Tcw
Matrix4d Tcw1 = computeTcw(feat_pi1->getCapture()->getTimeStamp()).matrix();
@@ -847,26 +842,27 @@ bool ProcessorVisualOdometry::tryTriangulationFromFeatures(FeaturePointImagePtr
// perform triangulation of one landmark
cv::Vec4d ptcv_w; // output: homogeneous landmark point expressed in world frame
cv::triangulatePoints(P1_cv, P2_cv, pix1, pix2, ptcv_w);
- // WOLF_INFO("YAY: ", ptcv_w)
- /////////////////////////////////////////////////////////
- // check that triangulation was done with enough parallax
- /////////////////////////////////////////////////////////
- bool triangulation_is_a_success = true; // Not implemented yet
- if (!triangulation_is_a_success)
+ // convert to Eigen
+ cv::cv2eigen(ptcv_w, pt_w);
+
+ // Normalization necessary since homogeneous point stateblock is supposed to be unitary
+ pt_w.normalize();
+ if (pt_w(3) < 0)
+ pt_w *= -1;
+
+ // Cheriality check
+ // transform to camera frame
+ auto pt_c = Tcw1.inverse().matrix() * pt_w;
+ // check 3rd coordinate is positive
+ if (pt_c(2) / pt_c(3) < 0)
{
+ WOLF_DEBUG("pt_w:", pt_w.transpose());
+ WOLF_DEBUG("pt_c:", pt_c.transpose());
+ WOLF_DEBUG("Triangulation failed: negative depth.");
return false;
}
- // normalize to make equivalent to a unit quaternion
- cv::cv2eigen(ptcv_w, pt_c);
-
- // HACK: to avoid "nans" in residual, set Z = 1
- // pt_c(2) = 1 * pt_c(3);
-
- // Normalization necessary since homogeneous point stateblock is supposed to be unitary
- pt_c.normalize();
-
return true;
}
@@ -888,15 +884,14 @@ Eigen::Isometry3d ProcessorVisualOdometry::computeTcw(TimeStamp _ts) const
return (T_w_r * T_r_c).inverse();
}
-LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkNaive(FeaturePointImagePtr _feat)
+LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmarkNaive(FeaturePointImagePtr _feat, double _distance)
{
- // Initialize the landmark in its ray (based on pixel meas) and using a arbitrary distance
- double distance = 1;
+ // Initialize the landmark in its ray (based on pixel meas) and using an arbitrary distance
Eigen::Vector3d point3d_c =
pinhole::backprojectPoint(getSensor()->getIntrinsic()->getState(),
(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
_feat->getMeasurement(),
- distance);
+ _distance);
// lmk from camera to world coordinate frame.
Transform<double, 3, Isometry> T_w_r = Translation<double, 3>(_feat->getFrame()->getP()->getState()) *
diff --git a/test/gtest_processor_visual_odometry.cpp b/test/gtest_processor_visual_odometry.cpp
index c682c3435beeeeb0ac2436b4afb328e263d8603b..d82e50bfaf66fd41e69312f861d99f529cec278a 100644
--- a/test/gtest_processor_visual_odometry.cpp
+++ b/test/gtest_processor_visual_odometry.cpp
@@ -114,9 +114,9 @@ class ProcessorVisualOdometryPublic : public ProcessorVisualOdometry
return ProcessorVisualOdometry::tryTriangulationFromFeatures(_feat, _track_kf, pt_c);
}
- LandmarkBasePtr emplaceLandmarkNaive(FeaturePointImagePtr _feature_ptr)
+ LandmarkBasePtr emplaceLandmarkNaive(FeaturePointImagePtr _feature_ptr, double _distance)
{
- return ProcessorVisualOdometry::emplaceLandmarkNaive(_feature_ptr);
+ return ProcessorVisualOdometry::emplaceLandmarkNaive(_feature_ptr, _distance);
}
LandmarkBasePtr emplaceLandmarkTriangulation(FeaturePointImagePtr _feature_ptr, Track _track_kf)
@@ -177,6 +177,7 @@ TEST(ProcessorVisualOdometry, kltTrack)
_params["landmark_creation"]["min_track_length"] = 4;
_params["landmark_creation"]["min_pixel_dist"] = 1;
_params["landmark_creation"]["use_homogeneous"] = true;
+ _params["landmark_creation"]["distance_init"] = 10.0;
ProcessorVisualOdometryPublicPtr processor = std::static_pointer_cast<ProcessorVisualOdometryPublic>(
ProcessorVisualOdometry::create(_params, {vision_dir, wolf_schema_dir}));
@@ -405,7 +406,7 @@ class ProcessorVOMultiView_class : public testing::Test
Eigen::Quaterniond q_21, q_12;
Eigen::Vector4d k;
cv::Mat Kcv;
- std::vector<Eigen::Vector4d> X_c1;
+ std::vector<Eigen::Vector4d> X_c1, X_w;
void SetUp() override
{
@@ -423,7 +424,7 @@ class ProcessorVOMultiView_class : public testing::Test
cv::eigen2cv(camera->getIntrinsicMatrix(), Kcv);
k << Kcv.at<double>(0, 2), Kcv.at<double>(1, 2), Kcv.at<double>(0, 0), Kcv.at<double>(1, 1);
- // Set 3D points on the previous camera frame -> all in front of the camera is better
+ // Set 3D points on the first camera frame -> all in front of the camera
X_c1.clear();
X_c1.push_back((Eigen::Vector4d() << 1.0, 1.0, 2.0, 1.0).finished());
X_c1.push_back((Eigen::Vector4d() << -1.0, -1.0, 2.0, 1.0).finished());
@@ -434,20 +435,24 @@ class ProcessorVOMultiView_class : public testing::Test
X_c1.push_back((Eigen::Vector4d() << 0.1, -1.0, 3.0, 1.0).finished());
X_c1.push_back((Eigen::Vector4d() << 0.75, 0.75, 2.0, 1.0).finished());
- // Project pixels in the previous view
+ // Keep 3D points in the world frame
+ 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();
for (auto i = 0; i < 8; i++)
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);
SE3::inverse(t_12, q_12, t_21, q_21);
+ // Project pixels in the second view
for (auto i = 0; i < 8; i++)
mwkps_c2.emplace(i, WKeyPoint(pinhole::projectPoint(k, cam_dist_vec, q_21 * X_c1.at(i).head<3>() + t_21)));
@@ -544,7 +549,7 @@ TEST_F(ProcessorVOMultiView_class, tryTriangulationFromFeatures)
track.emplace(1.0, f2);
Vector4d pt_c;
- 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
@@ -568,6 +573,9 @@ TEST_F(ProcessorVOMultiView_class, emplaceLandmarkTriangulation)
// create features
for (auto i = 0; i < 8; i++)
{
+ WOLF_DEBUG("i = ", i);
+ WOLF_DEBUG("X_c1 = ", X_c1.at(i).head<3>().transpose());
+
auto f1 = FeatureBase::emplace<FeaturePointImage>(c1, mwkps_c1.at(i), Matrix2d::Identity());
auto f2 = FeatureBase::emplace<FeaturePointImage>(c2, mwkps_c2.at(i), Matrix2d::Identity());
@@ -577,6 +585,7 @@ TEST_F(ProcessorVOMultiView_class, emplaceLandmarkTriangulation)
auto lmk = processor->emplaceLandmarkTriangulation(std::static_pointer_cast<FeaturePointImage>(f2), track);
+ ASSERT_TRUE(lmk);
EXPECT_QUATERNION_VECTOR_APPROX(X_c1.at(i).normalized(),
lmk->getState().at('H'),
1e-3); // Homogeneous (if normalized) is equivalent to quaternion
@@ -596,7 +605,7 @@ TEST_F(ProcessorVOMultiView_class, emplaceLandmarkNaive)
{
auto f1 = FeatureBase::emplace<FeaturePointImage>(c1, mwkps_c1.at(i), Matrix2d::Identity());
- auto lmk = processor->emplaceLandmarkNaive(f1);
+ auto lmk = processor->emplaceLandmarkNaive(f1, 10.0);
EXPECT_MATRIX_APPROX(X_c1.at(i).head<3>() / X_c1.at(i)(2),
lmk->getState().at('H').head<3>() / lmk->getState().at('H')(2),