public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove logs, trying to add more key frames by setting some situation for depth mean

Browse Source
This commit is contained in:
gfuse
2021-06-01 19:26:34 +02:00
parent faf00a6aad
commit 982217598a
8 changed files with 51 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/svo/include/gpu_svo/global.h
+7
View File
@@ -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(){}
};
GPU_version/svo/src/frame_handler_mono.cpp
+21 -4
View File
@@ -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:"<<overlap_kfs_.back().second
<<"\t nCell = "<<reprojector_.n_trials_<<"\t \t nMatches = "<<reprojector_.n_matches_
<<" \t Reprojected points after opmization: "<<sfba_n_edges_final<<'\n';
@@ -168,6 +176,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
//SVO_STOP_TIMER("point_optimizer");
new_frame_->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());
GPU_version/svo/src/imu_integration.cpp
+9 -6
View File
@@ -15,7 +15,7 @@
#include <vikit/params_helper.h>
#include <gpu_svo/config.h>
#include <gpu_svo/for_it.hpp>
#define debug 1
#define debug 0
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
valuesPtr=std::make_shared<gtsam::Values>();
@@ -52,9 +52,9 @@ bool Imu_Integration::update(double* imu){
auto in=std::chrono::steady_clock::now();
if(std::chrono::duration<double>(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<double>(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<double>(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<svo::Frame>& 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);
GPU_version/svo/src/pose_optimizer.cpp
-1
View File
@@ -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));
GPU_version/svo/src/reprojector.cpp
+1 -1
View File
@@ -72,7 +72,7 @@ void Reprojector::reprojectMap(
list< pair<FramePtr,double> > close_kfs;
map_.getCloseKeyframes(frame, close_kfs);
std::cout<<"The number of closed frames: "<<std::to_string(close_kfs.size())<<'\n';
//std::cout<<"The number of closed frames: "<<std::to_string(close_kfs.size())<<'\n';
// Sort KFs with overlap according to their closeness
close_kfs.sort(boost::bind(&std::pair<FramePtr, double>::second, _1) <
boost::bind(&std::pair<FramePtr, double>::second, _2));
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -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
GPU_version/vio/src/visualizer.cpp
+4 -4
View File
@@ -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);
GPU_version/vio/src/vo_node.cpp
+6 -6
View File
@@ -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<std::size_t>(6*sizeof(double)));
vo_->imu_integPtr_->update(imu_in);
}