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

change the triangulation

Browse Source
This commit is contained in:
gfuse
2021-10-02 20:15:40 +02:00
parent 5216608883
commit 2b6a1461bb
7 changed files with 113 additions and 1203 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-168
View File
@@ -1,168 +0,0 @@
[14:54:18.504] New key frame
[14:54:18.504] BA loop run
[14:54:18.505] init error: 24.763610
[14:54:18.506] end error: 0.000054
[14:54:19.426] New key frame
[14:54:19.426] BA loop run
[14:54:19.426] init error: 12.320394
[14:54:19.428] end error: 0.002815
[14:54:19.527] New key frame
[14:54:19.527] BA loop run
[14:54:19.528] init error: 323.515875
[14:54:19.529] end error: 0.003837
[14:54:19.645] New key frame
[14:54:19.645] BA loop run
[14:54:19.645] init error: 2.917523
[14:54:19.647] end error: 0.000002
[14:54:19.756] New key frame
[14:54:19.756] BA loop run
[14:54:19.756] init error: 2.562036
[14:54:19.758] end error: 0.041513
[14:54:19.866] New key frame
[14:54:19.866] BA loop run
[14:54:19.867] init error: 1910.325804
[14:54:19.867] end error: 0.000167
[14:54:19.963] New key frame
[14:54:19.963] BA loop run
[14:54:19.963] init error: 289.670025
[14:54:19.964] end error: 0.000167
[14:54:20.055] New key frame
[14:54:20.055] BA loop run
[14:54:20.055] init error: 194.073707
[14:54:20.060] end error: 0.074585
[14:54:20.501] New key frame
[14:54:20.501] BA loop run
[14:54:20.501] init error: 4052.719150
[14:54:20.504] end error: 0.478617
[14:54:21.096] New key frame
[14:54:21.096] BA loop run
[14:54:21.097] init error: 597.219624
[14:54:21.101] end error: 0.836308
[14:54:21.315] New key frame
[14:54:21.315] BA loop run
[14:54:21.316] init error: 6935.758919
[14:54:21.322] end error: 3.265738
[14:54:21.763] New key frame
[14:54:21.763] BA loop run
[14:54:21.764] init error: 161.973034
[14:54:21.975] end error: 2.271218
[14:54:22.267] New key frame
[14:54:22.267] BA loop run
[14:54:22.268] init error: 95.065037
[14:54:22.391] end error: 2.446271
[14:54:22.941] New key frame
[14:54:22.941] BA loop run
[14:54:22.943] init error: 4603.510437
[14:54:22.951] end error: 2.320479
[14:54:24.537] New key frame
[14:54:24.537] BA loop run
[14:54:24.540] init error: 2029.462857
[14:54:24.722] end error: 4.087532
[14:54:25.356] New key frame
[14:54:25.356] BA loop run
[14:54:25.358] init error: 75690.392815
[14:54:25.518] end error: 5.770364
[14:54:25.673] New key frame
[14:54:25.673] BA loop run
[14:54:25.674] init error: 15240.984229
[14:54:26.122] end error: 4.056428
[14:54:26.123] New key frame
[14:54:26.123] BA loop run
[14:54:26.127] init error: 386.422521
[14:54:26.710] end error: 3.468697
[14:54:27.018] New key frame
[14:54:27.018] BA loop run
[14:54:27.020] init error: 2927.060707
[14:54:27.234] end error: 3.722009
[14:54:27.501] New key frame
[14:54:27.501] BA loop run
[14:54:27.503] init error: 68846.950097
[14:54:27.510] end error: 3.166685
[14:54:27.930] New key frame
[14:54:27.930] BA loop run
[14:54:27.932] init error: 13881.061617
[14:54:27.997] end error: 8.823857
[14:54:28.144] New key frame
[14:54:28.144] BA loop run
[14:54:28.147] init error: 3859.481635
[14:54:28.329] end error: 8.667055
[14:54:28.590] New key frame
[14:54:28.590] BA loop run
[14:54:28.592] init error: 4412.399485
[14:54:28.992] end error: 7.702882
[14:54:28.994] New key frame
[14:54:28.994] BA loop run
[14:54:28.998] init error: 2083.696145
[14:54:29.013] end error: 11.568292
[14:54:29.168] New key frame
[14:54:29.168] BA loop run
[14:54:29.171] init error: 5318.544608
[14:54:29.355] end error: 80.936368
[14:54:29.366] New key frame
[14:54:29.366] BA loop run
[14:54:29.370] init error: 518178.847730
[14:54:29.381] end error: 234.161606
[14:54:29.759] New key frame
[14:54:29.759] BA loop run
[14:54:29.761] init error: 122235.358731
[14:54:29.769] end error: 62.169895
[14:54:30.253] New key frame
[14:54:30.253] BA loop run
[14:54:30.255] init error: 1847824.989597
[14:54:30.262] end error: 75.517423
[14:54:31.363] New key frame
[14:54:31.363] BA loop run
[14:54:31.365] init error: 43049.124812
[14:54:31.372] end error: 45.707574
[14:54:33.254] New key frame
[14:54:33.254] BA loop run
[14:54:33.256] init error: 332144.508586
[14:54:33.262] end error: 24.205326
[14:54:40.425] New key frame
[14:54:40.425] BA loop run
[14:54:40.427] init error: 8015.415519
[14:54:40.435] end error: 32.395425
[14:54:40.540] New key frame
[14:54:40.540] BA loop run
[14:54:40.542] init error: 12811.599311
[14:54:40.552] end error: 21.742364
[14:54:41.772] New key frame
[14:54:41.772] BA loop run
[14:54:41.775] init error: 7821.177608
[14:54:41.786] end error: 21.977430
[14:54:41.903] New key frame
[14:54:41.903] BA loop run
[14:54:41.905] init error: 78.574760
[14:54:42.003] end error: 9.756280
[14:54:42.173] New key frame
[14:54:42.173] BA loop run
[14:54:42.176] init error: 955.246709
[14:54:42.717] end error: 16.657278
[14:54:42.722] New key frame
[14:54:42.727] BA loop run
[14:54:42.733] init error: 41010.712911
[14:54:43.336] end error: 85.405648
[14:54:43.340] New key frame
[14:54:43.340] BA loop run
[14:54:43.345] init error: 75196195.162744
[14:54:43.675] end error: 462.211381
[14:54:43.692] New key frame
[14:54:43.692] BA loop run
[14:54:43.698] init error: 803.385900
[14:54:44.528] end error: 40.007984
[14:54:44.740] New key frame
[14:54:44.740] BA loop run
[14:54:44.744] init error: 508388.132806
[14:54:45.066] end error: 85.428830
[14:54:45.068] New key frame
[14:54:45.068] BA loop run
[14:54:45.075] init error: 50137.336823
[14:54:45.344] end error: 39.905726
[14:54:45.346] New key frame
[14:54:45.349] BA loop run
[14:54:45.366] init error: 21223.109285
[14:54:46.055] end error: 52.832861
[14:54:46.058] New key frame
[14:54:46.058] BA loop run
[14:54:46.064] init error: 907829.961200
[14:54:46.734] end error: 86.036304
GPU_version/vio/frame_handler_log.txt
+86 -1019
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/matcher.h
+3 -2
View File
@@ -124,7 +124,7 @@ public:
void createPatchFromPatchWithBorder(); void createPatchFromPatchWithBorder();
uint i=0; uint i=0;
void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res,uint8_t* patch,uint8_t* patch_border){ void debug(cv::Mat ref,cv::Mat cur, Vector2d ref_px,Vector2d cur_px_in,Vector2d cur_px_out,bool res,uint8_t* patch,uint8_t* patch_border, double depth){
cv::Mat Ref,Cur,Patch,Patch_b; cv::Mat Ref,Cur,Patch,Patch_b;
cv::cvtColor(cv::Mat(patch_size_,patch_size_,CV_8UC1,patch),Patch,cv::COLOR_GRAY2RGB); cv::cvtColor(cv::Mat(patch_size_,patch_size_,CV_8UC1,patch),Patch,cv::COLOR_GRAY2RGB);
cv::cvtColor(cv::Mat(patch_size_+2,patch_size_+2,CV_8UC1,patch_border),Patch_b,cv::COLOR_GRAY2RGB); cv::cvtColor(cv::Mat(patch_size_+2,patch_size_+2,CV_8UC1,patch_border),Patch_b,cv::COLOR_GRAY2RGB);
@@ -133,7 +133,8 @@ public:
cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2); cv::circle(Ref, cv::Point(ref_px.x(),ref_px.y() ), 2, cv::Scalar(0, 0,255),2);
cv::circle(Cur, cv::Point(cur_px_in.x(),cur_px_in.y() ), 2, cv::Scalar(0, 0,255),2); cv::circle(Cur, cv::Point(cur_px_in.x(),cur_px_in.y() ), 2, cv::Scalar(0, 0,255),2);
cv::circle(Cur, cv::Point(cur_px_out.x(),cur_px_out.y() ), 2, cv::Scalar(0, 255,0),2); cv::circle(Cur, cv::Point(cur_px_out.x(),cur_px_out.y() ), 2, cv::Scalar(0, 255,0),2);
cv::putText(Cur,res? "Accept": "reject",cv::Point(10,10),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA); cv::putText(Cur,res? "Accept": "reject",cv::Point(10,30),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA);
cv::putText(Cur,std::to_string(depth),cv::Point(100,30),cv::FONT_HERSHEY_COMPLEX_SMALL,1.0,cv::Scalar(0,255,0),1,cv:: LINE_AA);
cv::imwrite(std::string(PROJECT_DIR)+"/Ref"+std::to_string(i)+".png",Ref); cv::imwrite(std::string(PROJECT_DIR)+"/Ref"+std::to_string(i)+".png",Ref);
cv::imwrite(std::string(PROJECT_DIR)+"/Cur"+std::to_string(i)+".png",Cur); cv::imwrite(std::string(PROJECT_DIR)+"/Cur"+std::to_string(i)+".png",Cur);
cv::imwrite(std::string(PROJECT_DIR)+"/patch"+std::to_string(i)+".png",Patch); cv::imwrite(std::string(PROJECT_DIR)+"/patch"+std::to_string(i)+".png",Patch);
GPU_version/vio/src/feature_alignment.cpp
+3 -3
View File
@@ -69,7 +69,7 @@ bool align1D(
float v = cur_px_estimate.y(); float v = cur_px_estimate.y();
// termination condition // termination condition
const float min_update_squared = 0.001; const float min_update_squared = 1.0;
const int cur_step = cur_img.step.p[0]; const int cur_step = cur_img.step.p[0];
float chi2 = 0; float chi2 = 0;
Vector2f update; update.setZero(); Vector2f update; update.setZero();
@@ -337,7 +337,7 @@ bool align2D_SSE2(
float v = cur_px_estimate.y(); float v = cur_px_estimate.y();
// termination condition // termination condition
const float min_update_squared = 0.001; const float min_update_squared = 1.0;
const int cur_step = cur_img.step.p[0]; 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 const float Dinv = 1.0f/(A11*A22 - A12*A12); // we are missing an extra factor 16
float chi2 = 0; float chi2 = 0;
@@ -502,7 +502,7 @@ bool align2D_NEON (
float v = cur_px_estimate.y(); float v = cur_px_estimate.y();
// termination condition // termination condition
const float min_update_squared = 0.001; const float min_update_squared = 1.0;
const int cur_step = cur_img.step.p[0]; const int cur_step = cur_img.step.p[0];
Vector3f update; Vector3f update;
Vector3f Jres; Vector3f Jres;
GPU_version/vio/src/matcher.cpp
+4 -3
View File
@@ -41,7 +41,7 @@ void getWarpMatrixAffine(
Matrix2d& A_cur_ref) Matrix2d& A_cur_ref)
{ {
// Compute affine warp matrix A_ref_cur // Compute affine warp matrix A_ref_cur
const int halfpatch_size = 4; const int halfpatch_size = 8;
const Vector3d xyz_ref(f_ref*depth_ref); const Vector3d xyz_ref(f_ref*depth_ref);
Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref))); Vector3d xyz_du_ref(cam_ref.cam2world(px_ref + Vector2d(halfpatch_size,0)*(1<<level_ref)));
Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref))); Vector3d xyz_dv_ref(cam_ref.cam2world(px_ref + Vector2d(0,halfpatch_size)*(1<<level_ref)));
@@ -166,6 +166,7 @@ bool Matcher::findMatchDirect(
cur_frame.se3().inverse() * ref_ftr_->frame->se3(), ref_ftr_->level, A_cur_ref_); cur_frame.se3().inverse() * ref_ftr_->frame->se3(), ref_ftr_->level, A_cur_ref_);
//search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1); //search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
/// TODO paches will be mirrored while robot is rotating around it self
if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px, if(!warp::warpAffine(A_cur_ref_, ref_ftr_->frame->img_pyr_[ref_ftr_->level], ref_ftr_->px,
ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_))return false; ref_ftr_->level, ref_ftr_->level, halfpatch_size_+1, patch_with_border_))return false;
createPatchFromPatchWithBorder(); createPatchFromPatchWithBorder();
@@ -187,8 +188,8 @@ bool Matcher::findMatchDirect(
cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_, cur_frame.img_pyr_[ref_ftr_->level], patch_with_border_, patch_,
options_.align_max_iter, px_scaled); options_.align_max_iter, px_scaled);
} }
/* debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px, debug(ref_ftr_->frame->img_pyr_[ref_ftr_->level],cur_frame.img_pyr_[ref_ftr_->level],ref_ftr_->px,
px_cur,px_scaled ,success,patch_,patch_with_border_);*/ px_cur,px_scaled ,success,patch_,patch_with_border_,(ref_ftr_->frame->se3().inverse()*pt.pos_).norm());
px_cur = px_scaled; px_cur = px_scaled;
return success; return success;
} }
GPU_version/vio/src/math_utils.cpp
+16 -8
View File
@@ -19,36 +19,44 @@ Vector3d
triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
const Vector3d& feature1, const Vector3d& feature2 ) const Vector3d& feature1, const Vector3d& feature2 )
{ {
Vector3d R_F2 = R * feature2;//T*A=R*A+t /* Vector3d R_F2 = R * feature2;//T*A=R*A+t
Vector2d b; Vector2d b;
/* *//*
* b=cross(t,f1) * b=cross(t,f1)
* corss(t,Rf2) * corss(t,Rf2)
* */ * *//*
b[0] = t.dot(feature1); b[0] = t.dot(feature1);
b[1] = t.dot(R_F2);//cross(t,R*f2) b[1] = t.dot(R_F2);//cross(t,R*f2)
Matrix2d A; Matrix2d A;
/** *//**
* A<< cross(f1,f1), -z * A<< cross(f1,f1), -z
* z , -corss(R*f2,R*f2) * z , -corss(R*f2,R*f2)
* *
* det(A)= -corss(R*f2,R*f2)*cross(f1,f1) + pow(z,2) * det(A)= -corss(R*f2,R*f2)*cross(f1,f1) + pow(z,2)
*/ *//*
A(0,0) = feature1.dot(feature1); A(0,0) = feature1.dot(feature1);
A(1,0) = feature1.dot(R_F2);//z A(1,0) = feature1.dot(R_F2);//z
A(0,1) = -A(1,0); A(0,1) = -A(1,0);
A(1,1) = -R_F2.dot(R_F2); A(1,1) = -R_F2.dot(R_F2);
/* *//*
* A_inv<< cross(f1,f1)/det(A) , z/det(A) * A_inv<< cross(f1,f1)/det(A) , z/det(A)
* -z/det(A) , -corss(R*f2,R*f2)/det(A) * -z/det(A) , -corss(R*f2,R*f2)/det(A)
* *
* lambda[0]= cross(t,f1)*cross(f1,f1)/det(A) + corss(t,Rf2)*z/det(A) * lambda[0]= cross(t,f1)*cross(f1,f1)/det(A) + corss(t,Rf2)*z/det(A)
* lambda[1]= -cross(t,f1)*z/det(A) - corss(t,Rf2) * corss(R*f2,R*f2)/det(A) * lambda[1]= -cross(t,f1)*z/det(A) - corss(t,Rf2) * corss(R*f2,R*f2)/det(A)
* */ * *//*
Vector2d lambda = A.inverse() * b; Vector2d lambda = A.inverse() * b;
Vector3d xm = lambda[0] * feature1; Vector3d xm = lambda[0] * feature1;
Vector3d xn = t + lambda[1] * R_F2; Vector3d xn = t + lambda[1] * R_F2;
return ( xm + xn )/2; return ( xm + xn )/2;*/
Vector3d R_F2 = R * feature2;//T*A=R*A+t
Vector3d z = feature1.cross(R_F2);
Vector3d b0 = t.cross(feature1);
Vector3d b1 = t.cross(R_F2);
Vector3d xm = ((z.dot(b1)/pow(z.norm(), 2))*(feature1));
Vector3d xn = t + ((z.dot(b0)/pow(z.norm(), 2))*R_F2);
return ( xm + xn )/2;
} }
bool bool
GPU_version/vio/src/reprojector.cpp
+1
View File
@@ -179,6 +179,7 @@ namespace vio {
break; break;
} }
} }
exit(0);
} }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////