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

calibrate orintation

Browse Source
This commit is contained in:
gfuse
2021-08-26 20:27:01 +02:00
parent 50192e3b04
commit c1ac138890
12 changed files with 56 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/include/vio/depth_filter.h
+2 -2
View File
@@ -123,7 +123,7 @@ public:
std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; } std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
/// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
static void updateSeed( static bool updateSeed(
const float x, const float x,
const float tau2, const float tau2,
Seed* seed); Seed* seed);
@@ -150,7 +150,7 @@ protected:
double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds. double new_keyframe_min_depth_; //!< Minimum depth in the new keyframe. Used for range in new seeds.
double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds. double new_keyframe_mean_depth_; //!< Maximum depth in the new keyframe. Used for range in new seeds.
Matcher matcher_; Matcher matcher_;
uint seeds_cont_=0; //uint seeds_cont_=0;
/// Initialize new seeds from a frame. /// Initialize new seeds from a frame.
void initializeSeeds(FramePtr frame); void initializeSeeds(FramePtr frame);
GPU_version/vio/include/vio/matcher.h
+1 -1
View File
@@ -87,7 +87,7 @@ public:
max_epi_search_steps(1000), max_epi_search_steps(1000),
subpix_refinement(true), subpix_refinement(true),
epi_search_edgelet_filtering(true), epi_search_edgelet_filtering(true),
epi_search_edgelet_max_angle(0.7) epi_search_edgelet_max_angle(0.3)
{} {}
} options_; } options_;
GPU_version/vio/include/vio/ukf.h
+20 -31
View File
@@ -18,22 +18,16 @@ public:
virtual ~Base(){ virtual ~Base(){
}; };
void predict(double dx,double dz,double dpitch,double time){ void predict(double dx,double dz,double dpitch,double time){
if(t_==0.0){ state_h_(2)=state_(2)+dt_angular*dpitch;
t_=time; state_h_(1)=state_(1)+dt_liner*(dz*cos(state_(2))-dx*sin(state_(2)));
return; state_h_(0)=state_(0)+dt_liner*(dx*cos(state_(2))+dz*sin(state_(2)));
}
double dt=time-t_;
t_=time;
state_h_(2)=state_(2)+dt*dpitch;
state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2));
state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2));
Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,3> R;
R<<vio::Config::Cmd_Cov(),0,0, R<<vio::Config::Cmd_Cov(),0,0,
0,vio::Config::Cmd_Cov(),0, 0,vio::Config::Cmd_Cov(),0,
0,0,vio::Config::Cmd_Cov(); 0,0,vio::Config::Cmd_Cov();
Eigen::Matrix<double,3,3> G; Eigen::Matrix<double,3,3> G;
G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)), G<<1.0,0,dt_liner*(-dx*sin(state_(2))+dz*cos(state_(2))),
0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)), 0,1.0,dt_liner*(-dz*sin(state_(2))-dx*cos(state_(2))),
0,0,1.0; 0,0,1.0;
cov_h_=G*cov_*G.transpose()+R; cov_h_=G*cov_*G.transpose()+R;
predict_up=true; predict_up=true;
@@ -43,33 +37,30 @@ public:
Eigen::Matrix<double,3,3> Q; Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k; Eigen::Matrix<double,3,3> k;
double dt=time-t_; H<<1.0,0,pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))),
t_=time; 0,1.0,-pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
H<<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; 0,0,1.0;
Q<<vio::Config::ACC_Noise(),0,0, Q<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0, 0,vio::Config::ACC_Noise(),0,
0,0,vio::Config::GYO_Noise(); 0,0,vio::Config::GYO_Noise();
E(0)=state_(0)+0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0); E(0)=state_(0)+pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)))-state_h_(0);
E(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1); E(1)=state_(1)+pow(0.03*dt_liner,2)*(ddz*cos(state_(2))-ddx*sin(state_(2)))-state_h_(1);
E(2)=state_(2)+dt*dpitch-state_h_(2); E(2)=state_(2)+0.03*dt_angular*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;
cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_; cov_=(Eigen::MatrixXd::Identity(3,3)-k*H)*cov_h_;
state_h_(2)=state_(2)+dt*dpitch; state_h_(2)=state_(2)+0.03*dt_angular*dpitch;
state_h_(1)=state_(1)+0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))); state_h_(1)=state_(1)+pow(0.03*dt_liner,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))); state_h_(0)=state_(0)+pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2)));
Eigen::Matrix<double,3,3> R; Eigen::Matrix<double,3,3> R;
R<<vio::Config::ACC_Noise(),0,0, R<<vio::Config::ACC_Noise(),0,0,
0,vio::Config::ACC_Noise(),0, 0,vio::Config::ACC_Noise(),0,
0,0,vio::Config::GYO_Noise(); 0,0,vio::Config::GYO_Noise();
Eigen::Matrix<double,3,3> G; Eigen::Matrix<double,3,3> G;
G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), G<<1.0,0,pow(0.03*dt_liner,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,1.0,-pow(0.03*dt_liner,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))),
0,0,1.0; 0,0,1.0;
cov_h_=G*cov_*G.transpose()+R; 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){
@@ -77,7 +68,6 @@ public:
Eigen::Matrix<double,3,3> Q; Eigen::Matrix<double,3,3> Q;
Eigen::Matrix<double,3,1> E; Eigen::Matrix<double,3,1> E;
Eigen::Matrix<double,3,3> k; Eigen::Matrix<double,3,3> k;
t_=time;
++match; ++match;
H<<1.0,0,0, H<<1.0,0,0,
0,1.0,0, 0,1.0,0,
@@ -89,17 +79,16 @@ public:
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);
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_;
Eigen::Matrix<double, 3, 1> state_; Eigen::Matrix<double, 3, 1> state_;
bool predict_up=false; bool predict_up=false;
private: private:
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; double dt_liner=0.2;
double dt_angular=0.2;
}; };
@@ -128,16 +117,16 @@ public:
while(lock)usleep(5); while(lock)usleep(5);
lock=true; lock=true;
pitch=pitch+M_PI; pitch=pitch+M_PI;
if(filter_->predict_up)filter_->correct(-x,-z,pitch,match,1e-9*time.toNSec()); if(filter_->predict_up)filter_->correct(-z,-x,pitch,match,1e-9*time.toNSec());
lock=false; lock=false;
double pitch_f; double pitch_f;
pitch_f=filter_->state_(2)+M_PI; pitch_f=filter_->state_(2)+M_PI;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
}; };
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){ std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
double pitch_f; double pitch_f;
pitch_f=filter_->state_(2)+M_PI; pitch_f=filter_->state_(2)+M_PI;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(1),-filter_->state_(0),pitch_f));
} }
private: private:
Base* filter_= nullptr; Base* filter_= nullptr;
GPU_version/vio/out.txt
BIN
View File
Binary file not shown.
GPU_version/vio/param/px30.yaml
+5 -5
View File
@@ -2,8 +2,8 @@ vio:
cam_model: ATAN cam_model: ATAN
cam_width: 640 cam_width: 640
cam_height: 480 cam_height: 480
cam_fx: 0.463632 cam_fx: 0.490202
cam_fy: 0.823661 cam_fy: 0.866527
cam_cx: 0.497465 cam_cx: 0.485102
cam_cy: 0.498443 cam_cy: 0.508577
cam_d0: 1.01687 cam_d0: 1.03345
GPU_version/vio/param/vo_fast.yaml
+4 -4
View File
@@ -5,12 +5,12 @@ svo:
quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. 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. 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_tracked: 100
init_min_disparity: 60 init_min_disparity: 80
structureoptim_max_pts: 50 structureoptim_max_pts: 50
kfselect_mindist: 0.01 kfselect_mindist: 0.01
poseoptim_thresh: 4.0 poseoptim_thresh: 4.0
init_min_inliers: 30 init_min_inliers: 40
ACC_noise: 0.5 #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 GYO_noise: 0.005 #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.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 Cmd_Cov: 0.1 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+11 -26
View File
@@ -109,13 +109,8 @@ void DepthFilter::addKeyframe(FramePtr frame, double depth_mean, double depth_mi
new_keyframe_set_ = true; new_keyframe_set_ = true;
seeds_updating_halt_ = true; seeds_updating_halt_ = true;
frame_queue_cond_.notify_one(); frame_queue_cond_.notify_one();
}else if(seeds_cont_>3){
initializeSeeds(frame);
seeds_cont_=0;
}else if(seeds_cont_==0){
initializeSeeds(frame);
} }
++seeds_cont_; initializeSeeds(frame);
} }
void DepthFilter::initializeSeeds(FramePtr frame) void DepthFilter::initializeSeeds(FramePtr frame)
@@ -185,9 +180,6 @@ void DepthFilter::updateSeedsLoop()
if(new_keyframe_set_) if(new_keyframe_set_)
{ {
new_keyframe_set_ = false; new_keyframe_set_ = false;
if(seeds_cont_>3){
seeds_updating_halt_ = false;
}
clearFrameQueue(); clearFrameQueue();
frame = new_keyframe_; frame = new_keyframe_;
} }
@@ -198,12 +190,6 @@ void DepthFilter::updateSeedsLoop()
} }
} }
updateSeeds(frame); updateSeeds(frame);
if((frame->isKeyframe() && seeds_cont_>3) ||(frame->isKeyframe() && seeds_cont_==0)){
initializeSeeds(frame);
seeds_cont_=0;
}
} }
} }
@@ -211,7 +197,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
{ {
// update only a limited number of seeds, because we don't have time to do it // update only a limited number of seeds, because we don't have time to do it
// for all the seeds in every frame! // for all the seeds in every frame!
size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size(); size_t n_updates=0, n_failed_matches=0;
lock_t lock(seeds_mut_); lock_t lock(seeds_mut_);
std::list<Seed>::iterator it=seeds_.begin(); std::list<Seed>::iterator it=seeds_.begin();
@@ -224,11 +210,6 @@ void DepthFilter::updateSeeds(FramePtr frame)
if(seeds_updating_halt_) if(seeds_updating_halt_)
return; return;
// check if seed is not already too old
if((Seed::batch_counter - it->batch_id) > options_.max_n_kfs) {
it = seeds_.erase(it);
continue;
}
if(it->ftr->frame == NULL){ if(it->ftr->frame == NULL){
it = seeds_.erase(it); it = seeds_.erase(it);
continue; continue;
@@ -241,7 +222,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1); T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1);
SE3 T_ref_cur(R,Vector3d(T.translation()(0),0.0,T.translation()(1))); SE3 T_ref_cur(R,Vector3d(T.translation()(0),0.0,T.translation()(1)));
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) ); const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
if(xyz_f.z() < 0.0 && xyz_f.y() < 0.08) { if(xyz_f.z() < 0.0) {
++it; // behind the camera ++it; // behind the camera
continue; continue;
} }
@@ -271,7 +252,11 @@ void DepthFilter::updateSeeds(FramePtr frame)
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau)); double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
// update the estimate // update the estimate
updateSeed(1.0/z, tau_inverse*tau_inverse, &*it); if(!updateSeed(1.0/z, tau_inverse*tau_inverse, &*it)){
++it;
++n_failed_matches;
continue;
}
++n_updates; ++n_updates;
if(frame->isKeyframe()) if(frame->isKeyframe())
@@ -315,12 +300,11 @@ void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
seeds.push_back(*it); seeds.push_back(*it);
} }
} }
bool DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{ {
float norm_scale = sqrt(seed->sigma2 + tau2); float norm_scale = sqrt(seed->sigma2 + tau2);
if(std::isnan(norm_scale)) if(std::isnan(norm_scale))
return; return false;
boost::math::normal_distribution<float> nd(seed->mu, norm_scale); boost::math::normal_distribution<float> nd(seed->mu, norm_scale);
float s2 = 1./(1./seed->sigma2 + 1./tau2); float s2 = 1./(1./seed->sigma2 + 1./tau2);
float m = s2*(seed->mu/seed->sigma2 + x/tau2); float m = s2*(seed->mu/seed->sigma2 + x/tau2);
@@ -339,6 +323,7 @@ void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
seed->mu = mu_new; seed->mu = mu_new;
seed->a = (e-f)/(f-e/f); seed->a = (e-f)/(f-e/f);
seed->b = seed->a*(1.0f-f)/f; seed->b = seed->a*(1.0f-f)/f;
return true;
} }
double DepthFilter::computeTau( double DepthFilter::computeTau(
GPU_version/vio/src/frame_handler_mono.cpp
+4 -6
View File
@@ -171,11 +171,11 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); 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; if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE;
reprojector_.reprojectMap(new_frame_, overlap_kfs_); reprojector_.reprojectMap(new_frame_, overlap_kfs_);
/*
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_
<<"\t distance between two frames: "<< <<"\t distance between two frames: "<<
(last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';*/ (last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n';
if( reprojector_.n_trials_ < 10)return RESULT_FAILURE; if( reprojector_.n_trials_ < 10)return RESULT_FAILURE;
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;
@@ -183,16 +183,14 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
Config::poseOptimThresh(), Config::poseOptimNumIter(), false, Config::poseOptimThresh(), Config::poseOptimNumIter(), false,
new_frame_, sfba_thresh, sfba_error_init, sfba_error_final, sfba_n_edges_final); 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()>0.5 || 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){ 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()){
return RESULT_FAILURE; return RESULT_FAILURE;
} }
auto result=ukfPtr_.UpdateSvo(new_frame_->T_f_w_.se2().translation()(0), 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_.se2().translation()(1),new_frame_->T_f_w_.pitch(),sfba_n_edges_final,time_);
new_frame_->T_f_w_ =result.second; new_frame_->T_f_w_ =result.second;
new_frame_->Cov_ = result.first; new_frame_->Cov_ = result.first;
if(sfba_n_edges_final < Config::qualityMinFts() || reprojector_.n_matches_ < Config::qualityMinFts()){
return RESULT_FAILURE;
}
optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter()); optimizeStructure(new_frame_, Config::structureOptimMaxPts(), Config::structureOptimNumIter());
// select keyframe // select keyframe
GPU_version/vio/src/matcher.cpp
+1 -1
View File
@@ -249,7 +249,7 @@ bool Matcher::findEpipolarMatchDirect(
return false; return false;
} }
size_t n_steps = epi_length_/0.7; // one step per pixel size_t n_steps = epi_length_/0.3; // one step per pixel
Vector2d step = epi_dir_/n_steps; Vector2d step = epi_dir_/n_steps;
if(n_steps > options_.max_epi_search_steps) if(n_steps > options_.max_epi_search_steps)
GPU_version/vio/src/sparse_img_align_gpu.cpp
+2
View File
@@ -129,6 +129,8 @@ int SparseImgAlignGpu::solve()
x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2])); x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2])) if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
return 0; return 0;
double norm=x_.norm();
if(norm<=0 ||norm > 1.0)x_=Eigen::Vector3d(0.1,0.1,0.1);
return 1; return 1;
} }
void SparseImgAlignGpu::update() void SparseImgAlignGpu::update()
GPU_version/vio/src/visualizer.cpp
+3 -3
View File
@@ -55,8 +55,8 @@ void Visualizer::publishMinimal(
Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ())); Quaterniond q(AngleAxisd(odom.second.pitch(), Vector3d::UnitZ()));
geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped); geometry_msgs::PoseWithCovarianceStampedPtr msg_pose_with_cov(new geometry_msgs::PoseWithCovarianceStamped);
msg_pose_with_cov->header = header_msg; msg_pose_with_cov->header = header_msg;
msg_pose_with_cov->pose.pose.position.x = odom.second.se2().translation()(1); 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()(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.position.z = 0.0;
msg_pose_with_cov->pose.pose.orientation.x = q.x(); 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.y = q.y();
@@ -71,7 +71,7 @@ void Visualizer::publishMinimal(
pub_pose_with_cov_.publish(msg_pose_with_cov); pub_pose_with_cov_.publish(msg_pose_with_cov);
if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){ if(pub_frames_.getNumSubscribers() > 0 || pub_points_.getNumSubscribers() > 0){
publishPointMarker( publishPointMarker(
pub_points_, Eigen::Vector3d(odom.second.se2().translation()(1),odom.second.se2().translation()(0),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)); ros::Time::now(), trace_id_, 0, 0.01, Vector3d(0.,0.,0.5));
} }
} }
GPU_version/vio/src/vo_node.cpp
+3 -3
View File
@@ -154,7 +154,7 @@ void VioNode::imuCb(const sensor_msgs::ImuPtr &imu) {
}; };
imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x; imu_in[0] = 0.5*imu_[0]+0.5*imu->linear_acceleration.x;
imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y; imu_in[1] = 0.5*imu_[1]+0.5*imu->linear_acceleration.y;
imu_in[2] = 0.5*imu_[2]+0.5*imu->angular_velocity.z; imu_in[2] = imu->angular_velocity.z;
memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double))); memcpy(imu_, imu_in, static_cast<std::size_t>(3*sizeof(double)));
vo_->UpdateIMU(imu_in,imu->header.stamp); vo_->UpdateIMU(imu_in,imu->header.stamp);
imu_time_=imu->header.stamp; imu_time_=imu->header.stamp;
@@ -184,8 +184,8 @@ bool VioNode::getOdom(vio::getOdom::Request &req, vio::getOdom::Response &res) {
res.header.frame_id = "/world"; res.header.frame_id = "/world";
res.header.seq = trace_id_; res.header.seq = trace_id_;
auto odom=vo_->ukfPtr_.get_location(); auto odom=vo_->ukfPtr_.get_location();
res.x=odom.second.se2().translation()(1); res.x=odom.second.se2().translation()(0);
res.y=odom.second.se2().translation()(0); res.y=odom.second.se2().translation()(1);
res.yaw=odom.second.pitch(); res.yaw=odom.second.pitch();
res.cov={odom.first(0,0),odom.first(0,1),odom.first(0,2), 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), odom.first(1,0),odom.first(1,1),odom.first(1,2),