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

confirm that all motion are initialized and initially tracked

Browse Source
This commit is contained in:
gfuse
2021-09-23 13:35:31 +02:00
parent d174054c25
commit 0614c558df
3 changed files with 1812 additions and 1850 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/frame_handler_log.txt
+1806 -1844
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/ukf.h
+4 -4
View File
@@ -143,16 +143,16 @@ public:
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->predict(x,y,theta,1e-9*time.toNSec()); filter_->predict(x,y,theta,1e-9*time.toNSec());
}; };
//Camera frame z forward, x right, y down -> right hands (pitch counts from x)correct //Camera frame z back, x right, y down -> left hands (pitch counts from x)correct
//Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature //Notice T_F_W is the position of the first frame with respect to the new frame while they are looking at one feature
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) { std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> UpdateSvo(double x/*in camera frame*/,double z/*in camera frame*/,double pitch/*in camera frame*/) {
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
filter_->correct(x,z,-1.0*pitch); filter_->correct(x,z,pitch);
Eigen::Matrix<double,3,3> cov; Eigen::Matrix<double,3,3> cov;
cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2), cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2),
filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2), filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2),
filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2); filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),(filter_->state_(2))));
}; };
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){ std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
boost::unique_lock<boost::mutex> lock(ekf_mut_); boost::unique_lock<boost::mutex> lock(ekf_mut_);
@@ -160,7 +160,7 @@ public:
cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2), cov<<filter_->cov_(0,0),filter_->cov_(0,1),filter_->cov_(0,2),
filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2), filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,2),
filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2); filter_->cov_(2,0),filter_->cov_(2,1),filter_->cov_(2,2);
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),-1.0*(filter_->state_(2)))); return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(cov,vio::SE2_5(filter_->state_(0),filter_->state_(1),(filter_->state_(2))));
} }
size_t cmd_syn_count_=0; size_t cmd_syn_count_=0;
size_t imu_syn_count_=2; size_t imu_syn_count_=2;
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -222,7 +222,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(), new_frame_->T_f_w_.se2().translation().y()-init_f.second.se2().translation().y(),
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())); fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
#endif #endif
if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.1 || if((init_f.second.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<4){ fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch())>0.25*M_PI_2 || sfba_n_edges_final<4){
new_frame_=last_frame_; new_frame_=last_frame_;
return RESULT_FAILURE; return RESULT_FAILURE;
@@ -301,7 +301,7 @@ bool FrameHandlerMono::needNewKf()
new_frame_->T_f_w_.se2().translation().x(),new_frame_->T_f_w_.se2().translation().y(),new_frame_->T_f_w_.pitch(), new_frame_->T_f_w_.se2().translation().x(),new_frame_->T_f_w_.se2().translation().y(),new_frame_->T_f_w_.pitch(),
closest_kfs.se2().translation().x(),closest_kfs.se2().translation().y(),closest_kfs.pitch()); closest_kfs.se2().translation().x(),closest_kfs.se2().translation().y(),closest_kfs.pitch());
#endif #endif
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.2)return true; if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.43 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.15)return true;
return false; return false;
} }
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){ void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){