From 99fa000ad55f5178d9f6d45956de41aadb4876a9 Mon Sep 17 00:00:00 2001 From: gfuse Date: Sat, 10 Jul 2021 13:35:58 +0200 Subject: [PATCH] Edit EKF and VO pipline to publish the odometry as soon as algorithm initialized. And VO will not save those frames that do not have features. --- GPU_version/vio/include/gpu_svo/frame.h | 27 ++----- .../vio/include/gpu_svo/frame_handler_mono.h | 2 +- GPU_version/vio/include/gpu_svo/global.h | 19 ++++- GPU_version/vio/include/gpu_svo/matcher.h | 2 +- GPU_version/vio/include/gpu_svo/ukf.h | 25 ++++-- GPU_version/vio/include/gpu_svo/vision.h | 5 +- GPU_version/vio/include/gpu_svo/visualizer.h | 12 +-- GPU_version/vio/param/vo_fast.yaml | 8 +- GPU_version/vio/src/depth_filter.cpp | 2 +- GPU_version/vio/src/feature_alignment.cpp | 1 - GPU_version/vio/src/frame_handler_mono.cpp | 29 ++++--- GPU_version/vio/src/initialization.cpp | 6 +- GPU_version/vio/src/map.cpp | 2 +- GPU_version/vio/src/matcher.cpp | 76 +++++++++---------- GPU_version/vio/src/point.cpp | 14 ++-- GPU_version/vio/src/pose_optimizer.cpp | 4 +- GPU_version/vio/src/reprojector.cpp | 1 - GPU_version/vio/src/sparse_img_align.cpp | 8 +- GPU_version/vio/src/visualizer.cpp | 53 +++---------- GPU_version/vio/src/vo_node.cpp | 4 +- 20 files changed, 134 insertions(+), 166 deletions(-) diff --git a/GPU_version/vio/include/gpu_svo/frame.h b/GPU_version/vio/include/gpu_svo/frame.h index e02f1c6..3d1fd72 100644 --- a/GPU_version/vio/include/gpu_svo/frame.h +++ b/GPU_version/vio/include/gpu_svo/frame.h @@ -93,11 +93,7 @@ public: /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). inline Vector2d w2c(const Vector3d& xyz_w) const { - Eigen::Matrix R; - R<world2cam( SE3(R,Vector3d(T_f_w_.T2_.translation()(0),1e-19,T_f_w_.T2_.translation()(1)))*xyz_w); + return cam_->world2cam( T_f_w_.se3()*xyz_w); } /// Transforms point coordinates in world-frame (w) to camera pixel coordinates (c). inline Vector2d w2px(const Vector3d& xyz_w) const { return cam_->world2cam( xyz_w ); } @@ -110,35 +106,22 @@ public: /// Transforms point coordinates in world-frame (w) to camera-frams (f). inline Vector3d w2f(const Vector3d& xyz_w) const { - Eigen::Matrix R; - R< R; - R<world2cam( f ); } /// Return the pose of the frame in the (w)orld coordinate frame. - inline Vector2d pos() const { return T_f_w_.T2_.translation(); } + inline Vector2d pos() const { return T_f_w_.se2().translation(); } inline SE3 se3() const{ - Eigen::Matrix R; - R< imu_integPtr_; //Integration of IMU void UpdateIMU(double* value,const ros::Time& time); void UpdateCmd(double* value,const ros::Time& time); + UKF ukfPtr_; protected: vk::AbstractCamera* cam_; //!< Camera model, can be ATAN, Pinhole or Ocam (see vikit). @@ -76,7 +77,6 @@ protected: DepthFilter* depth_filter_; //!< Depth estimation algorithm runs in a parallel thread and is used to initialize new 3D points. opencl* gpu_fast_; ros::Time time_; - UKF ukfPtr_; /// Initialize the visual odometry algorithm. virtual void initialize(); diff --git a/GPU_version/vio/include/gpu_svo/global.h b/GPU_version/vio/include/gpu_svo/global.h index e522a69..60d0f55 100644 --- a/GPU_version/vio/include/gpu_svo/global.h +++ b/GPU_version/vio/include/gpu_svo/global.h @@ -52,7 +52,6 @@ namespace svo const double PI = 3.14159265; class SE2_5: public SE2{ public: - SE2 T2_; SE2_5(SE2&& se2):T2_(se2){ } SE2_5(SE2& se2):T2_(se2){ @@ -87,17 +86,31 @@ namespace svo -sin(pitch),cos(pitch); T2_=SE2(R,Vector2d(x,z)); }; - SE2 getSE2() const{ + SE2 se2() const{ return T2_; } SE2 inverse() const{ double yaw=atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0)); - return SE2(yaw+PI,T2_.translation()); + if(yaw<0.0) + yaw=M_PI-yaw; + else + yaw=yaw-M_PI; + return SE2(yaw,-1.0*T2_.translation()); } double pitch()const{ return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0)); } + SE3 se3() const{ + Eigen::Matrix R; + R<M_PI_4)state_(2)-=M_PI; - if(fabs(state_(2))>2*M_PI)state_(2)-=2.0*int(0.5*state_(2)/M_PI)*M_PI; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; state_h_(2)=state_(2)+dt*dpitch; state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))); @@ -90,8 +88,6 @@ public: E<M_PI_4)state_(2)-=M_PI; - if(fabs(state_(2))>2*M_PI)state_(2)-=2.0*int(0.5*state_(2)/M_PI)*M_PI; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; yaw_t_1_=state_(2); predict_up=false; @@ -131,10 +127,27 @@ public: std::pair,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { while(lock)usleep(5); lock=true; - if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec()); + if(pitch<0.0) + pitch=M_PI-pitch; + else + pitch=pitch-M_PI; + if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec()); lock=false; - return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(filter_->state_(0),filter_->state_(1),filter_->state_(2))); + double pitch_f; + if(filter_->state_(0)<0.0) + pitch_f=M_PI-filter_->state_(2); + else + pitch_f=filter_->state_(2)-M_PI; + return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); }; + std::pair,svo::SE2_5> get_location(){ + double pitch_f; + if(filter_->state_(0)<0.0) + pitch_f=M_PI-filter_->state_(2); + else + pitch_f=filter_->state_(2)-M_PI; + return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + } private: Base* filter_= nullptr; bool lock=false; diff --git a/GPU_version/vio/include/gpu_svo/vision.h b/GPU_version/vio/include/gpu_svo/vision.h index 1533775..3b95183 100644 --- a/GPU_version/vio/include/gpu_svo/vision.h +++ b/GPU_version/vio/include/gpu_svo/vision.h @@ -42,7 +42,8 @@ interpolateMat_32f(const cv::Mat& mat, float u, float v) inline float interpolateMat_8u(const cv::Mat& mat, float u, float v) { - assert(mat.type()==CV_8U); + if(mat.empty())return 0.0; + if(mat.type()!=CV_8U)return 0.0; int x = floor(u); int y = floor(v); float subpix_x = u-x; @@ -55,6 +56,8 @@ interpolateMat_8u(const cv::Mat& mat, float u, float v) const int stride = mat.step.p[0]; unsigned char* ptr = mat.data + y*stride + x; + if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0; + if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0; return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; } diff --git a/GPU_version/vio/include/gpu_svo/visualizer.h b/GPU_version/vio/include/gpu_svo/visualizer.h index 7c09cd1..819885d 100644 --- a/GPU_version/vio/include/gpu_svo/visualizer.h +++ b/GPU_version/vio/include/gpu_svo/visualizer.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace svo { @@ -54,18 +55,9 @@ public: ~Visualizer() {}; void publishMinimal( - const cv::Mat& img, - const FramePtr& frame, - const FrameHandlerMono& slam, + UKF& ukf, const double timestamp); - void visualizeMarkers( - const FramePtr& frame, - const set& core_kfs, - const Map& map); - - void removeDeletedPts(const Map& map); - }; } // end namespace svo diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index f0c5dd7..1ecb630 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -7,9 +7,9 @@ svo: init_min_tracked: 100 init_min_disparity: 30 structureoptim_max_pts: 50 - kfselect_mindist: 0.001 + kfselect_mindist: 0.01 poseoptim_thresh: 4.0 - ACC_noise: 0.001 #acc white noise in continuous in EKF + ACC_noise: 0.5 #acc white noise in continuous in EKF GYO_noise: 0.001 #gyro white noise in continuous in EKF - svo_ekf: 100 #Svo noise that will be devided by number of Reprojected points after opmization in EKF - Cmd_Cov: 0.001 #control vector noise in EKF + svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF + Cmd_Cov: 1.0 #control vector noise in EKF diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index cbf0f33..ba653f2 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -233,7 +233,7 @@ void DepthFilter::updateSeeds(FramePtr frame) continue; } // check if point is visible in the current image - SE2 T(it->ftr->frame->T_f_w_.getSE2() * frame->T_f_w_.inverse()); + SE2 T(it->ftr->frame->T_f_w_.se2() * frame->T_f_w_.inverse()); Eigen::Matrix R; R<= cur_img.cols-halfpatch_size_ || v_r >= cur_img.rows-halfpatch_size_){ - std::cout<<"not in bach size\n"; break; } diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 5f172ab..cc92553 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -89,7 +89,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const map_.getClosestKeyframe(last_frame_)); // set last frame - last_frame_ = new_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()); @@ -103,16 +103,18 @@ FrameHandlerMono::UpdateResult FrameHandlerMono::processFirstFrame() new_frame_->setKeyframe(); map_.addKeyframe(new_frame_); stage_ = STAGE_SECOND_FRAME; - ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0),new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); + ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); SVO_INFO_STREAM("Init: Selected first frame."); return RESULT_IS_KEYFRAME; } FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() { - new_frame_->T_f_w_ = last_frame_->T_f_w_; + auto init_f= ukfPtr_.get_location(); + new_frame_->T_f_w_=init_f.second; + new_frame_->Cov_ = init_f.first; initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); - auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0),new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); + auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0),new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_); new_frame_->T_f_w_ = result.second; new_frame_->Cov_ = result.first; if(res == initialization::NO_KEYFRAME) @@ -133,28 +135,31 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() { - new_frame_->T_f_w_ = last_frame_->T_f_w_; + auto init_f= ukfPtr_.get_location(); + 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); - img_align.run(last_frame_, new_frame_); + if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; reprojector_.reprojectMap(new_frame_, overlap_kfs_); size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; pose_optimizer::optimizeGaussNewton( Config::poseOptimThresh(), Config::poseOptimNumIter(), false, new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); - if((last_frame_->T_f_w_.getSE2().translation()-new_frame_->T_f_w_.getSE2().translation()).norm()>0.1){ - new_frame_->T_f_w_ = last_frame_->T_f_w_; + if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>2.0 || fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){ + new_frame_=last_frame_; + return RESULT_NO_KEYFRAME; } - auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0), - new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_); + auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0), + new_frame_->T_f_w_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_); new_frame_->T_f_w_ =result.second; new_frame_->Cov_ = result.first; std::cout<<"Reprojection Map nPoint: "<T_f_w_.getSE2().translation()-new_frame_->T_f_w_.getSE2().translation()).norm()<<'\n'; + (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ return RESULT_NO_KEYFRAME; } @@ -165,7 +170,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() setTrackingQuality(sfba_n_edges_final); if(tracking_quality_ == TRACKING_INSUFFICIENT) { - return RESULT_FAILURE; + return RESULT_FAILURE; } double depth_mean, depth_min; diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index 1ca4ced..379163f 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -69,9 +69,9 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur) depth_vec.push_back((xyz_in_cur_[i]).z()); double scene_depth_median = vk::getMedian(depth_vec); double scale = Config::mapScale()/scene_depth_median; - frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.getSE2() * frame_ref_->T_f_w_.getSE2()); - frame_cur->T_f_w_.T2_.translation() = -frame_cur->T_f_w_.getSE2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); - //frame_cur->T_f_w_.T2_.translation() = frame_cur->T_f_w_.getSE2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())); + frame_cur->T_f_w_ = SE2_5(T_cur_from_ref_.se2() * frame_ref_->T_f_w_.se2()); + frame_cur->T_f_w_ = SE2_5(SE2(frame_cur->T_f_w_.se2().rotation_matrix(), + -frame_cur->T_f_w_.se2().rotation_matrix()*(frame_ref_->pos() + scale*(frame_cur->pos() - frame_ref_->pos())))); // For each inlier create 3D point and add feature in both frames SE3 T_world_cur = frame_cur->getSE3Inv(); for(vector::iterator it=inliers_.begin(); it!=inliers_.end(); ++it) diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp index c6eb05b..3d0cace 100644 --- a/GPU_version/vio/src/map.cpp +++ b/GPU_version/vio/src/map.cpp @@ -118,7 +118,7 @@ void Map::getCloseKeyframes( { close_kfs.push_back( std::make_pair( - kf, (frame->T_f_w_.getSE2().translation()-kf->T_f_w_.getSE2().translation()).norm())); + kf, (frame->T_f_w_.se2().translation()-kf->T_f_w_.se2().translation()).norm())); break; // this keyframe has an overlapping field of view -> add to close_kfs } } diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 3e6d47b..767d542 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -69,7 +69,7 @@ int getBestSearchLevel( return search_level; } -void warpAffine( +bool warpAffine( const Matrix2d& A_cur_ref, const cv::Mat& img_ref, const Vector2d& px_ref, @@ -78,14 +78,14 @@ void warpAffine( const int halfpatch_size, uint8_t* patch) { + if(patch==NULL || img_ref.empty())return false; const int patch_size = halfpatch_size*2 ; const Matrix2f A_ref_cur = A_cur_ref.inverse().cast(); if(isnan(A_ref_cur(0,0))) { printf("Affine warp is NaN, probably camera has no translation\n"); // TODO - return; + return false; } - // Perform the warp on a larger patch. uint8_t* patch_ptr = patch; const Vector2f px_ref_pyr = px_ref.cast() / (1<=img_ref.cols-1 || px[1]>=img_ref.rows-1) - *patch_ptr = 0; - else - *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]); + if(patch_ptr == nullptr)continue; + if (px[0]<1 || px[1]<1 || px[0]>=img_ref.cols-1 || px[1]>=img_ref.rows-1){ + *patch_ptr = 0; + }else{ + *patch_ptr = (uint8_t) vk::interpolateMat_8u(img_ref, px[0], px[1]); + } } } + return true; } } // namespace warp @@ -117,7 +120,7 @@ bool depthFromTriangulation( if(AtA.determinant() < 0.000001) return false; const Vector2d depth2 = - AtA.inverse()*A.transpose()*T_search_ref.translation(); - depth = fabs(depth2[0]); + depth = fabs(depth2[0]/depth2[1]); return true; } @@ -142,35 +145,36 @@ bool Matcher::findMatchDirect( if(!ref_ftr_->frame->cam_->isInFrame( ref_ftr_->px.cast()/(1<level), halfpatch_size_+2, ref_ftr_->level)) return false; - + if(ref_ftr_->frame->img_pyr_.empty())return false; + if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false; // warp affine warp::getWarpMatrixAffine( *ref_ftr_->frame->cam_, *cur_frame.cam_, ref_ftr_->px, ref_ftr_->f, (Vector3d(ref_ftr_->frame->pos()(0),0.0,ref_ftr_->frame->pos()(1)) - pt.pos_).norm(), cur_frame.se3() * ref_ftr_->frame->getSE3Inv(), ref_ftr_->level, A_cur_ref_); search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); - warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, - ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_); + if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, + ref_ftr_->level, search_level_, halfpatch_size_+1, patch_with_border_))return false; createPatchFromPatchWithBorder(); // px_cur should be set Vector2d px_scaled(px_cur/(1<type == Feature::EDGELET) - { - Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); - dir_cur.normalize(); - success = feature_alignment::align1D( - cur_frame.img_pyr_[search_level_], dir_cur.cast(), - patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); - } - else - { + // if(ref_ftr_->type == Feature::EDGELET) + //{ + // Vector2d dir_cur(A_cur_ref_*ref_ftr_->grad); + // dir_cur.normalize(); + // success = feature_alignment::align1D( + // cur_frame.img_pyr_[search_level_], dir_cur.cast(), + // patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_); + //} + //else + //{ success = feature_alignment::align2D( cur_frame.img_pyr_[search_level_], patch_with_border_, patch_, options_.align_max_iter, px_scaled); - } + // } px_cur = px_scaled * (1< R; - R<cam2world(px_cur_), depth)) + if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth)) return true; } return false; @@ -314,13 +312,13 @@ bool Matcher::findEpipolarMatchDirect( if(res) { px_cur_ = px_scaled*(1<cam2world(px_cur_), depth)) + if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth)) return true; } return false; } px_cur_ = cur_frame.cam_->world2cam(uv_best); - if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) + if(depthFromTriangulation(T_cur_ref.se3(), ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth)) return true; } return false; diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index aee4bd8..cf12a1e 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -99,21 +99,19 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const { // TODO: get frame with same point of view AND same pyramid level! Vector3d obs_dir(Vector3d(framepos(0),0.0,framepos(1)) - pos_); obs_dir.normalize(); - auto min_it=obs_.begin(); - double min_cos_angle = 0; + auto max_it=obs_.begin(); + double max_cos_angle = 1; for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it) { Vector3d dir(Vector3d((*it)->frame->pos()(0),0.0,(*it)->frame->pos()(1)) - pos_); dir.normalize(); double cos_angle = obs_dir.dot(dir); - if(cos_angle > min_cos_angle) + if(cos_angle < max_cos_angle) { - min_cos_angle = cos_angle; - min_it = it; + max_cos_angle = cos_angle; + max_it = it; } } - ftr = *min_it; - if(min_cos_angle < 0.5) // assume that observations larger than 60° are useless - return false; + ftr = *max_it; return true; } diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp index 79e4686..3d45ab5 100644 --- a/GPU_version/vio/src/pose_optimizer.cpp +++ b/GPU_version/vio/src/pose_optimizer.cpp @@ -121,9 +121,9 @@ void optimizeGaussNewton( // update the model T_old = frame->T_f_w_; if(direction){ - frame->T_f_w_=SE2_5(SE2::exp(dT)*(T_old.getSE2())); + frame->T_f_w_=SE2_5(SE2::exp(dT)*(T_old.se2())); }else{ - frame->T_f_w_=SE2_5(SE2::exp(-1.0*dT)*(T_old.getSE2())); + frame->T_f_w_=SE2_5(SE2::exp(-1.0*dT)*(T_old.se2())); } chi2 = new_chi2; if(verbose) diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 7a598b6..897ce59 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -72,7 +72,6 @@ void Reprojector::reprojectMap( list< pair > close_kfs; map_.getCloseKeyframes(frame, close_kfs); - std::cout<<"The number of closed frames: "<::second, _1) < boost::bind(&std::pair::second, _2)); diff --git a/GPU_version/vio/src/sparse_img_align.cpp b/GPU_version/vio/src/sparse_img_align.cpp index 533c776..59c754c 100644 --- a/GPU_version/vio/src/sparse_img_align.cpp +++ b/GPU_version/vio/src/sparse_img_align.cpp @@ -46,7 +46,6 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) if(ref_frame->fts_.empty()) { - SVO_WARN_STREAM("SparseImgAlign: no features to track!"); return 0; } @@ -54,8 +53,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) cur_frame_ = cur_frame; ref_patch_cache_ = cv::Mat(ref_frame_->fts_.size(), patch_area_, CV_32F); jacobian_cache_.resize(Eigen::NoChange, ref_patch_cache_.rows*patch_area_); - //visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? - SE2 T_cur_from_ref(cur_frame_->T_f_w_.getSE2() * ref_frame_->T_f_w_.inverse()); + SE2 T_cur_from_ref(cur_frame_->T_f_w_.se2() * ref_frame_->T_f_w_.inverse()); for(level_=max_level_; level_>=min_level_; --level_) { visible_fts_.resize(ref_patch_cache_.rows, false); // TODO: should it be reset at each level? @@ -66,7 +64,7 @@ size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) printf("\nPYRAMID LEVEL %i\n---------------\n", level_); optimize(T_cur_from_ref); } - cur_frame_->T_f_w_ = SE2_5(T_cur_from_ref * ref_frame_->T_f_w_.getSE2()); + cur_frame_->T_f_w_ = SE2_5(T_cur_from_ref * ref_frame_->T_f_w_.se2()); return n_meas_/patch_area_; } @@ -251,7 +249,7 @@ void SparseImgAlign::update( const ModelType& T_curold_from_ref, ModelType& T_curnew_from_ref) { - T_curnew_from_ref = T_curold_from_ref * SE2::exp(-1.0*x_); + T_curnew_from_ref = T_curold_from_ref * SE2::exp(-0.25*x_); } void SparseImgAlign::startIteration() diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 95b3353..9e8808d 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -24,6 +24,7 @@ #include #include #include +#include namespace svo { @@ -40,9 +41,7 @@ Visualizer() : } void Visualizer::publishMinimal( - const cv::Mat& img, - const FramePtr& frame, - const FrameHandlerMono& slam, + UKF& ukf, const double timestamp) { ++trace_id_; @@ -50,63 +49,33 @@ void Visualizer::publishMinimal( header_msg.frame_id = "/world"; header_msg.seq = trace_id_; header_msg.stamp = ros::Time(timestamp); - if(pub_pose_with_cov_.getNumSubscribers() > 0 && slam.stage() == FrameHandlerBase::STAGE_DEFAULT_FRAME) + if(pub_pose_with_cov_.getNumSubscribers() > 0) { // publish cam in world frame (Estimated odometry in the worls frame) - Quaterniond q(AngleAxisd(frame->T_f_w_.pitch(), Vector3d::UnitZ())); + auto odom=ukf.get_location(); + Quaterniond q(AngleAxisd(odom.second.pitch()+M_PI_2, Vector3d::UnitZ())); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); msg_pose_with_cov->header = header_msg; - msg_pose_with_cov->pose.pose.position.x = frame->se3().translation().x(); - msg_pose_with_cov->pose.pose.position.y = frame->se3().translation().z(); + msg_pose_with_cov->pose.pose.position.x = -odom.second.se2().translation()(0); + msg_pose_with_cov->pose.pose.position.y = -odom.second.se2().translation()(1); msg_pose_with_cov->pose.pose.position.z = 0.0; msg_pose_with_cov->pose.pose.orientation.x = q.x(); msg_pose_with_cov->pose.pose.orientation.y = q.y(); msg_pose_with_cov->pose.pose.orientation.z = q.z(); msg_pose_with_cov->pose.pose.orientation.w = q.w(); - msg_pose_with_cov->pose.covariance = {frame->Cov_(0,0),frame->Cov_(0,1),0.0,0.0,frame->Cov_(0,2),0.0, - frame->Cov_(1,0),frame->Cov_(1,1),0.0,0.0,frame->Cov_(1,2),0.0, + msg_pose_with_cov->pose.covariance = {odom.first(0,0),odom.first(0,1),0.0,0.0,odom.first(0,2),0.0, + odom.first(1,0),odom.first(1,1),0.0,0.0,odom.first(1,2),0.0, 0.0,0.0,0.0000001,0.0,0.0,0.0, 0.0,0.0,0.0,0.0000001,0.0,0.0, - frame->Cov_(2,0),frame->Cov_(2,1),0.0,0.0,frame->Cov_(2,2),0.0, + odom.first(2,0),odom.first(2,1),0.0,0.0,odom.first(2,2),0.0, 0.0,0.0,0.0,0.0,0.0,0.0000001}; pub_pose_with_cov_.publish(msg_pose_with_cov); if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ publishPointMarker( - pub_points_, Eigen::Vector3d(frame->se3().translation().x(),frame->se3().translation().z(),0.0), "trajectory", + pub_points_, Eigen::Vector3d(-odom.second.se2().translation()(0),-odom.second.se2().translation()(1),0.0), "trajectory", ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5)); } } } - -void Visualizer::visualizeMarkers( - const FramePtr& frame, - const set& core_kfs, - const Map& map) -{ - if(frame == NULL) - return; - - publishTfTransform( - frame->se3()*T_world_from_vision_.inverse(), - ros::Time(frame->timestamp_), "cam_pos", "world", br_); - - if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0) - { - publishPointMarker( - pub_points_, T_world_from_vision_*Vector3d(frame->pos()(0),0.0,frame->pos()(1)), "trajectory", - ros::Time::now(), trace_id_, 0, 0.006, Vector3d(0.,0.,0.5)); - removeDeletedPts(map); - } -} - -void Visualizer::removeDeletedPts(const Map& map) -{ - if(pub_points_.getNumSubscribers() > 0) - { - for(list::const_iterator it=map.trash_points_.begin(); it!=map.trash_points_.end(); ++it) - publishPointMarker(pub_points_, Vector3d(), "pts", ros::Time::now(), (*it)->id_, 2, 0.006, Vector3d()); - } -} - } // end namespace gpu_svo diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 8fe3f84..dfae0ab 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -108,9 +108,6 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) auto start=std::chrono::steady_clock::now(); cv::Mat img=cv_bridge::toCvShare(msg, "mono8")->image; vo_->addImage(img, msg->header.stamp.toSec(),msg->header.stamp); - visualizer_.publishMinimal(img, vo_->lastFrame(), *vo_, msg->header.stamp.toSec()); - if(vo_->stage() == svo::FrameHandlerMono::STAGE_PAUSED) - usleep(100000); #if VIO_DEBUG fprintf(log_, ",,,,,,,%s,%f,%f,%f\n",std::to_string(std::chrono::duration(std::chrono::steady_clock::now()-start).count()).c_str(),vo_->lastFrame()->T_f_w_.translation()(0), vo_->lastFrame()->T_f_w_.translation()(1), @@ -158,6 +155,7 @@ void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { #endif vo_->UpdateIMU(imu_in,imu->header.stamp); imu_time_=imu->header.stamp; + visualizer_.publishMinimal(vo_->ukfPtr_, imu->header.stamp.toSec()); } void VoNode::cmdCb(const geometry_msgs::TwistPtr &cmd) { double _cmd[3]={cmd->linear.x,cmd->linear.y,cmd->angular.z};