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

debug T_cur_ref

Browse Source
This commit is contained in:
gfuse
2021-09-17 12:02:13 +02:00
parent c3ca89fed8
commit c374f21714
8 changed files with 3845 additions and 52104 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
+400 -48798
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+3427 -3289
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/ukf.h
+5 -5
View File
@@ -114,8 +114,8 @@ public:
if(!start_)return;
while(lock)usleep(5);
lock=true;
if(abs(x)<1.0)x=pow(x,3);//picked up from your code
if(abs(y)<1.0)y=pow(y,3);//picked up from your code
x=pow(x,3);//picked up from your code
y=pow(y,3);//picked up from your code
filter_->correct(x,y,theta,1e-9*time.toNSec());
lock=false;
};
@@ -132,12 +132,12 @@ public:
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*/) {
while(lock)usleep(5);
lock=true;
filter_->correct(10.0*x,-10.0*z,-1.0*pitch);
filter_->correct(10.0*x,-10.0*z,pitch);
lock=false;
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),(filter_->state_(2))));
};
std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5> get_location(){
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),-1.0*(filter_->state_(2))));
return std::pair<Eigen::Matrix<double,3,3>,vio::SE2_5>(filter_->cov_,vio::SE2_5(0.1*filter_->state_(0),-0.1*filter_->state_(1),(filter_->state_(2))));
}
bool start_=false;
private:
GPU_version/vio/param/vo_fast.yaml
+3 -3
View File
@@ -10,7 +10,7 @@ svo:
kfselect_mindist: 0.01
poseoptim_thresh: 8.0
init_min_inliers: 30
ACC_noise: 0.5 #acc white noise in continuous in EKF
GYO_noise: 0.5 #gyro white noise in continuous in EKF
ACC_noise: 200000 #0.5 #acc white noise in continuous in EKF
GYO_noise: 200000 #0.5 #gyro white noise in continuous in EKF
svo_ekf: 0.05 #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: 2000000 #0.5 #control vector noise in EKF
GPU_version/vio/src/depth_filter.cpp
+6 -4
View File
@@ -258,7 +258,7 @@ namespace vio {
continue;
}
// check if point is visible in the current image
SE2 T(frame->T_f_w_.pitch()-(*it)->ftr->frame->T_f_w_.pitch(),frame->T_f_w_.se2().translation()+(*it)->ftr->frame->T_f_w_.inverse().translation());
SE2 T(frame->T_f_w_.so2()*(*it)->ftr->frame->T_f_w_.inverse().so2(),frame->T_f_w_.se2().translation()+(*it)->ftr->frame->T_f_w_.inverse().translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(0.0, Vector3d::UnitX())
@@ -269,10 +269,10 @@ namespace vio {
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
#endif
if(xyz_f.z() < 0.0) {
/* if(xyz_f.z() > 0.0) {
++it; // behind the camera
continue;
}
}*/
if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
++it; // point does not project in image
continue;
@@ -280,7 +280,9 @@ namespace vio {
float z_min = (*it)->mu + sqrt((*it)->sigma2);
float z_max = max((*it)->mu - sqrt((*it)->sigma2), 0.00000001f);
#if VIO_DEBUG
fprintf(log_,"[%s] Z min: %f, Z max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
#endif
if(z_min < 0){
++it;
continue;
GPU_version/vio/src/feature_alignment.cpp
+2 -2
View File
@@ -69,7 +69,7 @@ bool align1D(
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.05;
const float min_update_squared = 0.001;
const int cur_step = cur_img.step.p[0];
float chi2 = 0;
Vector2f update; update.setZero();
@@ -202,7 +202,7 @@ bool align2D(
bool sign=true;// +
// termination condition
const float min_update_squared = 0.05;//0.001
const float min_update_squared = 0.001;//0.001
const int cur_step = cur_img.step.p[0];
// float chi2 = 0;
Vector3f update; update.setZero();
GPU_version/vio/src/frame_handler_mono.cpp
+1 -2
View File
@@ -225,8 +225,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch()));
#endif
if((last_frame_->T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()>0.5 ||
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2 ||
sfba_n_edges_final < Config::qualityMinFts()/*10*/){
fabs(new_frame_->T_f_w_.pitch()-last_frame_->T_f_w_.pitch())>M_PI_2){
auto init_f= ukfPtr_.get_location();
new_frame_->T_f_w_=init_f.second;
new_frame_->Cov_ = init_f.first;
GPU_version/vio/src/matcher.cpp
+1 -1
View File
@@ -198,7 +198,7 @@ bool Matcher::findEpipolarMatchDirect(
double& depth)
{
if(isnan(d_min) || isnan(d_max))return false;
SE2_5 T_cur_ref(SE2(cur_frame.T_f_w_.pitch() - ref_frame.T_f_w_.pitch(),cur_frame.T_f_w_.se2().translation() - ref_frame.T_f_w_.se2().translation()));
SE2_5 T_cur_ref(SE2(cur_frame.T_f_w_.so2() * ref_frame.T_f_w_.inverse().so2(),cur_frame.T_f_w_.se2().translation() - ref_frame.T_f_w_.se2().translation()));
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;