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

debug bandel adjustment

Browse Source
This commit is contained in:
gfuse
2021-09-30 15:05:12 +02:00
parent 461d5b3896
commit edc5e8fe96
8 changed files with 994 additions and 254 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+2
View File
@@ -85,5 +85,7 @@ ADD_DEFINITIONS(-DKERNEL_DIR=\"${PROJECT_SOURCE_DIR}/kernel\")
ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\") ADD_DEFINITIONS(-DPROJECT_DIR=\"${PROJECT_SOURCE_DIR}\")
ADD_DEFINITIONS(-DVIO_DEBUG=true) ADD_DEFINITIONS(-DVIO_DEBUG=true)
ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON) ADD_DEFINITIONS(-DG2O_USE_VENDORED_CERES=ON)
ADD_DEFINITIONS(-DG2O_DELETE_IMPLICITLY_OWNED_OBJECTS=OFF)
GPU_version/vio/depth_filter_log.txt
+2
View File
@@ -0,0 +1,2 @@
[15:02:36.165] New key frame
[15:02:36.165] BA loop
GPU_version/vio/frame_handler_log.txt
+929 -212
View File
File diff suppressed because it is too large Load Diff
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. 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. 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? bool is_keyframe_; //!< Was this frames selected as keyframe?
std::shared_ptr<g2o::VertexSE3Expmap> v_kf_; //!< Temporary pointer to the g2o node object of the keyframe. g2o::VertexSE3Expmap* v_kf_=nullptr; //!< Temporary pointer to the g2o node object of the keyframe.
int last_published_ts_; //!< Timestamp of last publishing. int last_published_ts_; //!< Timestamp of last publishing.
Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp); Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp);
GPU_version/vio/include/vio/global_optimizer.h
+11 -6
View File
@@ -34,6 +34,11 @@
#include <g2o/solvers/csparse/linear_solver_csparse.h> #include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h> #include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/structure_only/structure_only_solver.h> #include <g2o/solvers/structure_only/structure_only_solver.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/stuff/sampler.h>
#include <memory>
G2O_USE_OPTIMIZATION_LIBRARY(cholmod)
namespace vio { namespace vio {
@@ -76,7 +81,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{
std::shared_ptr<g2o::EdgeProjectXYZ2UV> edge; g2o::EdgeProjectXYZ2UV* edge;
std::shared_ptr<Frame> frame; std::shared_ptr<Frame> frame;
std::shared_ptr<Feature> feature; std::shared_ptr<Feature> feature;
}; };
@@ -90,19 +95,19 @@ protected:
double& final_error); double& final_error);
/// Create a g2o vertice from a keyframe object. /// Create a g2o vertice from a keyframe object.
std::shared_ptr<g2o::VertexSE3Expmap> createG2oFrameSE3( g2o::VertexSE3Expmap* createG2oFrameSE3(
FramePtr kf, FramePtr kf,
size_t id, size_t id,
bool fixed); bool fixed);
/// Creates a g2o vertice from a mappoint object. /// Creates a g2o vertice from a mappoint object.
std::shared_ptr<g2o::VertexPointXYZ> createG2oPoint( g2o::VertexSBAPointXYZ* createG2oPoint(
Vector3d pos, Vector3d pos,
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.
std::shared_ptr<g2o::EdgeProjectXYZ2UV> createG2oEdgeSE3( g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
std::shared_ptr<g2o::VertexSE3Expmap> v_kf, g2o::VertexSE3Expmap* v_kf,
std::shared_ptr<g2o::VertexPointXYZ> v_mp, g2o::VertexSBAPointXYZ* v_mp,
const Vector2d& f_up, const Vector2d& f_up,
bool robust_kernel, bool robust_kernel,
double huber_width, double huber_width,
GPU_version/vio/include/vio/point.h
+1 -1
View File
@@ -51,7 +51,7 @@ public:
bool normal_set_; //!< Flag whether the surface normal was estimated or not. bool normal_set_; //!< Flag whether the surface normal was estimated or not.
list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point. list<std::shared_ptr<Feature>> obs_; //!< References to keyframes which observe the point.
size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames. size_t n_obs_; //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
std::shared_ptr<g2o::VertexPointXYZ> v_pt_; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment. g2o::VertexSBAPointXYZ* v_pt_=nullptr; //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
int last_published_ts_; //!< Timestamp of last publishing. int last_published_ts_; //!< Timestamp of last publishing.
int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice. int last_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice.
PointType type_; //!< Quality of the point. PointType type_; //!< Quality of the point.
GPU_version/vio/src/feature_detection.cpp
+2
View File
@@ -76,6 +76,7 @@ void FastDetector::detect(
fts.clear(); fts.clear();
for(int L=0; L<n_pyr_levels_; ++L) for(int L=0; L<n_pyr_levels_; ++L)
{ {
if(L>img_pyr.size())return;
const int scale = (1<<L); const int scale = (1<<L);
cv::Mat img=img_pyr.at(L); cv::Mat img=img_pyr.at(L);
if(!gpu_fast_->reload_buf(0,0,img)){ if(!gpu_fast_->reload_buf(0,0,img)){
@@ -98,6 +99,7 @@ void FastDetector::detect(
} }
for(uint i=0;i<count[0];++i) for(uint i=0;i<count[0];++i)
{ {
if(fast_corners==NULL)return;
if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){ if(fast_corners[i].x<1 || fast_corners[i].x>img_pyr[L].cols || fast_corners[i].y>img_pyr[L].rows || fast_corners[i].y<1){
continue; continue;
} }
GPU_version/vio/src/global_optimizer.cpp
+46 -34
View File
@@ -33,7 +33,7 @@ namespace vio {
BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL) BA_Glob::BA_Glob(Map& map) :map_(map),thread_(NULL)
{ {
setupG2o();
#if VIO_DEBUG #if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+"); log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
assert(log_); assert(log_);
@@ -77,34 +77,48 @@ namespace vio {
vio::time_in_HH_MM_SS_MMM().c_str()); vio::time_in_HH_MM_SS_MMM().c_str());
#endif #endif
new_keyframe_=false; new_keyframe_=false;
std::cerr<<"here\t"; g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver=new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// setup camera
g2o::CameraParameters* cam_params = new g2o::CameraParameters(1.0, Vector2d(0.,0.), 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
// init g2o // init g2o
list<EdgeContainerSE3> edges; list<EdgeContainerSE3> edges;
list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges; list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
// Go through all Keyframes // Go through all Keyframes
size_t v_id = 0; size_t v_id = 0;
point_mut_.lock(); point_mut_.lock();
std::cerr<<"78\t";
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)
{ {
// New Keyframe Vertex // New Keyframe Vertex
std::shared_ptr<g2o::VertexSE3Expmap> v_kf = createG2oFrameSE3(*it_kf, v_id++, false); g2o::VertexSE3Expmap* v_kf = createG2oFrameSE3(*it_kf, v_id++, false);
(*it_kf)->v_kf_ = v_kf; (*it_kf)->v_kf_ = v_kf;
optimizer_->addVertex(v_kf.get()); optimizer.addVertex(v_kf);
for(auto&& it_ftr:(*it_kf)->fts_) for(auto&& it_ftr:(*it_kf)->fts_)
{ {
// for each keyframe add edges to all observed mapoints // for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = it_ftr->point; std::shared_ptr<Point> mp = it_ftr->point;
if(mp == NULL) if(mp == NULL)
continue; continue;
std::shared_ptr<g2o::VertexPointXYZ> v_mp = mp->v_pt_; g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
if(v_mp == NULL) if(v_mp == NULL)
{ {
// mappoint-vertex doesn't exist yet. create a new one: // mappoint-vertex doesn't exist yet. create a new one:
v_mp = createG2oPoint(mp->pos_, v_id++, false); v_mp = createG2oPoint(mp->pos_, v_id++, false);
mp->v_pt_ = v_mp; mp->v_pt_ = v_mp;
optimizer_->addVertex(v_mp.get()); optimizer.addVertex(v_mp);
} }
// Due to merging of mappoints it is possible that references in kfs suddenly // Due to merging of mappoints it is possible that references in kfs suddenly
@@ -114,7 +128,7 @@ namespace vio {
incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr)); incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
else else
{ {
std::shared_ptr<g2o::EdgeProjectXYZ2UV> 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;
@@ -122,11 +136,10 @@ namespace vio {
edge.edge=e; edge.edge=e;
edge.feature=it_ftr; edge.feature=it_ftr;
edges.push_back(edge); edges.push_back(edge);
optimizer_->addEdge(e.get()); optimizer.addEdge(e);
} }
} }
} }
std::cerr<<"129\t";
// Optimization // Optimization
double init_error=0.0, final_error=0.0; double init_error=0.0, final_error=0.0;
@@ -151,7 +164,6 @@ namespace vio {
mp->v_pt_ = NULL; mp->v_pt_ = NULL;
} }
} }
std::cerr<<"154\t";
// Remove Measurements with too large reprojection error // Remove Measurements with too large reprojection error
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin(); for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
@@ -165,8 +177,6 @@ namespace vio {
map_.removePtFrameRef(it->frame, it->feature); map_.removePtFrameRef(it->frame, it->feature);
} }
} }
std::cerr<<"168\n";
optimizer_->clear();
point_mut_.unlock(); point_mut_.unlock();
} }
} }
@@ -174,21 +184,20 @@ namespace vio {
void BA_Glob::setupG2o() void BA_Glob::setupG2o()
{ {
// TODO: What's happening with all this HEAP stuff? Memory Leak? // TODO: What's happening with all this HEAP stuff? Memory Leak?
optimizer_=std::make_shared<g2o::SparseOptimizer>(); /* optimizer_=std::make_shared<g2o::SparseOptimizer>();
optimizer_->setVerbose(false); optimizer_->setVerbose(false);
using LinearSolver = g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>; auto linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
Levenberg_ = std::make_shared<g2o::OptimizationAlgorithmLevenberg>( g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<g2o::BlockSolver_6_3>(g2o::make_unique<LinearSolver>())); g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
Levenberg_->setMaxTrialsAfterFailure(5); optimizer_->setAlgorithm(solver);
optimizer_->setAlgorithm(Levenberg_.get());
// setup camera // setup camera
std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.); std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
cam_params->setId(0); cam_params->setId(0);
if (!optimizer_->addParameter(cam_params.get())) { if (!optimizer_->addParameter(cam_params.get())) {
assert(false); assert(false);*/
} // }
} }
void void
@@ -202,42 +211,45 @@ namespace vio {
final_error = optimizer_->activeChi2(); final_error = optimizer_->activeChi2();
} }
std::shared_ptr<g2o::VertexSE3Expmap> g2o::VertexSE3Expmap*
BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed) BA_Glob::createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
{ {
std::shared_ptr<g2o::VertexSE3Expmap> v=std::make_shared<g2o::VertexSE3Expmap>(); g2o::VertexSE3Expmap* v= new g2o::VertexSE3Expmap();
v->setId(id); v->setId(id);
v->setFixed(fixed); v->setFixed(fixed);
v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation())); v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
return v; return v;
} }
std::shared_ptr<g2o::VertexPointXYZ> g2o::VertexSBAPointXYZ*
BA_Glob::createG2oPoint(Vector3d pos, BA_Glob::createG2oPoint(Vector3d pos,
size_t id, size_t id,
bool fixed) bool fixed)
{ {
std::shared_ptr<g2o::VertexPointXYZ> v =std::make_shared<g2o::VertexPointXYZ>(); g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id); v->setId(id);
v->setFixed(fixed); v->setFixed(fixed);
v->setEstimate(pos); v->setEstimate(pos);
return v; return v;
} }
std::shared_ptr<g2o::EdgeProjectXYZ2UV> BA_Glob::createG2oEdgeSE3( std::shared_ptr<g2o::VertexSE3Expmap> v_frame, g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
std::shared_ptr<g2o::VertexPointXYZ> 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)
{ {
std::shared_ptr<g2o::EdgeProjectXYZ2UV> e=std::make_shared<g2o::EdgeProjectXYZ2UV>(); g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
e->setVertex(0, (g2o::OptimizableGraph::Vertex*)(v_point).get()); e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, (g2o::OptimizableGraph::Vertex*)(v_frame).get()); e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(f_up); e->setMeasurement(Vector2d(static_cast<int>(g2o::Sampler::uniformRand(0, 640)),
e->information() = weight * Eigen::Matrix2d::Identity(2,2); static_cast<int>(g2o::Sampler::uniformRand(0, 480))));
e->information() = Eigen::Matrix2d::Identity();
e->setRobustKernel(new g2o::RobustKernelHuber()); g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
e->setRobustKernel(rk);
e->setParameterId(0, 0); //old: e->setId(v_point->id()); e->setParameterId(0, 0); //old: e->setId(v_point->id());
return e; return e;
} }