Skip to content
Snippets Groups Projects
Commit 74ed5d9f authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'feature/apriltag_factor_optimization' of...

Merge branch 'feature/apriltag_factor_optimization' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git into feature/apriltag_factor_optimization
parents de3fc2d1 30cee4fe
No related branches found
No related tags found
2 merge requests!258Apriltag: some improvements/changes,!233WIP: Apriltag
Pipeline #2730 passed
...@@ -161,7 +161,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) ...@@ -161,7 +161,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// eventually add more features // eventually add more features
if (last_ptr_->getFeatureList().size() < params_tracker_->min_features_for_keyframe) 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); processNew(params_tracker_->max_new_features);
} }
...@@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr) ...@@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We create a KF // We create a KF
// set KF on last // set KF on last
last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
last_ptr_->getFrame()->setKey(); last_ptr_->getFrame()->setKey();
// make F; append incoming to new F // make F; append incoming to new F
......
...@@ -457,8 +457,11 @@ unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const unsigned ...@@ -457,8 +457,11 @@ unsigned int ProcessorTrackerLandmarkApriltag::detectNewFeatures(const unsigned
_new_features_last.erase(it); _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. 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 } //otherwise we check the next feature
} }
...@@ -759,40 +762,40 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){ ...@@ -759,40 +762,40 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
// std::cout << "last_feature_list.size(): " << last_feature_list.size() << std::endl; // std::cout << "last_feature_list.size(): " << last_feature_list.size() << std::endl;
// std::cout << "ori_feature_list.size(): " << ori_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) // 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 Scalar lowest_ration = 1; // rep_error1/rep_error2 cannot be higher than 1
FeatureApriltagPtr best_feature; FeatureApriltagPtr best_feature;
bool useable_feature = false; bool useable_feature = false;
for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){ if (last_feature_list.size() > 0 && ori_feature_list.size() > 0){
FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last); for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){ FeatureApriltagPtr last_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_last);
FeatureApriltagPtr ori_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_ori); for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){ FeatureApriltagPtr ori_feat_ptr = std::static_pointer_cast<FeatureApriltag>(*it_ori);
Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2(); if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
if (ratio < lowest_ration){ Scalar ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
// if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){ //if (ratio < lowest_ration){
useable_feature = true; if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
lowest_ration = ratio; useable_feature = true;
best_feature = last_feat_ptr; lowest_ration = ratio;
// std::cout << "Best feature id: " << best_feature->getTagId() << std::endl; 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 // 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 // nothing can be estimated. In the case of aprilslam alone, this result in a broken factor map
if (!useable_feature){ if (!useable_feature)
return; return;
}
// std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl; // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
// Retrieve cam to landmark transform // Retrieve cam to landmark transform
Eigen::Vector7s cam_pose_lmk = best_feature->getMeasurement(); Eigen::Vector7s cam_pose_lmk = best_feature->getMeasurement();
Eigen::Quaternions cam_q_lmk(cam_pose_lmk.segment<4>(3).data()); 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; Eigen::Affine3ds cam_M_lmk = Eigen::Translation3ds(cam_pose_lmk.head(3)) * cam_q_lmk;
// Get corresponding landmarks in origin/last landmark list // Get corresponding landmarks in origin/last landmark list
Eigen::Affine3ds w_M_lmk; Eigen::Affine3ds w_M_lmk;
LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList(); LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
...@@ -803,14 +806,13 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){ ...@@ -803,14 +806,13 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
if (lmk_ptr == nullptr){ if (lmk_ptr == nullptr){
continue; continue;
} }
if (lmk_ptr->getTagId() == best_feature->getTagId()){ if (lmk_ptr->getTagId() == best_feature->getTagId()){
Eigen::Vector3s w_t_lmk = lmk_ptr->getP()->getState(); Eigen::Vector3s w_t_lmk = lmk_ptr->getP()->getState();
Eigen::Quaternions w_q_lmk(lmk_ptr->getO()->getState().data()); Eigen::Quaternions w_q_lmk(lmk_ptr->getO()->getState().data());
w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk; w_M_lmk = Eigen::Translation<Scalar,3>(w_t_lmk) * w_q_lmk;
} }
} }
// Compute last frame estimate // Compute last frame estimate
Eigen::Affine3ds w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse(); Eigen::Affine3ds w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
...@@ -819,7 +821,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){ ...@@ -819,7 +821,7 @@ void ProcessorTrackerLandmarkApriltag::reestimateLastFrame(){
Eigen::Quaternions quat_last(w_M_last.linear()); Eigen::Quaternions quat_last(w_M_last.linear());
getLast()->getFrame()->getP()->setState(pos_last); getLast()->getFrame()->getP()->setState(pos_last);
getLast()->getFrame()->getO()->setState(quat_last.coeffs()); getLast()->getFrame()->getO()->setState(quat_last.coeffs());
// if (!best_feature->getUserotation()){ // if (!best_feature->getUserotation()){
// return; // return;
// } // }
......
...@@ -79,6 +79,8 @@ void SolverManager::update() ...@@ -79,6 +79,8 @@ void SolverManager::update()
// UPDATE STATE BLOCKS (state, fix or local parameterization) // UPDATE STATE BLOCKS (state, fix or local parameterization)
for (auto state_ptr : wolf_problem_->getStateBlockPtrList()) 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 !"); assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
// state update // state update
...@@ -106,8 +108,8 @@ void SolverManager::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_->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_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
} }
wolf::ProblemPtr SolverManager::getProblem() wolf::ProblemPtr SolverManager::getProblem()
......
...@@ -82,7 +82,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) ...@@ -82,7 +82,7 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
{ {
FrameBasePtr closest_kf = nullptr; FrameBasePtr closest_kf = nullptr;
Scalar min_dt = 1e9; Scalar min_dt = 1e9;
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++)
if ((*frm_rit)->isKey()) if ((*frm_rit)->isKey())
{ {
...@@ -95,6 +95,14 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts) ...@@ -95,6 +95,14 @@ FrameBasePtr TrajectoryBase::closestKeyFrameToTimeStamp(const TimeStamp& _ts)
else else
break; 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; return closest_kf;
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment