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

confirm that backward motion is not supported during initialization

Browse Source
This commit is contained in:
gfuse
2021-09-23 19:52:48 +02:00
parent a64e064b37
commit 3fca9926bc
3 changed files with 1111 additions and 1044 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-22
View File
@@ -1,22 +0,0 @@
[16:31:45.913] init depth filter
[16:31:47.423] init depth filter
[16:31:50.898] DepthFilter: RESET.
[16:32:05.679] add key frame
[16:32:05.679] init seeds
[16:32:05.679] update 0 seeds
[16:32:05.679] init seeds
[16:32:05.682] DepthFilter: Initialized 82 new seeds
[16:32:05.682] add key frame
[16:32:05.682] update 82 seeds
[16:32:05.682] init seeds
[16:32:05.687] DepthFilter: Initialized 112 new seeds
[16:32:05.692] DepthFilter: Initialized 126 new seeds
[16:32:08.165] add new frame
[16:32:08.165] add key frame
[16:32:08.165] update 320 seeds
[16:32:08.165] init seeds
[16:32:08.168] DepthFilter: Initialized 149 new seeds
[16:32:10.624] add new frame
[16:32:10.624] update 469 seeds
[16:32:47.056] DepthFilter stop thread invoked.
[16:32:47.056] DepthFilter interrupt and join thread
GPU_version/vio/frame_handler_log.txt
+1096 -1013
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/src/math_utils.cpp
+15 -9
View File
@@ -6,6 +6,7 @@
*/ */
#include <vio/math_utils.h> #include <vio/math_utils.h>
#include <ros/ros.h>
namespace vk { namespace vk {
@@ -80,16 +81,21 @@ computeInliers(const vector<Vector3d>& features1, // c1
//triangulate all features and compute reprojection errors and inliers //triangulate all features and compute reprojection errors and inliers
for(size_t j=0; j<features1.size(); ++j) for(size_t j=0; j<features1.size(); ++j)
{ {
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] )); if(t.z()>0.0){
double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2); xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2); double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
if(e1 > reproj_thresh || e2 > reproj_thresh) double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
outliers.push_back(j); if(e1 > reproj_thresh || e2 > reproj_thresh)
else outliers.push_back(j);
{ else
inliers.push_back(j); {
tot_error += e1+e2; inliers.push_back(j);
tot_error += e1+e2;
}
}else{
ROS_ERROR("Backward motion is not supported for initialization, the robot needs to start its motion with a forward maneuver");
} }
} }
return tot_error; return tot_error;
} }