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

blocked

Browse Source
This commit is contained in:
gfuse
2021-09-23 10:21:28 +02:00
parent 93587b7859
commit d174054c25
6 changed files with 1862 additions and 4133 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-2248
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+1844 -1872
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/matcher.h
+1 -1
View File
@@ -120,7 +120,7 @@ public:
const double d_estimate,
const double d_min,
const double d_max,
double& depth);
double& depth,FILE* log);
void createPatchFromPatchWithBorder();
};
GPU_version/vio/src/depth_filter.cpp
+10 -8
View File
@@ -248,6 +248,7 @@ namespace vio {
#if VIO_DEBUG
fprintf(log_,"[%s] update %d seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),seeds_.size());
#endif
return;
const double focal_length = frame->cam_->errorMultiplier2();
double px_noise = 1.0;
double px_error_angle = atan2(px_noise,(2.0*focal_length))*2.0; // law of chord (sehnensatz)
@@ -260,18 +261,19 @@ namespace vio {
continue;
}
// check if point is visible in the current image
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());
//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());
SE2 T((*it)->ftr->frame->T_f_w_.so2()*frame->T_f_w_.inverse().so2(),frame->T_f_w_.inverse().translation()+(*it)->ftr->frame->T_f_w_.translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(-0.122173, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_cur_ref(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_cur_ref*(1.0/(*it)->mu * (*it)->ftr->f) );
SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/(*it)->mu * (*it)->ftr->f) );
#if VIO_DEBUG
fprintf(log_,"[%s] If point is visible? %f, %f, %f mu:%f unit_bearing:%f %f %f T_cur_ref: x=%f y=%f z=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fprintf(log_,"[%s] If point is visible? %f, %f, %f mu:%f unit_bearing:%f %f %f T_ref_cur: x=%f y=%f z=%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
xyz_f.x(),xyz_f.y(),xyz_f.z(),(*it)->mu,(*it)->ftr->f.x(),(*it)->ftr->f.y(),(*it)->ftr->f.z(),
T_cur_ref.translation().x(),T_cur_ref.translation().y(),T_cur_ref.translation().z());
T_ref_cur.translation().x(),T_ref_cur.translation().y(),T_ref_cur.translation().z());
#endif
if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
++it; // point does not project in image
@@ -281,12 +283,12 @@ 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 inverse min: %f, Z inverse max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
fprintf(log_,"[%s] Z inverse min: %f, Z inverse max: %f sigma2:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max,(*it)->sigma2);
#endif
double z;
if(!matcher_.findEpipolarMatchDirect(
*(*it)->ftr->frame, *frame, *(*it)->ftr, 1.0/(*it)->mu, 1.0/z_min, 1.0/z_max, z))
*(*it)->ftr->frame, *frame, *(*it)->ftr, 1.0/(*it)->mu, 1.0/z_min, 1.0/z_max, z,log_))
{
(*it)->b++; // increase outlier probability when no match was found
++it;
@@ -294,7 +296,7 @@ namespace vio {
continue;
}
double tau = computeTau(T_cur_ref.inverse(), (*it)->ftr->f, z, px_error_angle);
double tau = computeTau(T_ref_cur, (*it)->ftr->f, z, px_error_angle);
double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
#if VIO_DEBUG
fprintf(log_,"[%s] calculated Z= %f tau: %f tau_inv: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z,
GPU_version/vio/src/feature_alignment.cpp
+2 -2
View File
@@ -336,7 +336,7 @@ bool align2D_SSE2(
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.01*0.01;
const float min_update_squared = 0.001;
const int cur_step = cur_img.step.p[0];
const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
float chi2 = 0;
@@ -501,7 +501,7 @@ bool align2D_NEON (
float v = cur_px_estimate.y();
// termination condition
const float min_update_squared = 0.01*0.01;
const float min_update_squared = 0.001;
const int cur_step = cur_img.step.p[0];
Vector3f update;
Vector3f Jres;
GPU_version/vio/src/matcher.cpp
+5 -2
View File
@@ -195,7 +195,7 @@ bool Matcher::findEpipolarMatchDirect(
const double d_estimate,
const double d_min,
const double d_max,
double& depth)
double& depth,FILE* log)
{
if(isnan(d_min) || isnan(d_max))return false;
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()));
@@ -230,7 +230,10 @@ bool Matcher::findEpipolarMatchDirect(
Vector2d px_A(cur_frame.cam_->world2cam(A));
Vector2d px_B(cur_frame.cam_->world2cam(B));
epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
#if VIO_DEBUG
fprintf(log,"[%s] epi_dir x= %f, y=%f: A:%f,%f B:%f,%f epi_length_:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
epi_dir_.x(),epi_dir_.y(),A.x(),A.y(),B.x(),B.y(),epi_length_);
#endif
// Warp reference patch at ref_level
if(!warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,