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

edit EKF formula for odometry estimation problem

Browse Source
This commit is contained in:
gfuse
2021-07-06 18:43:09 +02:00
parent 9ca9e263ba
commit 3b110b299b
4 changed files with 38 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/gpu_svo/ukf.h
+26 -8
View File
@@ -56,8 +56,23 @@ public:
E(2)=state_(2)+dt*dpitch-state_h_(2); E(2)=state_(2)+dt*dpitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+k*E;
if(fabs(state_(2)-yaw_t_1_)>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_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
predict_up=false; 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)));
state_h_(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
Eigen::Matrix<double,3,3> R;
R<<svo::Config::ACC_Noise(),0,0,
0,svo::Config::ACC_Noise(),0,
0,0,svo::Config::GYO_Noise();
Eigen::Matrix<double,3,3> G;
G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
0,0,1.0;
cov_h_=G*cov_*G.transpose()+R;
yaw_t_1_=state_(2);
} }
void correct(double x, double z,double pitch,size_t match,double time){ void correct(double x, double z,double pitch,size_t match,double time){
Eigen::Matrix<double,3,3> H; Eigen::Matrix<double,3,3> H;
@@ -69,13 +84,16 @@ public:
H<<1.0,0,0, H<<1.0,0,0,
0,1.0,0, 0,1.0,0,
0,0,1.0; 0,0,1.0;
Q<<svo::Config::Svo_Ekf()/match,0,0, Q<<svo::Config::Svo_Ekf(),0,0,
0,svo::Config::Svo_Ekf()/match,0, 0,svo::Config::Svo_Ekf(),0,
0,0,50*svo::Config::Svo_Ekf()/match; 0,0,svo::Config::Svo_Ekf();
E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2); E<<x-state_h_(0),z-state_h_(1),pitch-state_h_(2);
k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse(); k=cov_h_*H.transpose()*(H*cov_h_*H.transpose()+Q).inverse();
state_=state_h_+k*E; state_=state_h_+(k*E);
if(fabs(state_(2)-yaw_t_1_)>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_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
yaw_t_1_=state_(2);
predict_up=false; predict_up=false;
} }
Eigen::Matrix<double,3,3> cov_; Eigen::Matrix<double,3,3> cov_;
@@ -85,6 +103,7 @@ private:
double t_=0.0; double t_=0.0;
Eigen::Matrix<double,3,3> cov_h_; Eigen::Matrix<double,3,3> cov_h_;
Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch} Eigen::Matrix<double, 3, 1> state_h_;//state{x,z,pitch}
double yaw_t_1_=0.0;
}; };
@@ -109,13 +128,12 @@ public:
filter_->predict(x,z,pitch,1e-9*time.toNSec()); filter_->predict(x,z,pitch,1e-9*time.toNSec());
lock=false; lock=false;
}; };
std::pair<Eigen::Matrix<double,3,3>,svo::SE2> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) {
while(lock)usleep(5); while(lock)usleep(5);
lock=true; lock=true;
if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec()); if(filter_->predict_up)filter_->correct(x,z,pitch,match,1e-9*time.toNSec());
lock=false; lock=false;
svo::SE2 out(filter_->state_(2),Eigen::Vector2d(filter_->state_(0),filter_->state_(1))); return std::pair<Eigen::Matrix<double,3,3>,svo::SE2_5>(filter_->cov_,svo::SE2_5(filter_->state_(0),filter_->state_(1),filter_->state_(2)));
return std::pair<Eigen::Matrix<double,3,3>,svo::SE2>(filter_->cov_,out);
}; };
private: private:
Base* filter_= nullptr; Base* filter_= nullptr;
GPU_version/vio/param/vo_fast.yaml
+5 -5
View File
@@ -7,9 +7,9 @@ svo:
init_min_tracked: 100 init_min_tracked: 100
init_min_disparity: 30 init_min_disparity: 30
structureoptim_max_pts: 50 structureoptim_max_pts: 50
kfselect_mindist: 0.05 kfselect_mindist: 0.001
poseoptim_thresh: 4.0 poseoptim_thresh: 4.0
ACC_noise: 0.1 #acc white noise in continuous in EKF ACC_noise: 0.001 #acc white noise in continuous in EKF
GYO_noise: 0.1 #gyro white noise in continuous in EKF GYO_noise: 0.001 #gyro white noise in continuous in EKF
svo_ekf: 5 #Svo noise that will be devided by number of Reprojected points after opmization in EKF svo_ekf: 100 #Svo noise that will be devided by number of Reprojected points after opmization in EKF
Cmd_Cov: 0.5 #control vector noise in EKF Cmd_Cov: 0.001 #control vector noise in EKF
GPU_version/vio/src/frame_handler_mono.cpp
+7 -14
View File
@@ -113,7 +113,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
new_frame_->T_f_w_ = last_frame_->T_f_w_; new_frame_->T_f_w_ = last_frame_->T_f_w_;
initialization::InitResult res = klt_homography_init_->addSecondFrame(new_frame_); 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_.getSE2().translation()(0),new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),1000,time_);
new_frame_->T_f_w_ = SE2_5(result.second); new_frame_->T_f_w_ = result.second;
new_frame_->Cov_ = result.first; new_frame_->Cov_ = result.first;
if(res == initialization::NO_KEYFRAME) if(res == initialization::NO_KEYFRAME)
return RESULT_NO_KEYFRAME; return RESULT_NO_KEYFRAME;
@@ -138,8 +138,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),
30, SparseImgAlign::LevenbergMarquardt, false, false); 30, SparseImgAlign::LevenbergMarquardt, false, false);
img_align.run(last_frame_, new_frame_); img_align.run(last_frame_, new_frame_);
// std::cout<<"img_align:\n"<<new_frame_->T_f_w_.getSE2()<<"\n";
// map reprojection & feature alignment
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
size_t sfba_n_edges_final=0; size_t sfba_n_edges_final=0;
double sfba_thresh, sfba_error_init, sfba_error_final; double sfba_thresh, sfba_error_init, sfba_error_final;
@@ -151,7 +149,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
} }
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.getSE2().translation()(0), 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_); new_frame_->T_f_w_.getSE2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
new_frame_->T_f_w_ = SE2_5(result.second); new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first; new_frame_->Cov_ = result.first;
std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second std::cout<<"Reprojection Map nPoint: "<<overlap_kfs_.back().second
<<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_ <<"\tnCell: "<<reprojector_.n_trials_<<"\t nMatches: "<<reprojector_.n_matches_
@@ -172,10 +170,6 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
double depth_mean, depth_min; double depth_mean, depth_min;
frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min); frame_utils::getSceneDepth(*new_frame_, depth_mean, depth_min);
if(depth_mean > 50 || depth_min < 0.0)
{
return RESULT_NO_KEYFRAME;
}
if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited if(!needNewKf(depth_mean) || tracking_quality_ == TRACKING_BAD)//edited
{ {
depth_filter_->addFrame(new_frame_); depth_filter_->addFrame(new_frame_);
@@ -191,6 +185,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
// init new depth-filters // init new depth-filters
depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min); depth_filter_->addKeyframe(new_frame_, depth_mean, 0.5*depth_min);
// if limited number of keyframes, remove the one furthest apart // if limited number of keyframes, remove the one furthest apart
if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs()) if(Config::maxNKfs() > 2 && map_.size() >= Config::maxNKfs())
{ {
@@ -275,13 +270,11 @@ bool FrameHandlerMono::needNewKf(double scene_depth_mean)
if(scene_depth_mean >10.0)return true; if(scene_depth_mean >10.0)return true;
for(auto&& it:overlap_kfs_) for(auto&& it:overlap_kfs_)
{ {
Vector3d relpos = new_frame_->w2f(Vector3d(it.first->pos()(0),0.0,it.first->pos()(1))); if(fabs(it.first->T_f_w_.pitch()-new_frame_->T_f_w_.pitch())>0.0523599)return true;
if(fabs(relpos.norm())/scene_depth_mean < Config::kfSelectMinDist()){ if(fabs((it.first->T_f_w_.translation()-new_frame_->T_f_w_.translation()).norm()) > Config::kfSelectMinDist())
return false;
}
}
return true; return true;
}
return false;
} }
void FrameHandlerMono::setCoreKfs(size_t n_closest) void FrameHandlerMono::setCoreKfs(size_t n_closest)
GPU_version/vio/src/map.cpp
-1
View File
@@ -109,7 +109,6 @@ void Map::getCloseKeyframes(
{ {
for(auto kf : keyframes_) for(auto kf : keyframes_)
{ {
std::cout<<"key points:"<<std::to_string(kf->key_pts_.size())<<'\t';
// check if kf has overlaping field of view with frame, use therefore KeyPoints // check if kf has overlaping field of view with frame, use therefore KeyPoints
for(auto keypoint : kf->key_pts_) for(auto keypoint : kf->key_pts_)
{ {