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

interest wrong result

Browse Source
This commit is contained in:
gfuse
2021-10-08 10:17:08 +02:00
parent fd2d859010
commit bcba6b30d0
8 changed files with 14 additions and 2443 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/depth_filter_log.txt
-135
View File
@@ -1,135 +0,0 @@
[08:29:34.448] New key frame
[08:29:34.448] BA loop run
[08:29:34.448] init error: -nan
[08:29:34.450] end error: nan
[08:29:35.070] New key frame
[08:29:35.070] BA loop run
[08:29:35.071] init error: -nan
[08:29:35.073] end error: nan
[08:29:35.178] New key frame
[08:29:35.178] BA loop run
[08:29:35.178] init error: -nan
[08:29:35.181] end error: nan
[08:29:35.307] New key frame
[08:29:35.307] BA loop run
[08:29:35.307] init error: -nan
[08:29:35.311] end error: nan
[08:29:35.575] New key frame
[08:29:35.575] BA loop run
[08:29:35.575] init error: -nan
[08:29:35.577] end error: nan
[08:29:35.688] New key frame
[08:29:35.688] BA loop run
[08:29:35.689] init error: -nan
[08:29:35.691] end error: nan
[08:29:36.088] New key frame
[08:29:36.088] BA loop run
[08:29:36.088] init error: -nan
[08:29:36.090] end error: nan
[08:29:36.302] New key frame
[08:29:36.302] BA loop run
[08:29:36.303] init error: -nan
[08:29:36.305] end error: nan
[08:29:36.531] New key frame
[08:29:36.531] BA loop run
[08:29:36.531] init error: -nan
[08:29:36.534] end error: nan
[08:29:37.070] New key frame
[08:29:37.070] BA loop run
[08:29:37.070] init error: -nan
[08:29:37.073] end error: nan
[08:29:38.421] New key frame
[08:29:38.421] BA loop run
[08:29:38.422] init error: -nan
[08:29:38.423] end error: nan
[08:29:38.532] New key frame
[08:29:38.532] BA loop run
[08:29:38.532] init error: -nan
[08:29:38.533] end error: nan
[08:29:38.745] New key frame
[08:29:38.745] BA loop run
[08:29:38.746] init error: -nan
[08:29:38.747] end error: nan
[08:29:39.045] New key frame
[08:29:39.045] BA loop run
[08:29:39.046] init error: -nan
[08:29:39.047] end error: nan
[08:29:39.259] New key frame
[08:29:39.259] BA loop run
[08:29:39.259] init error: -nan
[08:29:39.261] end error: nan
[08:29:39.498] New key frame
[08:29:39.498] BA loop run
[08:29:39.498] init error: -nan
[08:29:39.501] end error: nan
[08:29:39.619] New key frame
[08:29:39.619] BA loop run
[08:29:39.620] init error: -nan
[08:29:39.623] end error: nan
[08:30:06.987] New key frame
[08:30:06.988] BA loop run
[08:30:06.988] init error: -nan
[08:30:06.989] end error: nan
[08:30:07.098] New key frame
[08:30:07.098] BA loop run
[08:30:07.099] init error: -nan
[08:30:07.100] end error: nan
[08:30:07.237] New key frame
[08:30:07.237] BA loop run
[08:30:07.237] init error: -nan
[08:30:07.239] end error: nan
[08:30:07.357] New key frame
[08:30:07.357] BA loop run
[08:30:07.357] init error: -nan
[08:30:07.359] end error: nan
[08:30:25.067] New key frame
[08:30:25.067] BA loop run
[08:30:25.068] init error: -nan
[08:30:25.068] end error: nan
[08:30:25.169] New key frame
[08:30:25.169] BA loop run
[08:30:25.169] init error: -nan
[08:30:25.170] end error: nan
[08:30:25.993] New key frame
[08:30:25.993] BA loop run
[08:30:25.993] init error: -nan
[08:30:25.994] end error: nan
[08:30:26.116] New key frame
[08:30:26.116] BA loop run
[08:30:26.117] init error: -nan
[08:30:26.117] end error: nan
[08:30:26.451] New key frame
[08:30:26.451] BA loop run
[08:30:26.451] init error: -nan
[08:30:26.452] end error: nan
[08:30:27.334] New key frame
[08:30:27.334] BA loop run
[08:30:27.335] init error: -nan
[08:30:27.335] end error: nan
[08:30:27.584] New key frame
[08:30:27.584] BA loop run
[08:30:27.584] init error: -nan
[08:30:27.585] end error: nan
[08:30:27.719] New key frame
[08:30:27.719] BA loop run
[08:30:27.719] init error: -nan
[08:30:27.720] end error: nan
[08:30:27.987] New key frame
[08:30:27.987] BA loop run
[08:30:27.987] init error: -nan
[08:30:27.987] end error: nan
[08:30:28.565] New key frame
[08:30:28.565] BA loop run
[08:30:28.565] init error: -nan
[08:30:28.566] end error: nan
[08:30:28.682] New key frame
[08:30:28.682] BA loop run
[08:30:28.683] init error: -nan
[08:30:28.683] end error: nan
[08:30:28.807] New key frame
[08:30:28.807] BA loop run
[08:30:28.808] init error: -nan
[08:30:28.808] end error: nan
[08:30:28.912] New key frame
[08:30:28.913] BA loop run
[08:30:28.91
GPU_version/vio/frame_handler_log.txt
-2293
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/bundle_adjustment.h
+1 -1
View File
@@ -15,7 +15,7 @@ namespace g2o {
namespace vio { namespace vio {
typedef EdgeProjectP2MC g2oEdgeSE3; typedef EdgeProjectXYZ2UV g2oEdgeSE3;
typedef g2o::VertexSE2Expmap g2oFrameSE2; typedef g2o::VertexSE2Expmap g2oFrameSE2;
typedef g2o::VertexSBAPointXYZ g2oPoint; typedef g2o::VertexSBAPointXYZ g2oPoint;
GPU_version/vio/include/vio/global_optimizer.h
+2 -2
View File
@@ -78,7 +78,7 @@ protected:
#endif #endif
/// Temporary container to hold the g2o edge with reference to frame and point. /// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3{ struct EdgeContainerSE3{
EdgeProjectP2MC* edge; g2o::EdgeProjectXYZ2UV* edge;
std::shared_ptr<Frame> frame; std::shared_ptr<Frame> frame;
std::shared_ptr<Feature> feature; std::shared_ptr<Feature> feature;
}; };
@@ -94,7 +94,7 @@ protected:
size_t id, size_t id,
bool fixed); bool fixed);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement. /// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2o::EdgeProjectP2MC* createG2oEdgeSE3( g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
g2o::VertexCam* v_kf, g2o::VertexCam* v_kf,
g2o::VertexSBAPointXYZ* v_mp, g2o::VertexSBAPointXYZ* v_mp,
const Vector2d& f_up, const Vector2d& f_up,
GPU_version/vio/param/vo_fast.yaml
+2 -2
View File
@@ -11,8 +11,8 @@ vio:
poseoptim_thresh: 8.0 poseoptim_thresh: 8.0
init_min_inliers: 40 init_min_inliers: 40
ACC_ekf: 0.2 #acc white noise in continuous in EKF ACC_ekf: 0.2 #acc white noise in continuous in EKF
GYO_ekf: 0.1 #gyro white noise in continuous in EKF GYO_ekf: 0.5 #gyro white noise in continuous in EKF
VO_ekf_translation: 1.0 #VO noise in EKF VO_ekf_translation: 1.0 #VO noise in EKF
VO_ekf_orientation: 3.0 #VO noise in EKF VO_ekf_orientation: 0.05 #VO noise in EKF
CMD_ekf_translation: 0.1 #control vector noise in EKF CMD_ekf_translation: 0.1 #control vector noise in EKF
CMD_ekf_orientation: 0.01 #control vector noise in EKF CMD_ekf_orientation: 0.01 #control vector noise in EKF
GPU_version/vio/src/frame_handler_mono.cpp
+1 -1
View File
@@ -239,7 +239,7 @@ bool FrameHandlerMono::needNewKf()
fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(), fprintf(log_,"[%s] need key frame pitch dis: %f translation dif:%f\n",vio::time_in_HH_MM_SS_MMM().c_str(),
fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()); fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
#endif #endif
if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.05 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.05)return true; if(fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()) > 0.1 || fabs((closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm())>0.1)return true;
return false; return false;
} }
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){ void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
GPU_version/vio/src/global_optimizer.cpp
+7 -8
View File
@@ -130,7 +130,7 @@ namespace vio {
else else
{ {
g2o::Edge_XYZ_VSC e1; g2o::Edge_XYZ_VSC e1;
g2o::EdgeProjectP2MC* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f), g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
true, true,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth()); Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
EdgeContainerSE3 edge; EdgeContainerSE3 edge;
@@ -147,7 +147,7 @@ namespace vio {
optimizer.initializeOptimization(); optimizer.initializeOptimization();
optimizer.computeActiveErrors(); optimizer.computeActiveErrors();
g2o::StructureOnlySolver<3> structure_only_ba; /* g2o::StructureOnlySolver<3> structure_only_ba;
g2o::OptimizableGraph::VertexContainer points; g2o::OptimizableGraph::VertexContainer points;
for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) { for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second); g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
@@ -155,8 +155,7 @@ namespace vio {
points.push_back(v); points.push_back(v);
} }
structure_only_ba.calc(points, 10); structure_only_ba.calc(points, 10);*/
/*
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] init error: %f \n", fprintf(log_,"[%s] init error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
@@ -165,7 +164,7 @@ namespace vio {
#if VIO_DEBUG #if VIO_DEBUG
fprintf(log_,"[%s] end error: %f \n", fprintf(log_,"[%s] end error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2()); vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
#endif*/ #endif
// Update Keyframe and MapPoint Positions // Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin(); for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
it_kf != map_.keyframes_.end();++it_kf) it_kf != map_.keyframes_.end();++it_kf)
@@ -225,21 +224,21 @@ namespace vio {
{ {
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ(); g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id); v->setId(id);
//v->setFixed(fixed); v->setFixed(fixed);
v->setMarginalized(true); v->setMarginalized(true);
v->setEstimate(pos); v->setEstimate(pos);
return v; return v;
} }
g2o::EdgeProjectP2MC* BA_Glob::createG2oEdgeSE3( g2o::VertexCam* v_frame, g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexCam* v_frame,
g2o::VertexSBAPointXYZ* v_point, g2o::VertexSBAPointXYZ* v_point,
const Vector2d& f_up, const Vector2d& f_up,
bool robust_kernel, bool robust_kernel,
double huber_width, double huber_width,
double weight) double weight)
{ {
g2o::EdgeProjectP2MC* e= new g2o::EdgeProjectP2MC; g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point)); e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame)); e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(f_up); e->setMeasurement(f_up);
GPU_version/vio/src/math_utils.cpp
+1 -1
View File
@@ -37,7 +37,7 @@ triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t,
double lamda0 = z.dot(b0)/pow(z.norm(), 2); double lamda0 = z.dot(b0)/pow(z.norm(), 2);
double lamda1 = z.dot(b1)/pow(z.norm(), 2); double lamda1 = z.dot(b1)/pow(z.norm(), 2);
if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0); if (lamda0 <= 0 || lamda1 <= 0)return Vector3d(0.0,0.0,0.0);
if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > 0.70*M_PI_2)return Vector3d(0.0,0.0,0.0); if (max(m0_hat.dot(R_F2_p), feature1.dot(f1_p)) > M_PI_2)return Vector3d(0.0,0.0,0.0);
if(R_F2_p.dot(f1_p) < 0.01)return Vector3d(0.0,0.0,0.0); if(R_F2_p.dot(f1_p) < 0.01)return Vector3d(0.0,0.0,0.0);
Vector3d xm = lamda1 * f1_p; Vector3d xm = lamda1 * f1_p;
Vector3d xn = t + lamda0 * R_F2_p; Vector3d xn = t + lamda0 * R_F2_p;