From fb6de808cdcffa61dc54833ea3eeb3af0c451670 Mon Sep 17 00:00:00 2001 From: gfuse Date: Mon, 30 Aug 2021 19:00:17 +0200 Subject: [PATCH] review with original version --- GPU_version/vio/kernel/compute-residual.cl | 27 ---- GPU_version/vio/src/depth_filter.cpp | 4 +- GPU_version/vio/src/feature_alignment.cpp | 12 +- GPU_version/vio/src/frame.cpp | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 2 +- GPU_version/vio/src/reprojector.cpp | 139 +++++++++------------ 6 files changed, 67 insertions(+), 119 deletions(-) diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl index b9d933b..2f25f60 100644 --- a/GPU_version/vio/kernel/compute-residual.cl +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -34,33 +34,6 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) return (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x, R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z, R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y); -/* - if(ref.z < 0.0) - ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z; - else - ref.z = ref.z - 3.1415926535897932384626433832795028841971693993; - float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z); - float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z); - - return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z), - ref_feature.y, - -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z)); - - // alfa = 0, betha = 15 degree - 0.261799 radian - //return (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.y)+(float)(sin(0.261799)*cos(cur.z)*ref_feature.z)), - // ((float)(cos(cur.z)*ref_feature.y)-(float)(sin(cur.z)*ref_feature.z)), - // ((float)(-1*sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*sin(cur.z)*ref_feature.y)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.z))); - // - // betha = 0, alfa = 15 degree - 0.261799 radian - //return (float3)(((float)(cos(0.261799)*ref_feature.x)+(float)(-1*sin(0.261799)*cos(cur.z)*ref_feature.y)+(float)(sin(0.261799)*sin(cur.z)*ref_feature.z)), - // ((float)(sin(0.261799)*ref_feature.x)+(float)(cos(0.261799)*cos(cur.z)*ref_feature.y)-(float)(cos(0.261799)*sin(cur.z)*ref_feature.z)), - // ((float)(sin(cur.z)*ref_feature.y)+(float)(cos(cur.z)*ref_feature.z))); - } - - - return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z), - ref_feature.y, - -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));*/ } void jacobian_xyz2uv(float3 xyz_in_f, float* J) diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index cda056d..aa98e56 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -190,6 +190,8 @@ namespace vio { } } updateSeeds(frame); + if(frame->isKeyframe()) + initializeSeeds(frame); } } @@ -215,7 +217,7 @@ namespace vio { continue; } // check if point is visible in the current image - 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()); + 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()) diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 0d8b455..34ad66c 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -36,9 +36,9 @@ bool align1D( Vector2d& cur_px_estimate, double& h_inv) { - const int halfpatch_size_ = 1; - const int patch_size = 2; - const int patch_area = 4; + const int halfpatch_size_ = 4; + const int patch_size = 8; + const int patch_area = 64; bool converged=false; // compute derivative of template and prepare inverse compositional @@ -163,9 +163,9 @@ bool align2D( return align2D_NEON(cur_img, ref_patch_with_border, ref_patch, n_iter, cur_px_estimate); #endif - const int halfpatch_size_ = 1; - const int patch_size_ = 2; - const int patch_area_ = 4; + const int halfpatch_size_ = 4; + const int patch_size_ = 8; + const int patch_area_ = 64; bool converged=false; // compute derivative of template and prepare inverse compositional diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp index a9438d5..0a0b7ea 100644 --- a/GPU_version/vio/src/frame.cpp +++ b/GPU_version/vio/src/frame.cpp @@ -138,7 +138,7 @@ void Frame::removeKeyPoint(Feature* ftr) bool Frame::isVisible(const Vector3d& xyz_w) const { - Vector3d xyz_f = Vector3d(this->getSE3Inv()*xyz_w); + Vector3d xyz_f = Vector3d(this->se3()*xyz_w); if(xyz_f.z() < 0.0) return false; // point is behind the camera Vector2d px = f2c(xyz_f); diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 86ac4d2..a67f222 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -171,7 +171,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() 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_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_); reprojector_.reprojectMap2(new_frame_, overlap_kfs_); //assert (false); diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index b55c11a..647f863 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -70,55 +70,63 @@ void Reprojector::reprojectMap( FramePtr frame, std::vector< std::pair >& overlap_kfs) { - resetGrid(); - list< pair > close_kfs; - map_.getCloseKeyframes(frame, close_kfs); - close_kfs.sort(boost::bind(&std::pair::second, _1) < - boost::bind(&std::pair::second, _2)); - size_t n = 0; - overlap_kfs.reserve(options_.max_n_kfs); - for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); - it_frame!=ite_frame && nfirst; - overlap_kfs.push_back(pair(ref_frame,0)); - for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); - it_ftr!=ite_ftr; ++it_ftr) - { - if((*it_ftr)->point == NULL) - continue; - if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) - continue; - (*it_ftr)->point->last_projected_kf_id_ = frame->id_; - if(reprojectPoint(frame, (*it_ftr)->point)){ - overlap_kfs.back().second++; - } - } - } - 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) + resetGrid(); + + list< pair > close_kfs; + map_.getCloseKeyframes(frame, close_kfs); + + close_kfs.sort(boost::bind(&std::pair::second, _1) < + boost::bind(&std::pair::second, _2)); + + size_t n = 0; + overlap_kfs.reserve(options_.max_n_kfs); + for(auto it_frame=close_kfs.begin(), ite_frame=close_kfs.end(); + it_frame!=ite_frame && nfirst; + overlap_kfs.push_back(pair(ref_frame,0)); + + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); + it_ftr!=ite_ftr; ++it_ftr) + { + if((*it_ftr)->point == NULL) + continue; + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) + continue; + (*it_ftr)->point->last_projected_kf_id_ = frame->id_; + if(reprojectPoint(frame, (*it_ftr)->point)){ + overlap_kfs.back().second++; + } + + } + } + { + 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 (size_t) Config::maxFts()) + break; } - } - ++it; - } - for(size_t i=0; i (size_t) Config::maxFts()) - break; - } } + //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// // TODO - First reimplementation void Reprojector::reprojectMap1( @@ -214,11 +222,6 @@ void Reprojector::reprojectMap( extractor->compute(frame->img(), keypoints_cur, descriptors_cur); list< pair > close_kfs; map_.getCloseKeyframes(frame, close_kfs); -<<<<<<< HEAD - - // Sort KFs with overlap according to their closeness -======= ->>>>>>> internal/dev2 close_kfs.sort(boost::bind(&std::pair::second, _1) < boost::bind(&std::pair::second, _2)); size_t n = 0; @@ -228,20 +231,14 @@ void Reprojector::reprojectMap( { std::vector keypoints_kfs; FramePtr ref_frame = it_frame->first; -<<<<<<< HEAD - // Try to reproject each mappoint that the other KF observes -======= overlap_kfs.push_back(pair(ref_frame,0)); ->>>>>>> internal/dev2 + for(auto it_ftr=ref_frame->fts_.begin(), ite_ftr=ref_frame->fts_.end(); it_ftr!=ite_ftr; ++it_ftr) { if((*it_ftr)->point == NULL) continue; -<<<<<<< HEAD - // make sure we project a point only once -======= ->>>>>>> internal/dev2 + if((*it_ftr)->point->last_projected_kf_id_ == frame->id_) continue; (*it_ftr)->point->last_projected_kf_id_ = frame->id_; @@ -250,26 +247,7 @@ void Reprojector::reprojectMap( kp->pt.y = (*it_ftr)->px.y(); keypoints_kfs.push_back(*kp); } -<<<<<<< HEAD - keypoints_ref.push_back(keypoints_kfs); - } - //SVO_STOP_TIMER("reproject_kfs"); - - cv::Mat descriptors_ref, descriptors_cur; - std::vector keypoints_cur; - cv::FAST(frame->img(),keypoints_cur,50); - std::vector matches; - cv::Ptr extractor = cv::xfeatures2d::FREAK::create(true, true, 50.0f, 4); - cv::Ptr matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true); - int ii = 0; - extractor->compute(frame->img(), keypoints_cur, descriptors_cur); - for(auto it_frame = close_kfs.begin(), ite_frame = close_kfs.end(); - it_frame != ite_frame; ++it_frame, ++ii) - { - extractor->compute(it_frame->first->img(), keypoints_ref[ii], descriptors_ref); -======= extractor->compute(it_frame->first->img(), keypoints_kfs, descriptors_ref); ->>>>>>> internal/dev2 matcher->match(descriptors_ref, descriptors_cur, matches); //std::cerr<point, keypoints_cur[i].pt.x, keypoints_cur[i].pt.y)) { overlap_kfs.back().second++; } -<<<<<<< HEAD - //keypoints_cur[i].class_id = 0; - -======= ->>>>>>> internal/dev2 } } boost::unique_lock lock(map_.point_candidates_.mut_);