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_);
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
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_);
filter_->correct(x,z,-1.0*pitch);
filter_->correct(x,z,pitch);
Eigen::Matrix<double,3,3> cov;
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_(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(){
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),
filter_->cov_(1,0),filter_->cov_(1,1),filter_->cov_(1,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 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(),
fabs(new_frame_->T_f_w_.pitch()-init_f.second.pitch()));
#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){
new_frame_=last_frame_;
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(),
closest_kfs.se2().translation().x(),closest_kfs.se2().translation().y(),closest_kfs.pitch());
#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;
}
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){