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
+6
View File
@@ -6,6 +6,7 @@
*/
#include <vio/math_utils.h>
#include <ros/ros.h>
namespace vk {
@@ -80,6 +81,7 @@ computeInliers(const vector<Vector3d>& features1, // c1
//triangulate all features and compute reprojection errors and inliers
for(size_t j=0; j<features1.size(); ++j)
{
if(t.z()>0.0){
xyz_vec.push_back(triangulateFeatureNonLin(R, t, features1[j], features2[j] ));
double e1 = reprojError(features1[j], xyz_vec.back(), error_multiplier2);
double e2 = reprojError(features2[j], R.transpose()*(xyz_vec.back()-t), error_multiplier2);
@@ -90,6 +92,10 @@ computeInliers(const vector<Vector3d>& features1, // c1
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;
}