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

debugging

Browse Source
This commit is contained in:
gfuse
2021-10-08 08:58:40 +02:00
parent 91a01ec03d
commit fd2d859010
9 changed files with 2475 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+1
View File
@@ -68,6 +68,7 @@ LIST(APPEND LINK_LIBS
${G2O_SOLVER_PCG}
${G2O_TYPES_SBA}
${G2O_TYPES_SLAM3D}
${G2O_TYPES_ICP}
cholmod cxsparse
)
GPU_version/vio/depth_filter_log.txt
Executable
+135
View File
@@ -0,0 +1,135 @@
[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
Executable
+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 {
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
typedef EdgeProjectP2MC g2oEdgeSE3;
typedef g2o::VertexSE2Expmap g2oFrameSE2;
typedef g2o::VertexSBAPointXYZ g2oPoint;
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -50,7 +50,7 @@ namespace vio {
Features fts_; //!< List of features in the image.
vector<std::shared_ptr<Feature>> key_pts_; //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
bool is_keyframe_; //!< Was this frames selected as keyframe?
g2o::VertexSE3Expmap* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe.
g2o::VertexCam* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe.
int last_published_ts_; //!< Timestamp of last publishing.
Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
GPU_version/vio/include/vio/global_optimizer.h
+5 -6
View File
@@ -31,10 +31,9 @@
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/types/icp/types_icp.h>
#include <g2o/stuff/sampler.h>
#include <memory>
@@ -79,13 +78,13 @@ protected:
#endif
/// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3{
g2o::EdgeProjectXYZ2UV* edge;
EdgeProjectP2MC* edge;
std::shared_ptr<Frame> frame;
std::shared_ptr<Feature> feature;
};
/// Create a g2o vertice from a keyframe object.
g2o::VertexSE3Expmap* createG2oFrameSE3(
g2o::VertexCam* createG2oFrameSE3(
FramePtr kf,
size_t id,
bool fixed);
@@ -95,8 +94,8 @@ protected:
size_t id,
bool fixed);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
g2o::VertexSE3Expmap* v_kf,
g2o::EdgeProjectP2MC* createG2oEdgeSE3(
g2o::VertexCam* v_kf,
g2o::VertexSBAPointXYZ* v_mp,
const Vector2d& f_up,
bool robust_kernel,
GPU_version/vio/launch/Px30.launch
+1 -1
View File
@@ -1,6 +1,6 @@
<launch>
<!-- node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" / -->
<node pkg="vio" type="vio" name="vio" clear_params="true" output="screen" />
<!-- Camera topic to subscribe to -->
<param name="vio/cam_topic" value="/CoreNode/grey_img" type="str" />
GPU_version/vio/src/frame_handler_mono.cpp
+2 -2
View File
@@ -132,7 +132,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processSecondFrame()
fprintf(log_,"[%s] Init: Selected Second frame. \t The number of features: %d depth mean:%f min:%f\n",
vio::time_in_HH_MM_SS_MMM().c_str(),new_frame_->fts_.size(),depth_mean,depth_min);
#endif
ba_glob_->new_key_frame();
//ba_glob_->new_key_frame();
ROS_INFO("VIO initialized :)");
ROS_INFO("Running ...");
return RESULT_IS_KEYFRAME;
@@ -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(),
fabs(closest_kfs.pitch()-new_frame_->T_f_w_.pitch()),(closest_kfs.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm());
#endif
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;
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;
return false;
}
void FrameHandlerMono::UpdateIMU(double* value,const ros::Time& time){
GPU_version/vio/src/global_optimizer.cpp
+36 -12
View File
@@ -24,6 +24,7 @@
#include <vio/point.h>
#include <vio/feature.h>
#include <vio/config.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#if VIO_DEBUG
#include <sys/types.h>
#include <sys/stat.h>
@@ -103,7 +104,7 @@ namespace vio {
it_kf != map_.keyframes_.end(); ++it_kf)
{
// New Keyframe Vertex
g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
g2o::VertexCam* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
(*it_kf)->v_kf_ = v_kf;
optimizer.addVertex(v_kf);
for(auto&& it_ftr:(*it_kf)->fts_)
@@ -111,6 +112,7 @@ namespace vio {
if(it_ftr->point==NULL)continue;
// for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = it_ftr->point;
if(mp->pos_.hasNaN())continue;
g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
if(v_mp == NULL)
{
@@ -127,7 +129,8 @@ namespace vio {
incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
else
{
g2o::EdgeProjectXYZ2UV* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
g2o::Edge_XYZ_VSC e1;
g2o::EdgeProjectP2MC* e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d(it_ftr->f),
true,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
EdgeContainerSE3 edge;
@@ -143,6 +146,17 @@ namespace vio {
// Optimization
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
g2o::StructureOnlySolver<3> structure_only_ba;
g2o::OptimizableGraph::VertexContainer points;
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);
if (v->dimension() == 3)
points.push_back(v);
}
structure_only_ba.calc(points, 10);
/*
#if VIO_DEBUG
fprintf(log_,"[%s] init error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
@@ -151,7 +165,7 @@ namespace vio {
#if VIO_DEBUG
fprintf(log_,"[%s] end error: %f \n",
vio::time_in_HH_MM_SS_MMM().c_str(),optimizer.activeChi2());
#endif
#endif*/
// Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
it_kf != map_.keyframes_.end();++it_kf)
@@ -161,9 +175,12 @@ namespace vio {
(*it_kf)->v_kf_ = NULL;
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
{
if((*it_ftr)->point == NULL)
continue;
std::shared_ptr<Point> mp = (*it_ftr)->point;
if(mp == NULL)
continue;
if(mp->v_pt_ == NULL)
continue; // mp was updated before
mp->pos_ = mp->v_pt_->estimate();
mp->v_pt_ = NULL;
}
}
@@ -184,13 +201,20 @@ namespace vio {
}
}
g2o::VertexSE3Expmap*
g2o::VertexCam*
BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
{
g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
if(frame->id_==0 || frame->id_==1){
g2o::VertexCam* v= new g2o::VertexCam();
v->setId(id);
v->setToOrigin();
v->setFixed(true);
v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
}
g2o::VertexCam* v= new g2o::VertexCam();
v->setId(id);
v->setFixed(false);
v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
v->setEstimate(g2o::SE3Quat(frame->se3().unit_quaternion(), frame->se3().translation()));
return v;
}
@@ -201,25 +225,25 @@ namespace vio {
{
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id);
v->setFixed(fixed);
//v->setFixed(fixed);
v->setMarginalized(true);
v->setEstimate(pos);
return v;
}
g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
g2o::EdgeProjectP2MC* BA_Glob::createG2oEdgeSE3( g2o::VertexCam* v_frame,
g2o::VertexSBAPointXYZ* v_point,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight)
{
g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
g2o::EdgeProjectP2MC* e= new g2o::EdgeProjectP2MC;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(f_up);
e->information() = weight*Eigen::Matrix2d::Identity();
e->information() = Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
e->setRobustKernel(rk);