From 2545349542867f91de54d2c5f6220d0ed2e36d42 Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 25 Aug 2021 23:00:53 +0200 Subject: [PATCH] edit frame and change atan to atan2 --- GPU_version/vio/include/vio/global.h | 4 +-- GPU_version/vio/include/vio/ukf.h | 15 +++-------- GPU_version/vio/param/vo_fast.yaml | 4 +-- GPU_version/vio/src/depth_filter.cpp | 2 +- GPU_version/vio/src/feature_alignment.cpp | 2 +- GPU_version/vio/src/frame_handler_base.cpp | 2 +- GPU_version/vio/src/frame_handler_mono.cpp | 26 +++++++++----------- GPU_version/vio/src/sparse_img_align_gpu.cpp | 9 +++---- GPU_version/vio/src/visualizer.cpp | 6 ++--- GPU_version/vio/src/vo_node.cpp | 8 +++--- 10 files changed, 32 insertions(+), 46 deletions(-) diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index 69fae99..d3adc3c 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -90,7 +90,7 @@ namespace vio return T2_; } SE2 inverse() const{ - double yaw=atan(T2_.so2().unit_complex().imag()/T2_.so2().unit_complex().real()); + double yaw=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real()); if(yaw<0.0) yaw=M_PI-yaw; else @@ -98,7 +98,7 @@ namespace vio return SE2(yaw,-1.0*T2_.translation()); } double pitch()const{ - return atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0)); + return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0)); } SE3 se3() const{ Eigen::Matrix R; diff --git a/GPU_version/vio/include/vio/ukf.h b/GPU_version/vio/include/vio/ukf.h index 66b16b3..066e25c 100644 --- a/GPU_version/vio/include/vio/ukf.h +++ b/GPU_version/vio/include/vio/ukf.h @@ -127,25 +127,16 @@ public: std::pair,vio::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { while(lock)usleep(5); lock=true; - if(pitch<0.0) - pitch=M_PI-pitch; - else - pitch=pitch-M_PI; + pitch=pitch+M_PI; if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec()); lock=false; double pitch_f; - if(filter_->state_(0)<0.0) - pitch_f=M_PI-filter_->state_(2); - else - pitch_f=filter_->state_(2)-M_PI; + pitch_f=filter_->state_(2)+M_PI; return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); }; std::pair,vio::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; + pitch_f=filter_->state_(2)+M_PI; return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); } private: diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index 0899a28..04d1559 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -5,12 +5,12 @@ svo: quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. init_min_tracked: 100 - init_min_disparity: 40 + init_min_disparity: 60 structureoptim_max_pts: 50 kfselect_mindist: 0.01 poseoptim_thresh: 4.0 init_min_inliers: 30 ACC_noise: 0.5 #acc white noise in continuous in EKF GYO_noise: 0.001 #gyro white noise in continuous in EKF - svo_ekf: 1.5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF + svo_ekf: 1.0 #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 45abf92..a02b47a 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -217,7 +217,7 @@ void DepthFilter::updateSeeds(FramePtr frame) const double focal_length = frame->cam_->errorMultiplier2(); double px_noise = 1.0; - double px_error_angle = atan(px_noise/(2.0*focal_length))*2.0; // law of chord (sehnensatz) + double px_error_angle = atan2(px_noise,(2.0*focal_length))*2.0; // law of chord (sehnensatz) while( it!=seeds_.end()) { // set this value true when seeds updating should be interrupted diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 0d8b455..f46d185 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -202,7 +202,7 @@ bool align2D( bool sign=true;// + // termination condition - const float min_update_squared = 0.05;//0.001 + const float min_update_squared = 0.5;//0.001 const int cur_step = cur_img.step.p[0]; // float chi2 = 0; Vector3f update; update.setZero(); diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp index 9f41cbc..e697bd9 100644 --- a/GPU_version/vio/src/frame_handler_base.cpp +++ b/GPU_version/vio/src/frame_handler_base.cpp @@ -77,7 +77,7 @@ int FrameHandlerBase::finishFrameProcessingCommon( if(dropout == RESULT_FAILURE && (stage_ == STAGE_DEFAULT_FRAME || stage_ == STAGE_RELOCALIZING )) { - stage_ = STAGE_RELOCALIZING; + //stage_ = STAGE_RELOCALIZING; tracking_quality_ = TRACKING_INSUFFICIENT; } if(set_reset_) diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index ff44ab1..02ad775 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -170,34 +170,31 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false); 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; - //assert (false); + if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 || + fabs(last_frame_->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>M_PI){ + return RESULT_FAILURE; + } reprojector_.reprojectMap(new_frame_, overlap_kfs_); -/* + /* std::cout<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/ + if( reprojector_.n_matches_ < 5)return RESULT_FAILURE; 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_.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 || - sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){ - new_frame_=last_frame_; - return RESULT_NO_KEYFRAME; - } + 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_.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_FAILURE; + } + optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); // select keyframe core_kfs_.insert(new_frame_); @@ -209,6 +206,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() double depth_mean, depth_min; frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); + if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited { depth_filter_->addFrame(new_frame_); diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp index aec6f46..a64adf5 100644 --- a/GPU_version/vio/src/sparse_img_align_gpu.cpp +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -64,8 +64,6 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) { cv::Mat cur_img = cur_frame->img_pyr_.at(level_); cv::Mat ref_img = ref_frame->img_pyr_.at(level_); - // cv::imwrite(std::string(getenv("HOME"))+"/Project/cur"+to_string(level_)+".png",cur_img); - // cv::imwrite(std::string(getenv("HOME"))+"/Project/fer"+to_string(level_)+".png",ref_img); residual_->reload_buf(1,0,cur_img); residual_->reload_buf(1,1,ref_img); residual_->write_buf(1,6,level_); @@ -74,6 +72,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) } cl_float3 pos[1]={0}; residual_->read(1,2,1,pos); + if(isnan(pos[0].x) || isnan(pos[0].y) || isnan(pos[0].z))return 0; cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); return 1; } @@ -90,7 +89,6 @@ double SparseImgAlignGpu::computeResiduals( residual_->reload_buf(1,10,chi2_); residual_->reload_buf(1,8,H); residual_->reload_buf(1,9,J); - /// TODO GPU compute based on the feature residual_->run(1,feature_counter_); float error[300]={0.0}; float chi[300]={0.0}; @@ -129,19 +127,18 @@ int SparseImgAlignGpu::solve() (double)H[6],(double)H[7],(double)H[8]}; double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z}; x_ = Eigen::Matrix(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2])); - if((bool) std::isnan((double) x_[0])) + if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2])) return 0; return 1; } void SparseImgAlignGpu::update() { -/// TODO the update situation may have a smarter solution cl_float3 pos[1]={0.0,0.0,0.0}; residual_->read(1,2,1,pos); Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_); pos[0].x=(float)update.translation()(0); pos[0].y=(float)update.translation()(1); - pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real()); + pos[0].z=(float)atan2(update.so2().unit_complex().imag(),update.so2().unit_complex().real()); residual_->reload_buf(1,2,pos); } diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 6cf2e32..0145baa 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -55,8 +55,8 @@ void Visualizer::publishMinimal( Quaterniond q(AngleAxisd(odom.second.pitch(), 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 = -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.x = odom.second.se2().translation()(1); + msg_pose_with_cov->pose.pose.position.y = odom.second.se2().translation()(0); 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(); @@ -71,7 +71,7 @@ void Visualizer::publishMinimal( pub_pose_with_cov_.publish(msg_pose_with_cov); if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ publishPointMarker( - pub_points_, Eigen::Vector3d(-odom.second.se2().translation()(0),-odom.second.se2().translation()(1),0.0), "trajectory", + pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),odom.second.se2().translation()(0),0.0), "trajectory", ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5)); } } diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 9ea9e31..ef818eb 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -13,7 +13,7 @@ // // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#define VIO_DEBUG false +#define VIO_DEBUG true #include #include #include @@ -107,7 +107,7 @@ VioNode::VioNode() : if(!vk::camera_loader::loadFromRosNs("vio", cam_)) throw std::runtime_error("Camera model not correctly specified."); Eigen::Matrix init; - init<<1e-19,1e-19,1e-19; + init<<1e-19,1e-19,M_PI; vo_ = new vio::FrameHandlerMono(cam_,init); usleep(500); } @@ -184,8 +184,8 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) { res.header.frame_id = "/world"; res.header.seq = trace_id_; auto odom=vo_->ukfPtr_.get_location(); - res.x=-odom.second.se2().translation()(0); - res.y=-odom.second.se2().translation()(1); + res.x=odom.second.se2().translation()(1); + res.y=odom.second.se2().translation()(0); res.yaw=odom.second.pitch(); res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2), odom.first(1,0),odom.first(1,1),odom.first(1,2),