From 5e8e31d691d626207da7b99d8cd64ee46d4a4450 Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 30 Aug 2021 15:06:44 +0200 Subject: [PATCH] local m: working version 3 --- GPU_version/vio/include/vio/frame.h | 2 +- GPU_version/vio/include/vio/global.h | 2 +- GPU_version/vio/kernel/compute-residual.cl | 2 +- GPU_version/vio/src/depth_filter.cpp | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 10 +++---- GPU_version/vio/src/map.cpp | 1 + GPU_version/vio/src/reprojector.cpp | 32 +++------------------- 7 files changed, 14 insertions(+), 37 deletions(-) diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 53fb1bb..b95c86a 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -128,7 +128,7 @@ namespace vio { //TODO add 15 degree roll orientation SE2 inv=T_f_w_.inverse(); Quaterniond q; - q = AngleAxisd(0.261799, Vector3d::UnitX()) + q = AngleAxisd(-0.261799, Vector3d::UnitX()) * AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1))); diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index f115137..20835f5 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -92,7 +92,7 @@ namespace vio SE3 se3() const{ //Todo add 15 roll orientation Quaterniond q; - q = AngleAxisd(0.261799, Vector3d::UnitX()) + 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))); diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index f614ae1..b9d933b 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -21,7 +21,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) float3 error=ref+cur; float yaw=0.0; float pitch=error.z; - float roll=0.26; + float roll=-0.26; float R00=cos(yaw)*cos(pitch);//cos(pitch) float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0 float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch) diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index 62fa181..cda056d 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -218,7 +218,7 @@ namespace vio { SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation()); ///TODO add 15 degrees roll orientation Quaterniond q; - q = AngleAxisd(0.261799, Vector3d::UnitX()) + q = AngleAxisd(-0.261799, Vector3d::UnitX()) * AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY()) * AngleAxisd(0.0, Vector3d::UnitZ()); SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1))); diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 5daa451..c2c92eb 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -94,7 +94,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const overlap_kfs_.clear(); // create new frame - new_frame_.reset(new Frame(cam_, img.clone(), timestamp)); + new_frame_=boost::make_shared(cam_, img.clone(), timestamp); time_=time; // process frame UpdateResult res = RESULT_FAILURE; @@ -107,7 +107,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const // set last frame if(!new_frame_->fts_.empty() || stage_ != STAGE_DEFAULT_FRAME)last_frame_ = new_frame_; - new_frame_.reset(); // finish processing finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs()); if(stage_ == STAGE_RELOCALIZING){ @@ -168,11 +167,12 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() 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; -// reprojector_.reprojectMap(new_frame_, overlap_kfs_); - reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); -// reprojector_.reprojectMap2(new_frame_, overlap_kfs_); + // reprojector_.reprojectMap(new_frame_, overlap_kfs_); + // reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); + reprojector_.reprojectMap2(new_frame_, overlap_kfs_); //assert (false); std::cout<<"Reprojection Map nPoint: "<key_pts_.size()<<'\t'; for(auto keypoint : kf->key_pts_) { if(keypoint == nullptr) diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 52fa5e5..f4dd63c 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -71,18 +71,11 @@ void Reprojector::reprojectMap( std::vector< std::pair >& overlap_kfs) { resetGrid(); - - // Identify those Keyframes which share a common field of view. - // SVO_START_TIMER("reproject_kfs"); list< pair > close_kfs; + //assert(frame== nullptr); map_.getCloseKeyframes(frame, close_kfs); - - // Sort KFs with overlap according to their closeness close_kfs.sort(boost::bind(&std::pair::second, _1) < boost::bind(&std::pair::second, _2)); - - // Reproject all mappoints of the closest N kfs with overlap. We only store - // in which grid cell the points fall. size_t n = 0; overlap_kfs.reserve(options_.max_n_kfs); for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); @@ -90,16 +83,11 @@ void Reprojector::reprojectMap( { FramePtr ref_frame = it_frame->first; overlap_kfs.push_back(pair(ref_frame,0)); - - // Try to reproject each mappoint that the other KF observes for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); it_ftr!=ite_ftr; ++it_ftr) { - // check if the feature has a mappoint assigned if((*it_ftr)->point == NULL) continue; - - // make sure we project a point only once if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) continue; (*it_ftr)->point->last_projected_kf_id_ = frame->id_; @@ -108,11 +96,6 @@ void Reprojector::reprojectMap( } } } - //SVO_STOP_TIMER("reproject_kfs"); - - // Now project all point candidates - // SVO_START_TIMER("reproject_candidates"); - { boost::unique_lock lock(map_.point_candidates_.mut_); auto it=map_.point_candidates_.candidates_.begin(); while(it!=map_.point_candidates_.candidates_.end()) @@ -129,21 +112,14 @@ void Reprojector::reprojectMap( } ++it; } - } // unlock the mutex when out of scope - //SVO_STOP_TIMER("reproject_candidates"); - // Now we go through each grid cell and select one point to match. - // At the end, we should have at maximum one reprojected point per cell. - //SVO_START_TIMER("feature_align"); for(size_t i=0; i (size_t) Config::maxFts()) break; } -// SVO_STOP_TIMER("feature_align"); } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO - First reimplementation @@ -373,7 +349,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) it = cell.erase(it); continue; } - /*bool found_match = true; + bool found_match = true; if(options_.find_match_direct) found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px); if(!found_match) @@ -385,7 +361,7 @@ bool Reprojector::reprojectCell(Cell& cell, FramePtr frame) map_.point_candidates_.deleteCandidatePoint(it->pt); it = cell.erase(it); continue; - }*/ + } it->pt->n_succeeded_reproj_++; if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10) it->pt->type_ = Point::TYPE_GOOD;