From f3fdd5c0b19ff9e7d84a8f1b5b1b0000e912269e Mon Sep 17 00:00:00 2001 From: gfuse Date: Thu, 2 Sep 2021 10:12:22 +0200 Subject: [PATCH] left so3 error --- GPU_version/vio/include/vio/global.h | 14 ++++++++++---- GPU_version/vio/src/frame.cpp | 17 +++++++++-------- GPU_version/vio/src/frame_handler_mono.cpp | 7 +++---- GPU_version/vio/src/map.cpp | 11 +++++++---- GPU_version/vio/src/point.cpp | 2 +- GPU_version/vio/src/reprojector.cpp | 19 +++++++++++++------ 6 files changed, 43 insertions(+), 27 deletions(-) diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index 2d86eaa..2c037f0 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -81,20 +81,26 @@ namespace vio return *T2_; } SE2 inverse() const{ - double roll=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real()); - return SE2(roll+M_PI,-1.0*T2_->translation()); + assert(T2_!= nullptr); + double pitch=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real()); + SE2 tem=SE2(pitch+M_PI,-1.0*T2_->translation()); + return tem; } // Rotation around y double pitch()const{ - return atan2(T2_->rotation_matrix()(0,1),T2_->rotation_matrix()(0,0)); + assert(T2_!= nullptr); + double pitch=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real()); + return pitch; } SE3 se3() const{ + assert(T2_!= nullptr); //Todo add 15 roll orientation Quaterniond q; q = AngleAxisd(0.261799, Vector3d::UnitX()) * AngleAxisd(pitch(), Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); - return SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1))); + SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1))); + return tem; } ~SE2_5(){ } diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp index c6822c2..0e52a6a 100644 --- a/GPU_version/vio/src/frame.cpp +++ b/GPU_version/vio/src/frame.cpp @@ -125,33 +125,34 @@ void Frame::checkKeyPoints(Feature* ftr) void Frame::removeKeyPoint(Feature* ftr) { - std::cerr<se3()*xyz_w); + if(!id_)return false; + std::cerr<<"148is\t"; + SE3 tem=this->se3(); + std::cerr<<"150is\t"; + Vector3d xyz_f = tem*xyz_w; + std::cerr<<"152is\t"; if(xyz_f.z() < 0.0) return false; // point is behind the camera + std::cerr<<"153is\t"; Vector2d px = f2c(xyz_f); if(px[0] >= 0.0 && px[1] >= 0.0 && px[0] < cam_->width() && px[1] < cam_->height()) return true; diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 6cc9696..5844b19 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -106,7 +106,8 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const res = processFirstFrame(); // set last frame - if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_; + //if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME) + last_frame_ = new_frame_; // finish processing finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); if(stage_ == STAGE_RELOCALIZING){ @@ -166,10 +167,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() new_frame_->T_f_w_=init_f.second; new_frame_->Cov_ = init_f.first; // sparse image align - //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false); - //assert(new_frame_.get()== nullptr); SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); - if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; + img_align.run(last_frame_, new_frame_); reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_); //assert (false); int n_point=0; diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp index 295fb75..51fef6e 100644 --- a/GPU_version/vio/src/map.cpp +++ b/GPU_version/vio/src/map.cpp @@ -83,10 +83,13 @@ void Map::removePtFrameRef(Frame* frame, Feature* ftr) void Map::safeDeletePoint(Point* pt) { // Delete references to mappoints in all keyframes - for(auto&& ftr:pt->obs_){ - assert(ftr); - if(ftr->frame!= nullptr)ftr->frame->removeKeyPoint(ftr); - ftr->point=NULL; + if(pt->obs_.size()>1){ + for(auto&& ftr:pt->obs_){ + ftr->frame->removeKeyPoint(ftr); + ftr->point=NULL; + } + }else{ + pt->obs_.back()->point=NULL; } pt->obs_.clear(); diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index 8b46051..2ee3ade 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -100,9 +100,9 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) cons { // TODO: get frame with same point of view AND same pyramid level! ftr= nullptr; + if(id_<1)return false; 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->id_==id){ ftr = ob; diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index c0d90e3..795b82a 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -154,10 +154,12 @@ namespace vio { for (auto &&f: _for(it_frame.item.first->fts_)) { for (auto &&match:matches) { if (match.queryIdx != f.index)continue; + if(keypoints_cur.size() < match.trainIdx)continue; if (f.item->point == NULL){ const int k = static_cast(keypoints_cur.at(match.trainIdx).pt.y / grid_.cell_size) * grid_.grid_n_cols + static_cast(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size); + assert(grid_.cells.at(k) != nullptr); Vector2d px((int) keypoints_cur.at(match.trainIdx).pt.x, (int) keypoints_cur.at(match.trainIdx).pt.y); frame->fts_.push_back(new Feature(it_frame.item.first.get(), @@ -170,7 +172,6 @@ namespace vio { }else{ 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 + static_cast(keypoints_cur.at(match.trainIdx).pt.x / grid_.cell_size); @@ -209,7 +210,11 @@ namespace vio { while(it!=cell.end()) { ++n_trials_; - if(it->pt->type_ == Point::TYPE_DELETED || it->pt ==NULL) + if( it->pt ==NULL){ + it = cell.erase(it); + continue; + } + if(it->pt->type_ == Point::TYPE_DELETED) { it = cell.erase(it); continue; @@ -233,21 +238,23 @@ namespace vio { Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_); frame->addFeature(new_feature); - // Here we add a reference in the feature to the 3D point, the other way // round is only done if this frame is selected as keyframe. new_feature->point = it->pt; - + if(matcher_.ref_ftr_== nullptr){ + cell.erase(it); + return true; + } + assert(matcher_.ref_ftr_!= nullptr); if(matcher_.ref_ftr_->type == Feature::EDGELET) { new_feature->type = Feature::EDGELET; new_feature->grad = matcher_.A_cur_ref_*matcher_.ref_ftr_->grad; new_feature->grad.normalize(); } - // If the keyframe is selected and we reproject the rest, we don't have to // check this point anymore. - it = cell.erase(it); + cell.erase(it); // Maximum one point per cell. return true;