From 034158ef3978b04aa315d234c9e8401aa2fd6331 Mon Sep 17 00:00:00 2001 From: gfuse Date: Tue, 31 Aug 2021 19:04:34 +0200 Subject: [PATCH] latest working version --- GPU_version/vio/include/vio/frame.h | 2 +- GPU_version/vio/include/vio/global.h | 2 +- GPU_version/vio/include/vio/point.h | 2 +- GPU_version/vio/include/vio/ukf.h | 13 ++++------- GPU_version/vio/kernel/compute-residual.cl | 2 +- GPU_version/vio/src/depth_filter.cpp | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 4 ++-- GPU_version/vio/src/point.cpp | 4 ++-- GPU_version/vio/src/reprojector.cpp | 26 +++++----------------- 9 files changed, 18 insertions(+), 39 deletions(-) diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index b95c86a..53fb1bb 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 20835f5..f115137 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/include/vio/point.h b/GPU_version/vio/include/vio/point.h index 6d7c20b..01de101 100644 --- a/GPU_version/vio/include/vio/point.h +++ b/GPU_version/vio/include/vio/point.h @@ -54,7 +54,7 @@ public: size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. g2oPoint* v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. int last_published_ts_; //!< Timestamp of last publishing. - int last_projected_kf_id_; //!< Flag for the reprojection: don't reproject a pt twice. + int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice. PointType type_; //!< Quality of the point. int n_failed_reproj_; //!< Number of failed reprojections. Used to assess the quality of the point. int n_succeeded_reproj_; //!< Number of succeeded reprojections. Used to assess the quality of the point. diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h index cf70bd2..3b61c3c 100644 --- a/GPU_version/vio/include/vio/ukf.h +++ b/GPU_version/vio/include/vio/ukf.h @@ -125,20 +125,15 @@ public: filter_->predict(x,y,pitch,1e-9*time.toNSec()); lock=false; }; - std::pair,vio::SE2_5> UpdateSvo(double y/*in camera frame*/,double z/*in camera frame*/,double roll/*in camera frame*/,size_t match,ros::Time& time) { + std::pair,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/,size_t match,ros::Time& time) { while(lock)usleep(5); lock=true; - roll=roll+M_PI; - if(filter_->predict_up)filter_->correct(z,y,roll,match,1e-9*time.toNSec()); + //if(filter_->predict_up)filter_->correct(z,x,pitch,match,1e-9*time.toNSec()); lock=false; - double roll_f; - roll_f=filter_->state_(2)+M_PI; - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),roll_f)); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(1),filter_->state_(0),filter_->state_(2))); }; std::pair,vio::SE2_5> get_location(){ - double pitch_f; - pitch_f=filter_->state_(2)+M_PI; - return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f)); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(filter_->state_(1),filter_->state_(0),filter_->state_(2))); } private: Base* filter_= nullptr; diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index 2f25f60..402d7eb 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 aa98e56..01e210a 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -220,7 +220,7 @@ namespace vio { SE2 T(it->ftr->frame->T_f_w_.pitch()+frame->T_f_w_.pitch()+M_PI,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 d40723b..9b2063b 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -168,8 +168,8 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() // 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; + //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_, last_frame_,overlap_kfs_); diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index adb2c2b..38d47f5 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -31,7 +31,7 @@ Point::Point(const Vector3d& pos) : n_obs_(0), v_pt_(NULL), last_published_ts_(0), - last_projected_kf_id_(-1), + last_frame_overlap_id_(-10), type_(TYPE_UNKNOWN), n_failed_reproj_(0), n_succeeded_reproj_(0), @@ -45,7 +45,7 @@ Point::Point(const Vector3d& pos, Feature* ftr) : n_obs_(1), v_pt_(NULL), last_published_ts_(0), - last_projected_kf_id_(-1), + last_frame_overlap_id_(-10), type_(TYPE_UNKNOWN), n_failed_reproj_(0), n_succeeded_reproj_(0), diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 57c267b..c5772b3 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -92,9 +92,9 @@ void Reprojector::reprojectMap( { if((*it_ftr)->point == NULL) continue; - if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + if((*it_ftr)->point->last_frame_overlap_id_ == frame->id_) continue; - (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + (*it_ftr)->point->last_frame_overlap_id_= frame->id_; if(reprojectPoint(frame, (*it_ftr)->point)){ overlap_kfs.back().second++; } @@ -240,15 +240,15 @@ void Reprojector::reprojectMap( 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->last_projected_kf_id_ == it_frame.item.first->id_ || f.index > matches.size()-1)continue; - // f.item->point->last_projected_kf_id_ = it_frame.item.first->id_; + if(f.item->point==NULL)f.item->point=new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(),f.item->px.y())); + 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; 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); assert(grid_.cells.at(k)!= nullptr); - if(f.item->point==NULL)continue; Vector2d px((int)keypoints_cur.at(match.trainIdx).pt.x,(int)keypoints_cur.at(match.trainIdx).pt.y); grid_.cells.at(k)->push_back(Candidate(f.item->point,px)); overlap_kfs.back().second++; @@ -256,22 +256,6 @@ void Reprojector::reprojectMap( } } } - boost::unique_lock lock(map_.point_candidates_.mut_); - auto it=map_.point_candidates_.candidates_.begin(); - while(it!=map_.point_candidates_.candidates_.end()) - { - if(!reprojectPoint(frame, it->first)) - { - it->first->n_failed_reproj_ += 3; - if(it->first->n_failed_reproj_ > 30) - { - map_.point_candidates_.deleteCandidate(*it); - it = map_.point_candidates_.candidates_.erase(it); - continue; - } - } - ++it; - } for(size_t i=0; i