From f1f498043d6f7aa4fa92d3508de36d398f500340 Mon Sep 17 00:00:00 2001
From: jongwonjlee <jongwon5@illinois.edu>
Date: Sat, 23 Sep 2023 15:06:26 -0500
Subject: [PATCH] make changes compatible with new wolfcore for enabling
multiple processors per capture
---
src/processor/processor_visual_odometry.cpp | 20 +++++++++++---------
1 file changed, 11 insertions(+), 9 deletions(-)
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 068492d66..c2c6cd1b0 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -434,7 +434,7 @@ void ProcessorVisualOdometry::establishFactors()
LandmarkBasePtr associated_lmk = nullptr;
for (auto lmk: getProblem()->getMap()->getLandmarkList())
{
- if (lmk->id() == feat_pi->trackId()){
+ if (lmk->trackId() == feat_pi->trackId()){
associated_lmk = lmk;
}
}
@@ -444,11 +444,14 @@ void ProcessorVisualOdometry::establishFactors()
if (associated_lmk)
{
LandmarkHpPtr associated_lmk_hp = std::dynamic_pointer_cast<LandmarkHp>(associated_lmk);
- FactorBase::emplace<FactorPixelHp>(feat_pi,
- feat_pi,
- associated_lmk_hp,
- shared_from_this(),
- params_visual_odometry_->apply_loss_function);
+
+ if (associated_lmk_hp) {
+ FactorBase::emplace<FactorPixelHp>(feat_pi,
+ feat_pi,
+ associated_lmk_hp,
+ shared_from_this(),
+ params_visual_odometry_->apply_loss_function);
+ }
}
// 2) create landmark if track is not associated with one and has enough length
@@ -456,7 +459,6 @@ void ProcessorVisualOdometry::establishFactors()
{
// std::cout << "NEW valid track \\o/" << std::endl;
LandmarkBasePtr lmk = emplaceLandmark(feat_pi);
- lmk->setId(feat_pi->trackId());
// Add factors from all KFs of this track to the new lmk
Track track_kf = track_matrix_.trackAtKeyframes(feat->trackId());
@@ -513,8 +515,8 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
// Set all IDs equal to track ID
size_t track_id = _feat->trackId();
- lmk_hp_ptr->setId(track_id);
- _feat->setLandmarkId(track_id);
+ lmk_hp_ptr->setTrackId(track_id);
+ _feat->setLandmarkId(lmk_hp_ptr->id());
return lmk_hp_ptr;
}
--
GitLab