diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index e502ba296a1a4fee6a355839c9d7a6d52871a54a..d0321def6d175ce2f51be659045572c7229fa83e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -161,7 +161,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// eventually add more features
if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe)
{
- WOLF_TRACE("Adding more features...");
+ //WOLF_TRACE("Adding more features...");
processNew(params_tracker_->max_new_features);
}
@@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We create a KF
// set KF on last
+ last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->setKey();
// make F; append incoming to new F
diff --git a/src/processor/processor_tracker_landmark_apriltag.cpp b/src/processor/processor_tracker_landmark_apriltag.cpp
index 27f6be72295c60e01ccce2dbacdaaeae5f215068..9ee56cfb9e4c79805fe1faac9c67150915ccca2f 100644
--- a/src/processor/processor_tracker_landmark_apriltag.cpp
+++ b/src/processor/processor_tracker_landmark_apriltag.cpp
@@ -457,8 +457,11 @@ unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const unsigned
_new_features_last.erase(it);
break; //it should not be possible two detection with the same id before getting there so we can stop here.
}
+ // discard features that do not have orientation information
+ if (!std::static_pointer_cast<FeatureApriltag>(feature_in_image)->getUserotation())
+ continue;
- _new_features_last.push_back(feature_in_image); // If the feature is not in the map and not in the list of newly detected features yet then we add it.
+ _new_features_last.push_back(feature_in_image); // If the feature is not in the map & not in the list of newly detected features yet then we add it.
} //otherwise we check the next feature
}
@@ -759,40 +762,40 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
// std::cout << "last_feature_list.size(): " << last_feature_list.size() << std::endl;
// std::cout << "ori_feature_list.size(): " << ori_feature_list.size() << std::endl;
- if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){
- return;
- }
+
// Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence)
Scalar lowest_ration = 1; // rep_error1/rep_error2 cannot be higher than 1
FeatureApriltagPtr best_feature;
bool useable_feature = false;
- for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
- FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
- for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
- FeatureApriltagPtr ori_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_ori);
- if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
- Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
- if (ratio < lowest_ration){
- // if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
- useable_feature = true;
- lowest_ration = ratio;
- best_feature = last_feat_ptr;
- // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl;
+ if (last_feature_list.size() > 0 && ori_feature_list.size() > 0){
+ for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
+ FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
+ for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
+ FeatureApriltagPtr ori_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_ori);
+ if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
+ Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
+ //if (ratio < lowest_ration){
+ if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
+ useable_feature = true;
+ lowest_ration = ratio;
+ best_feature = last_feat_ptr;
+ // std::cout << "Best feature id: " << best_feature->getTagId() << std::endl;
+ }
}
}
}
}
// If there is no common feature between the two images, the continuity is broken and
// nothing can be estimated. In the case of aprilslam alone, this result in a broken factor map
- if (!useable_feature){
+ if (!useable_feature)
return;
- }
+
// std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
// Retrieve cam to landmark transform
Eigen::Vector7s cam_pose_lmk = best_feature->getMeasurement();
Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
Eigen::Affine3ds cam_M_lmk = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk;
-
+
// Get corresponding landmarks in origin/last landmark list
Eigen::Affine3ds w_M_lmk;
LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
@@ -803,14 +806,13 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
if (lmk_ptr == nullptr){
continue;
}
-
+
if (lmk_ptr->getTagId() == best_feature->getTagId()){
Eigen::Vector3s w_t_lmk = lmk_ptr->getP()->getState();
Eigen::Quaternions w_q_lmk(lmk_ptr->getO()->getState().data());
w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk;
}
}
-
// Compute last frame estimate
Eigen::Affine3ds w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
@@ -819,7 +821,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
Eigen::Quaternions quat_last(w_M_last.linear());
getLast()->getFrame()->getP()->setState(pos_last);
getLast()->getFrame()->getO()->setState(quat_last.coeffs());
-
+
// if (!best_feature->getUserotation()){
// return;
// }
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 5fe5321742ad13ccba46d777f86bea847ae1a09d..9e7d70a18701afc80ef52d169ffcec914207bfcf 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -79,6 +79,8 @@ void SolverManager::update()
// UPDATE STATE BLOCKS (state, fix or local parameterization)
for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
{
+ if (state_blocks_.find(state_ptr)==state_blocks_.end())
+ continue;
assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
// state update
@@ -106,8 +108,8 @@ void SolverManager::update()
}
}
- assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
- assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
+ //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
+ //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}
wolf::ProblemPtr SolverManager::getProblem()
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 2e96c6a72006f530c9e6a2598e6e8d0245281910..9c6abbae964f33c1be7d5aa1fbfa1dac4edc875c 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -82,7 +82,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
{
FrameBasePtr closest_kf = nullptr;
Scalar min_dt = 1e9;
-
+
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
if ((*frm_rit)->isKey())
{
@@ -95,6 +95,14 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
else
break;
}
+ if (closest_kf != nullptr)
+ {
+ WOLF_INFO("TrajectoryBase::closestKeyFrameToTimeStamp: returning KF", closest_kf->id(), " with state ", closest_kf->getState().transpose());
+ }
+ else
+ {
+ WOLF_INFO("TrajectoryBase::closestKeyFrameToTimeStamp: returning nullptr");
+ }
return closest_kf;
}