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(-DVIO_DEBUG=true)
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.
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?
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.
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/dense/linear_solver_dense.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 {
@@ -76,7 +81,7 @@ protected:
#endif
/// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3{
std::shared_ptr<g2o::EdgeProjectXYZ2UV> edge;
g2o::EdgeProjectXYZ2UV* edge;
std::shared_ptr<Frame> frame;
std::shared_ptr<Feature> feature;
};
@@ -90,19 +95,19 @@ protected:
double& final_error);
/// Create a g2o vertice from a keyframe object.
std::shared_ptr<g2o::VertexSE3Expmap> createG2oFrameSE3(
g2o::VertexSE3Expmap* createG2oFrameSE3(
FramePtr kf,
size_t id,
bool fixed);
/// Creates a g2o vertice from a mappoint object.
std::shared_ptr<g2o::VertexPointXYZ> createG2oPoint(
g2o::VertexSBAPointXYZ* createG2oPoint(
Vector3d pos,
size_t id,
bool fixed);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
std::shared_ptr<g2o::EdgeProjectXYZ2UV> createG2oEdgeSE3(
std::shared_ptr<g2o::VertexSE3Expmap> v_kf,
std::shared_ptr<g2o::VertexPointXYZ> v_mp,
g2o::EdgeProjectXYZ2UV* createG2oEdgeSE3(
g2o::VertexSE3Expmap* v_kf,
g2o::VertexSBAPointXYZ* v_mp,
const Vector2d& f_up,
bool robust_kernel,
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.
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.
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_frame_overlap_id_; //!< Flag for the reprojection: don't reproject a pt twice.
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();
for(int L=0; L<n_pyr_levels_; ++L)
{
if(L>img_pyr.size())return;
const int scale = (1<<L);
cv::Mat img=img_pyr.at(L);
if(!gpu_fast_->reload_buf(0,0,img)){
@@ -98,6 +99,7 @@ void FastDetector::detect(
}
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){
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)
{
setupG2o();
#if VIO_DEBUG
log_ =fopen((std::string(PROJECT_DIR)+"/depth_filter_log.txt").c_str(),"w+");
assert(log_);
@@ -77,34 +77,48 @@ namespace vio {
vio::time_in_HH_MM_SS_MMM().c_str());
#endif
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
list<EdgeContainerSE3> edges;
list< pair<FramePtr,std::shared_ptr<Feature>> > incorrect_edges;
// Go through all Keyframes
size_t v_id = 0;
point_mut_.lock();
std::cerr<<"78\t";
for(list<FramePtr>::iterator it_kf = map_.keyframes_.begin();
it_kf != map_.keyframes_.end(); ++it_kf)
{
// 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;
optimizer_->addVertex(v_kf.get());
optimizer.addVertex(v_kf);
for(auto&& it_ftr:(*it_kf)->fts_)
{
// for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = it_ftr->point;
if(mp == NULL)
continue;
std::shared_ptr<g2o::VertexPointXYZ> v_mp = mp->v_pt_;
g2o::VertexSBAPointXYZ* v_mp = mp->v_pt_;
if(v_mp == NULL)
{
// mappoint-vertex doesn't exist yet. create a new one:
v_mp = createG2oPoint(mp->pos_, v_id++, false);
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
@@ -114,7 +128,7 @@ namespace vio {
incorrect_edges.push_back(pair<FramePtr,std::shared_ptr<Feature>>(*it_kf, it_ftr));
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,
Config::poseOptimThresh()/(*it_kf)->cam_->errorMultiplier2()*Config::lobaRobustHuberWidth());
EdgeContainerSE3 edge;
@@ -122,11 +136,10 @@ namespace vio {
edge.edge=e;
edge.feature=it_ftr;
edges.push_back(edge);
optimizer_->addEdge(e.get());
optimizer.addEdge(e);
}
}
}
std::cerr<<"129\t";
// Optimization
double init_error=0.0, final_error=0.0;
@@ -151,7 +164,6 @@ namespace vio {
mp->v_pt_ = NULL;
}
}
std::cerr<<"154\t";
// Remove Measurements with too large reprojection error
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);
}
}
std::cerr<<"168\n";
optimizer_->clear();
point_mut_.unlock();
}
}
@@ -174,21 +184,20 @@ namespace vio {
void BA_Glob::setupG2o()
{
// 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);
using LinearSolver = g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>;
Levenberg_ = std::make_shared<g2o::OptimizationAlgorithmLevenberg>(
g2o::make_unique<g2o::BlockSolver_6_3>(g2o::make_unique<LinearSolver>()));
auto linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
Levenberg_->setMaxTrialsAfterFailure(5);
optimizer_->setAlgorithm(Levenberg_.get());
optimizer_->setAlgorithm(solver);
// setup camera
std::shared_ptr<g2o::CameraParameters> cam_params = std::make_shared<g2o::CameraParameters>(1.0, Vector2d(0.,0.), 0.);
cam_params->setId(0);
if (!optimizer_->addParameter(cam_params.get())) {
assert(false);
}
assert(false);*/
// }
}
void
@@ -202,42 +211,45 @@ namespace vio {
final_error = optimizer_->activeChi2();
}
std::shared_ptr<g2o::VertexSE3Expmap>
g2o::VertexSE3Expmap*
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->setFixed(fixed);
v->setEstimate(g2o::SE3Quat(frame->T_f_w_.se3().unit_quaternion(), frame->T_f_w_.se3().translation()));
return v;
}
std::shared_ptr<g2o::VertexPointXYZ>
g2o::VertexSBAPointXYZ*
BA_Glob::createG2oPoint(Vector3d pos,
size_t id,
bool fixed)
{
std::shared_ptr<g2o::VertexPointXYZ> v =std::make_shared<g2o::VertexPointXYZ>();
g2o::VertexSBAPointXYZ* v =new g2o::VertexSBAPointXYZ();
v->setId(id);
v->setFixed(fixed);
v->setEstimate(pos);
return v;
}
std::shared_ptr<g2o::EdgeProjectXYZ2UV> BA_Glob::createG2oEdgeSE3( std::shared_ptr<g2o::VertexSE3Expmap> v_frame,
std::shared_ptr<g2o::VertexPointXYZ> v_point,
g2o::EdgeProjectXYZ2UV* BA_Glob::createG2oEdgeSE3( g2o::VertexSE3Expmap* v_frame,
g2o::VertexSBAPointXYZ* v_point,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight)
{
std::shared_ptr<g2o::EdgeProjectXYZ2UV> e=std::make_shared<g2o::EdgeProjectXYZ2UV>();
e->setVertex(0, (g2o::OptimizableGraph::Vertex*)(v_point).get());
e->setVertex(1, (g2o::OptimizableGraph::Vertex*)(v_frame).get());
e->setMeasurement(f_up);
e->information() = weight * Eigen::Matrix2d::Identity(2,2);
e->setRobustKernel(new g2o::RobustKernelHuber());
g2o::EdgeProjectXYZ2UV* e= new g2o::EdgeProjectXYZ2UV;
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_point));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_frame));
e->setMeasurement(Vector2d(static_cast<int>(g2o::Sampler::uniformRand(0, 640)),
static_cast<int>(g2o::Sampler::uniformRand(0, 480))));
e->information() = Eigen::Matrix2d::Identity();
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
rk->setDelta(huber_width);
e->setRobustKernel(rk);
e->setParameterId(0, 0); //old: e->setId(v_point->id());
return e;
}