From 4d857d790cb95b60143600aecb44d31ae207fc60 Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 1 Sep 2021 22:49:21 +0200 Subject: [PATCH] debug close obsview --- GPU_version/vio/CMakeLists.txt | 2 +- GPU_version/vio/include/sophus/se2.cpp | 14 ++++++------ GPU_version/vio/include/sophus/se2.h | 6 +++--- GPU_version/vio/include/vio/frame.h | 4 ++-- GPU_version/vio/include/vio/global.h | 30 ++++++++++++-------------- GPU_version/vio/include/vio/point.h | 1 + GPU_version/vio/src/depth_filter.cpp | 2 +- GPU_version/vio/src/matcher.cpp | 5 ++--- GPU_version/vio/src/point.cpp | 6 ++++-- GPU_version/vio/src/reprojector.cpp | 22 +++++++++++-------- 10 files changed, 48 insertions(+), 44 deletions(-) diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index 395e8c9..abc1a59 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/GPU_version/vio/CMakeLists.txt @@ -39,7 +39,7 @@ add_service_files(FILES generate_messages(DEPENDENCIES std_msgs) catkin_package( - DEPENDS Eigen OpenCV Sophus Boost + DEPENDS Eigen OpenCV Boost CATKIN_DEPENDS roscpp roslib image_transport cv_bridge tf geometry_msgs message_runtime INCLUDE_DIRS include ) diff --git a/GPU_version/vio/include/sophus/se2.cpp b/GPU_version/vio/include/sophus/se2.cpp index 2d61e0c..caf7cbe 100644 --- a/GPU_version/vio/include/sophus/se2.cpp +++ b/GPU_version/vio/include/sophus/se2.cpp @@ -62,15 +62,15 @@ SE2 SE2 { SE2 result(*this); result.translation_ += so2_*(other.translation_); - result.so2_*=other.so2_; + result.so2_ *= other.so2_; return result; } SE2& SE2 ::operator *= (const SE2 & other) { - translation_+= so2_*(other.translation_); - so2_*=other.so2_; + translation_ += so2_ * (other.translation_); + so2_ *= other.so2_; return *this; } @@ -79,7 +79,7 @@ SE2 SE2 { SE2 ret; ret.so2_= so2_.inverse(); - ret.translation_ = ret.so2_*(translation_*-1.); + ret.translation_ = ret.so2_ * (translation_ *-1.); return ret; } @@ -113,8 +113,8 @@ Matrix SE2 Matrix3d res; res.setIdentity(); res.topLeftCorner<2,2>() = R; - res(0,2) = translation_[1]; - res(1,2) = -translation_[0]; + res(0,2) = translation_.y(); + res(1,2) = -translation_.x(); return res; } @@ -220,7 +220,7 @@ Vector3d SE2 Matrix2d V_inv; V_inv(0,0) = halftheta_by_tan_of_halftheta; V_inv(1,0) = -halftheta; V_inv(0,1) = halftheta; V_inv(1,1) = halftheta_by_tan_of_halftheta; - upsilon_theta.head<2>() = V_inv*se2.translation_; + upsilon_theta.head<2>() = V_inv* se2.translation_; return upsilon_theta; } diff --git a/GPU_version/vio/include/sophus/se2.h b/GPU_version/vio/include/sophus/se2.h index 3774a05..f7b90a1 100644 --- a/GPU_version/vio/include/sophus/se2.h +++ b/GPU_version/vio/include/sophus/se2.h @@ -44,7 +44,7 @@ public: const Vector2d & translation); SE2 (double theta, - const Vector2d & translation_); + const Vector2d & translation); SE2 (const SE2 & other); @@ -95,11 +95,11 @@ public: { return translation_; } - +/* Vector2d& translation() { return translation_; - } + }*/ Matrix2d rotation_matrix() const { diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 3c91654..bf21bd6 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -119,8 +119,8 @@ namespace vio { /// Return the pose of the frame in the (w)orld coordinate frame. inline Vector2d pos() const { - Vector2d tem=T_f_w_.se2().translation(); - return tem; + SE2 tem=T_f_w_.se2(); + return tem.translation(); } inline SE3 se3() const{ diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index 28eed6c..2d86eaa 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -52,31 +52,29 @@ namespace vio const double PI = 3.14159265; class SE2_5: public SE2{ public: - SE2_5(SE2&& se2):T2_(new SE2(se2)){ + SE2_5(SE2&& se2){ + T2_=new SE2(se2); + assert(T2_!= nullptr); } - SE2_5(SE2& se2):T2_(new SE2(se2)){ + SE2_5(SE2& se2){ + T2_=new SE2(se2); + assert(T2_!= nullptr); } SE2_5(SE3&& se3){ Quaterniond q=se3.unit_quaternion().normalized(); auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw - Eigen::Matrix R; - R< R; - R< R; - R<ftr->frame == frame.get()) + if(it->ftr->frame->id_ == frame->id_) { it = seeds_.erase(it); ++n_removed; diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 843be4c..7fb8832 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -141,10 +141,9 @@ bool Matcher::findMatchDirect( Vector2d& px_cur) { if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false; - //if(ref_ftr_->frame->cam_== nullptr)return false; - assert(ref_ftr_->frame!= nullptr); + if(ref_ftr_== nullptr)return false; + if(ref_ftr_->frame== nullptr)return false; if(ref_ftr_->frame->cam_== nullptr)return false; - assert(ref_ftr_->frame->cam_!= nullptr); Vector2i pxi=ref_ftr_->px.cast(); if(ref_ftr_->level==NULL){ if(!ref_ftr_->frame->cam_->isInFrame(pxi, 6))return false; diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index d88e4ab..3868847 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -75,7 +75,7 @@ bool Point::deleteFrameRef(Frame* frame) { for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) { - if((*it)->frame == frame) + if((*it)->frame->id_ == frame->id_) { obs_.erase(it); return true; @@ -98,11 +98,12 @@ void Point::initNormal() bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const { // TODO: get frame with same point of view AND same pyramid level! + ftr= nullptr; Vector3d obs_dir(Vector3d(framepos(0),1e-9,framepos(1)) + pos_); obs_dir.normalize(); double max_cos_angle = 1.0; if(obs_.empty())return false; for(auto&& ob:obs_){ - if(ob->frame->T_f_w_.empty())continue; + if(ob == nullptr)continue; Vector2d t=ob->frame->pos(); Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_); double cos_angle = obs_dir.dot(dir.normalized()); @@ -112,6 +113,7 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const ftr = ob; } } + if(ftr== nullptr)return false; return true; } diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 0bccbf4..e4f3c74 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -139,6 +139,7 @@ namespace vio { close_kfs.sort(boost::bind(&std::pair::second, _1) < boost::bind(&std::pair::second, _2)); overlap_kfs.reserve(options_.max_n_kfs); + boost::unique_lock lock(map_.point_candidates_.mut_); for (auto &&it_frame:_for(close_kfs)) { if (it_frame.index > options_.max_n_kfs)continue; std::vector keypoints_kfs; @@ -151,11 +152,14 @@ namespace vio { extractor->compute(it_frame.item.first->img(), keypoints_kfs, descriptors_ref); matcher->match(descriptors_ref, descriptors_cur, matches); for (auto &&f: _for(it_frame.item.first->fts_)) { - if (f.item->point == NULL)f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item); - if (f.item->point->last_frame_overlap_id_ == frame->id_)continue; - f.item->point->last_frame_overlap_id_ = frame->id_; for (auto &&match:matches) { if (match.queryIdx != f.index)continue; + if (f.item->point == NULL){ + f.item->point = new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y())); + // assert(!f.item->point->obs_.empty()); + } + if (f.item->point->last_frame_overlap_id_ == frame->id_)continue; + f.item->point->last_frame_overlap_id_ = frame->id_; assert(keypoints_cur.size() > match.trainIdx); const int k = static_cast(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols @@ -163,6 +167,8 @@ namespace vio { assert(grid_.cells.at(k) != nullptr); Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x, (int) keypoints_cur.at(match.trainIdx).pt.y); + f.item->point->addFrameRef(new Feature(it_frame.item.first.get(),f.item->px,0)); + //f.item->point->addFrameRef(f.item); grid_.cells.at(k)->push_back(Candidate(f.item->point, px)); overlap_kfs.back().second++; break; @@ -170,7 +176,6 @@ namespace vio { } } { - boost::unique_lock lock(map_.point_candidates_.mut_); for (auto&& cell : grid_.cells) { if (reprojectCell(*cell, frame)) ++n_matches_; @@ -193,16 +198,14 @@ namespace vio { while(it!=cell.end()) { ++n_trials_; - if(it->pt->type_ == Point::TYPE_DELETED) { it = cell.erase(it); continue; } - bool found_match = true; - if(options_.find_match_direct) - found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); - if(!found_match) + + syn_=true; + if(!matcher_.findMatchDirect(*it->pt, *frame, it->px)) { it->pt->n_failed_reproj_++; if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15) @@ -212,6 +215,7 @@ namespace vio { it = cell.erase(it); continue; } + syn_=false; it->pt->n_succeeded_reproj_++; if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) it->pt->type_ = Point::TYPE_GOOD;