diff --git a/GPU_version/svo/include/gpu_svo/global.h b/GPU_version/svo/include/gpu_svo/global.h index f5292c4..a307503 100644 --- a/GPU_version/svo/include/gpu_svo/global.h +++ b/GPU_version/svo/include/gpu_svo/global.h @@ -62,6 +62,13 @@ namespace svo -sin_theta_,0.0,cos_theta_; T=new SE3(R,Vector3d(X_,0.0,Z_)); }; + void set(){ + X_=T->translation().x(); + Z_=T->translation().z(); + tetha_=atan(T->matrix()(0,2)/T->matrix()(0,0)); + cos_theta_=cos(tetha_); + sin_theta_=sin(tetha_); + } ~SE2_5(){} }; diff --git a/GPU_version/svo/src/frame_handler_mono.cpp b/GPU_version/svo/src/frame_handler_mono.cpp index 92a6607..7dcaffc 100644 --- a/GPU_version/svo/src/frame_handler_mono.cpp +++ b/GPU_version/svo/src/frame_handler_mono.cpp @@ -137,24 +137,32 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame() FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() { SE3 tem=imu_integPtr_->preintegrate_predict(); - new_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0))); + if((new_frame_->T_f_w_.T->translation()-tem.translation()).norm()>0.5){ + new_frame_->T_f_w_ = SE2_5(tem.translation().x(),tem.translation().z(),atan(tem.rotation_matrix()(0,2)/tem.rotation_matrix()(0,0))); + }else{ + new_frame_->T_f_w_ = last_frame_->T_f_w_; + } + new_frame_->T_f_w_.set(); //new_frame_->T_f_w_ = last_frame_->T_f_w_; // sparse image align SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), 30, SparseImgAlign::LevenbergMarquardt, false, false); img_align.run(last_frame_, new_frame_); + new_frame_->T_f_w_.set(); // map reprojection & feature alignment reprojector_.reprojectMap(new_frame_, overlap_kfs_); size_t sfba_n_edges_final=0; - + new_frame_->T_f_w_.set(); double sfba_thresh, sfba_error_init, sfba_error_final; if(!imu_integPtr_->predict(new_frame_,last_frame_,sfba_n_edges_final,reprojector_.n_matches_)){ return RESULT_FAILURE; } + new_frame_->T_f_w_.set(); pose_optimizer::optimizeGaussNewton( - Config::poseOptimThresh(), Config::poseOptimNumIter(), true, + Config::poseOptimThresh(), Config::poseOptimNumIter(), false, new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); + new_frame_->T_f_w_.set(); std::cerr<<"Reprojection Map: "<<"\t nPoint:"<T_f_w_.set(); // select keyframe core_kfs_.insert(new_frame_); setTrackingQuality(sfba_n_edges_final); @@ -176,20 +185,26 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() return RESULT_FAILURE; } double depth_mean, depth_min; + + new_frame_->T_f_w_.set(); frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); - if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD) + + new_frame_->T_f_w_.set(); + if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited { depth_filter_->addFrame(new_frame_); return RESULT_NO_KEYFRAME; } new_frame_->setKeyframe(); + new_frame_->T_f_w_.set(); // new keyframe selected for(auto&& it:new_frame_->fts_){ if(it->point != NULL)it->point->addFrameRef(it); } map_.point_candidates_.addCandidatePointToFrame(new_frame_); + new_frame_->T_f_w_.set(); // init new depth-filters depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); @@ -201,6 +216,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() map_.safeDeleteFrame(furthest_frame); } + new_frame_->T_f_w_.set(); // add keyframe to map map_.addKeyframe(new_frame_); std::cerr<<"Svo pipline was finished\n"; @@ -274,6 +290,7 @@ void FrameHandlerMono::setFirstFrame(const FramePtr& first_frame) bool FrameHandlerMono::needNewKf(double scene_depth_mean) { + if(scene_depth_mean >10.0)return true; for(auto&& it:overlap_kfs_) { Vector3d relpos = new_frame_->w2f(it.first->pos()); diff --git a/GPU_version/svo/src/imu_integration.cpp b/GPU_version/svo/src/imu_integration.cpp index 571ea4e..4346c0d 100644 --- a/GPU_version/svo/src/imu_integration.cpp +++ b/GPU_version/svo/src/imu_integration.cpp @@ -15,7 +15,7 @@ #include #include #include -#define debug 1 +#define debug 0 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ graphPtr=std::make_shared(); valuesPtr=std::make_shared(); @@ -52,9 +52,9 @@ bool Imu_Integration::update(double* imu){ auto in=std::chrono::steady_clock::now(); if(std::chrono::duration(in-t_1).count()>20.0){ t_1=in; - preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),0.0,-1.0*imu[1]-svo::Config::Y_off()),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.005); + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),0.005); }else{ - preintegratedPtr->integrateMeasurement(Eigen::Vector3d(imu[0]-svo::Config::X_off(),0.0,-1.0*imu[1]-svo::Config::Y_off()),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),std::chrono::duration(in-t_1).count()); + preintegratedPtr->integrateMeasurement(Eigen::Vector3d(-1.0*(imu[0]-svo::Config::X_off()),0.0,-1.0*(imu[1]-svo::Config::Y_off())),Eigen::Vector3d(0.0,imu[5]-svo::Config::Teta_off(),0.0),std::chrono::duration(in-t_1).count()); t_1=in; ++imu_n; } @@ -98,13 +98,16 @@ bool Imu_Integration::reset(gtsam::LevenbergMarquardtOptimizer& optimizer,boost: ++imu_factor_id; syn=false; gtsam::Marginals marginals(*graphPtr,result); - if(debug)fprintf(logtime, "%f,%f,%f,%f,%f,%f,\n", + if(debug)fprintf(logtime, "%f,%f,%f,%f,%f,%f,%f,%f,\n", new_frame->T_f_w_.T->translation().x(), new_frame->T_f_w_.T->translation().y(), new_frame->T_f_w_.T->translation().z(), + atan(new_frame->T_f_w_.T->rotation_matrix()(0,2)/new_frame->T_f_w_.T->rotation_matrix()(0,0)), statePtr->pose().x(), statePtr->pose().y(), - statePtr->pose().z()); + statePtr->pose().z(), + atan(statePtr->R()(0,2)/statePtr->R()(0,0))); + if((statePtr->t()-new_frame->T_f_w_.T->translation()).norm()>2.0)return false; new_frame->T_f_w_=svo::SE2_5(statePtr->t().x(),statePtr->t().z(),atan(statePtr->R()(0,2)/statePtr->R()(0,0))); new_frame->Cov_=marginals.marginalCovariance(P(imu_factor_id-1)); graphPtr->resize(0); @@ -147,7 +150,7 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame, P(imu_factor_id)); gtsam::LevenbergMarquardtParams params0; //params0.verbosity=gtsam::LevenbergMarquardtParams::LINEAR; - params0.verbosityLM = gtsam::LevenbergMarquardtParams::SUMMARY; + params0.verbosityLM = gtsam::LevenbergMarquardtParams::SILENT; params0.setlambdaFactor(10.0); params0.setlambdaInitial(100); params0.setlambdaUpperBound(1e10); diff --git a/GPU_version/svo/src/pose_optimizer.cpp b/GPU_version/svo/src/pose_optimizer.cpp index 8e8e47c..5dcbee3 100644 --- a/GPU_version/svo/src/pose_optimizer.cpp +++ b/GPU_version/svo/src/pose_optimizer.cpp @@ -98,7 +98,6 @@ void optimizeGaussNewton( b.noalias() -= J.transpose()*e*weight; new_chi2 += e.squaredNorm()*weight; } - // solve linear system const Vector6d dT(A.ldlt().solve(b)); diff --git a/GPU_version/svo/src/reprojector.cpp b/GPU_version/svo/src/reprojector.cpp index 0305797..b7f9022 100644 --- a/GPU_version/svo/src/reprojector.cpp +++ b/GPU_version/svo/src/reprojector.cpp @@ -72,7 +72,7 @@ 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/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index 13ada7a..c2189bd 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -17,6 +17,6 @@ svo: EBP: 0.1 #error in the bias used for preintegration NRP: 0.001 #noise used for projection in preintegration gravity: -9.65 - Acceleration_offset_x: -0.05 - Acceleration_offset_y: -0.04 - Gyroscope_offset_z: 0.001 + Acceleration_offset_x: 0.0 #-0.05 + Acceleration_offset_y: 0.0 #-0.04 + Gyroscope_offset_z: 0.0 #0.001 diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 0264b03..3d58393 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -65,10 +65,10 @@ void Visualizer::publishMinimal( msg_pose->pose.pose.position.x = p[0]; msg_pose->pose.pose.position.y = p[1]; msg_pose->pose.pose.position.z = p[2]; - msg_pose->pose.pose.orientation.x = q.x(); - msg_pose->pose.pose.orientation.y = q.y(); - msg_pose->pose.pose.orientation.z = q.z(); - msg_pose->pose.pose.orientation.w = q.w(); + msg_pose->pose.pose.orientation.x = q.x()/q.norm(); + msg_pose->pose.pose.orientation.y = q.y()/q.norm(); + msg_pose->pose.pose.orientation.z = q.z()/q.norm(); + msg_pose->pose.pose.orientation.w = q.w()/q.norm(); for(size_t i=0; i<36; ++i) msg_pose->pose.covariance[i] = Cov(i%6, i/6); pub_pose_.publish(msg_pose); diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index 0550d5a..0087b0c 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -105,12 +105,12 @@ void VoNode::imgCb(const sensor_msgs::ImageConstPtr& msg) } void VoNode::imuCb(const sensor_msgs::ImuPtr &imu) { double imu_in[6]; - imu_in[0] = 0.1*imu_[0]+0.9*imu->linear_acceleration.x; - imu_in[1] = 0.1*imu_[1]+0.9*imu->linear_acceleration.y; - imu_in[2] = 0.1*imu_[2]+0.9*imu->linear_acceleration.z; - imu_in[3] = 0.1*imu_[3]+0.9*imu->angular_velocity.x; - imu_in[4] = 0.1*imu_[4]+0.9*imu->angular_velocity.y; - imu_in[5] = 0.1*imu_[5]+0.9*imu->angular_velocity.z; + imu_in[0] = 0.4*imu_[0]+0.6*imu->linear_acceleration.x; + imu_in[1] = 0.4*imu_[1]+0.6*imu->linear_acceleration.y; + imu_in[2] = 0.4*imu_[2]+0.6*imu->linear_acceleration.z; + imu_in[3] = 0.4*imu_[3]+0.6*imu->angular_velocity.x; + imu_in[4] = 0.4*imu_[4]+0.6*imu->angular_velocity.y; + imu_in[5] = 0.4*imu_[5]+0.6*imu->angular_velocity.z; memcpy(imu_, imu_in, static_cast(6*sizeof(double))); vo_->imu_integPtr_->update(imu_in); }