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

add 7 degrees for camera and g2o dependency

Browse Source
This commit is contained in:
gfuse
2021-09-18 15:17:03 +02:00
parent 81c302ab2a
commit db6bacfe4d
10 changed files with 14715 additions and 14375 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GPU_version/vio/CMakeLists.txt
+6 -2
View File
@@ -28,8 +28,9 @@ FIND_PACKAGE(OpenCV 4 REQUIRED)
FIND_PACKAGE(Eigen REQUIRED)
FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
FIND_PACKAGE(OpenCL REQUIRED)
FIND_PACKAGE(G2O REQUIRED)
set(GTSAM_INCLUDE_DIRS "${GTSAM_INCLUDE_DIR}")
FIND_PACKAGE(catkin REQUIRED COMPONENTS roscpp roslib cmake_modules image_transport cv_bridge tf geometry_msgs message_generation)
add_service_files(FILES
@@ -52,15 +53,18 @@ INCLUDE_DIRECTORIES(
${Boost_INCLUDE_DIRS}
${OpenCL_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${G2O_INCLUDE_DIR}
/usr/include/suitesparse # for cholmod
)
# Set link libraries
LIST(APPEND LINK_LIBS
#gtsam
${OpenCV_LIBS}
${Boost_LIBRARIES}
${OpenCL_LIBRARIES}
${catkin_LIBRARIES}
${G2O_CORE_LIBRARY} ${G2O_STUFF_LIBRARY} ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_DENSE}
${G2O_SOLVER_PCG} ${G2O_TYPES_SBA} cholmod cxsparse
)
file(GLOB SRC CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp")
GPU_version/vio/depth_filter_log.txt
+13222 -13418
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/frame_handler_log.txt
+985 -878
View File
File diff suppressed because it is too large Load Diff
GPU_version/vio/include/vio/bundle_adjustment.h
+57 -72
View File
@@ -1,109 +1,94 @@
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
// Created by root on 9/18/21.
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef SVO_BUNDLE_ADJUSTMENT_H_
#define SVO_BUNDLE_ADJUSTMENT_H_
#ifndef VIO_BUNDLE_ADJUSTMENT_H
#define VIO_BUNDLE_ADJUSTMENT_H
#include <vio/global.h>
namespace g2o {
class EdgeProjectXYZ2UV;
class SparseOptimizer;
class VertexSE3Expmap;
class VertexSBAPointXYZ;
class EdgeProjectXYZ2UV;
class SparseOptimizer;
class VertexSE2Expmap;
class VertexSBAPointXYZ;
}
namespace vio {
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
typedef g2o::VertexSE3Expmap g2oFrameSE3;
typedef g2o::VertexSBAPointXYZ g2oPoint;
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
typedef g2o::VertexSE2Expmap g2oFrameSE2;
typedef g2o::VertexSBAPointXYZ g2oPoint;
class Frame;
class Point;
class Feature;
class Map;
class Frame;
class Point;
class Feature;
class Map;
/// Local, global and 2-view bundle adjustment with g2o
namespace ba {
/// Temporary container to hold the g2o edge with reference to frame and point.
struct EdgeContainerSE3
{
g2oEdgeSE3* edge;
Frame* frame;
Feature* feature;
bool is_deleted;
EdgeContainerSE3(g2oEdgeSE3* e, Frame* frame, Feature* feature) :
edge(e), frame(frame), feature(feature), is_deleted(false)
{}
};
struct EdgeContainerSE3{
std::shared_ptr<g2oEdgeSE3> edge;
std::shared_ptr<Frame> frame;
std::shared_ptr<Feature> feature;
bool is_deleted;
EdgeContainerSE3(std::shared_ptr<g2oEdgeSE3> e, std::shared_ptr<Frame> frame, std::shared_ptr<Feature> feature) :
edge(e), frame(frame), feature(feature), is_deleted(false)
{}
};
/// Optimize two camera frames and their observed 3D points.
/// Is used after initialization.
void twoViewBA(Frame* frame1, Frame* frame2, double reproj_thresh, Map* map);
void twoViewBA(std::shared_ptr<Frame> frame1, std::shared_ptr<Frame> frame2, double reproj_thresh, Map* map);
/// Local bundle adjustment.
/// Optimizes core_kfs and their observed map points while keeping the
/// neighbourhood fixed.
void localBA(
Frame* center_kf,
set<FramePtr>* core_kfs,
Map* map,
size_t& n_incorrect_edges_1,
size_t& n_incorrect_edges_2,
double& init_error,
double& final_error);
void localBA(
std::shared_ptr<Frame> center_kf,
set<FramePtr>* core_kfs,
Map* map,
size_t& n_incorrect_edges_1,
size_t& n_incorrect_edges_2,
double& init_error,
double& final_error);
/// Global bundle adjustment.
/// Optimizes the whole map. Is currently not used in SVO.
void globalBA(Map* map);
void globalBA(Map* map);
/// Initialize g2o with solver type, optimization strategy and camera model.
void setupG2o(g2o::SparseOptimizer * optimizer);
void setupG2o(g2o::SparseOptimizer * optimizer);
/// Run the optimization on the provided graph.
void runSparseBAOptimizer(
g2o::SparseOptimizer* optimizer,
unsigned int num_iter,
double& init_error,
double& final_error);
void runSparseBAOptimizer(
g2o::SparseOptimizer* optimizer,
unsigned int num_iter,
double& init_error,
double& final_error);
/// Create a g2o vertice from a keyframe object.
g2oFrameSE3* createG2oFrameSE3(
Frame* kf,
size_t id,
bool fixed);
std::shared_ptr<g2oFrameSE2> createG2oFrameSE2(
FramePtr kf,
size_t id,
bool fixed);
/// Creates a g2o vertice from a mappoint object.
g2oPoint* createG2oPoint(
Vector3d pos,
size_t id,
bool fixed);
g2oPoint* createG2oPoint(
Vector3d pos,
size_t id,
bool fixed);
/// Creates a g2o edge between a g2o keyframe and mappoint vertice with the provided measurement.
g2oEdgeSE3* createG2oEdgeSE3(
g2oFrameSE3* v_kf,
g2oPoint* v_mp,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight = 1);
std::shared_ptr<g2oEdgeSE3> createG2oEdgeSE3(
std::shared_ptr<g2oFrameSE2> v_kf,
std::shared_ptr<g2oPoint> v_mp,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight = 1);
} // namespace ba
} // namespace vio
} // namespace svo
#endif // SVO_BUNDLE_ADJUSTMENT_H_
#endif //VIO_BUNDLE_ADJUSTMENT_H
GPU_version/vio/include/vio/frame.h
+1 -1
View File
@@ -132,7 +132,7 @@ namespace vio {
//TODO add 15 degree roll orientation
SE2 inv=T_f_w_.inverse();
Quaterniond q;
q = AngleAxisd(0.0, Vector3d::UnitX())
q = AngleAxisd(-0.122173, Vector3d::UnitX())
* AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0), 0.0,inv.translation()(1)));
GPU_version/vio/include/vio/global.h
+1 -1
View File
@@ -122,7 +122,7 @@ namespace vio
//Todo add 15 roll orientation
//Camera frame z front, x right, y down -> right hands
Quaterniond q;
q = AngleAxisd(0.0, Vector3d::UnitX()) // roll
q = AngleAxisd(-0.122173, Vector3d::UnitX()) // roll
* AngleAxisd(pitch(), Vector3d::UnitY())// pitch
* AngleAxisd(0.0, Vector3d::UnitZ()); //yaw
SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
GPU_version/vio/kernel/compute-residual.cl
+1 -1
View File
@@ -26,7 +26,7 @@ float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
float3 error=ref+cur;
float yaw=0.0;
float pitch=error.z;
float roll=-0.26;
float roll=-0.122173;
float R00=cos(yaw)*cos(pitch);//cos(pitch)
float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)
GPU_version/vio/src/bundle_adjustment.cpp
+440
View File
@@ -0,0 +1,440 @@
//
// Created by root on 9/18/21.
//
#include "vio/bundle_adjustment.h"
// This file is part of SVO - Semi-direct Visual Odometry.
//
// Copyright (C) 2014 Christian Forster <forster at ifi dot uzh dot ch>
// (Robotics and Perception Group, University of Zurich, Switzerland).
//
// SVO is free software: you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the Free Software
// Foundation, either version 3 of the License, or any later version.
//
// SVO is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
/*
#include <vio/math_utils.h>
#include <boost/thread.hpp>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#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/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/structure_only/structure_only_solver.h>
#include <vio/bundle_adjustment.h>
#include <vio/frame.h>
#include <vio/feature.h>
#include <vio/point.h>
#include <vio/config.h>
#include <vio/map.h>
#define SCHUR_TRICK 1
namespace vio {
void twoViewBA(
FramePtr frame1,
FramePtr frame2,
double reproj_thresh,
Map* map)
{
// scale reprojection threshold in pixels to unit plane
reproj_thresh /= frame1->cam_->errorMultiplier2();
// init g2o
g2o::SparseOptimizer optimizer;
setupG2o(&optimizer);
list<EdgeContainerSE3> edges;
size_t v_id = 0;
// New Keyframe Vertex 1: This Keyframe is set to fixed!
std::shared_ptr<g2oFrameSE2> v_frame1 = createG2oFrameSE2(frame1, v_id++, true);
optimizer.addVertex(v_frame1);
// New Keyframe Vertex 2
std::shared_ptr<g2oFrameSE2> v_frame2 = createG2oFrameSE2(frame2, v_id++, false);
optimizer.addVertex(v_frame2);
// Create Point Vertices
for(Features::iterator it_ftr=frame1->fts_.begin(); it_ftr!=frame1->fts_.end(); ++it_ftr)
{
std::shared_ptr<Point> pt = (*it_ftr)->point;
if(pt == NULL)
continue;
std::shared_ptr<g2oPoint> v_pt = createG2oPoint(pt->pos_, v_id++, false);
optimizer.addVertex(v_pt);
pt->v_pt_ = v_pt;
std::shared_ptr<g2oEdgeSE3> e = createG2oEdgeSE3(v_frame1, v_pt, vk::project2d((*it_ftr)->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
optimizer.addEdge(e);
edges.push_back(EdgeContainerSE3(e, frame1, *it_ftr)); // TODO feature now links to frame, so we can simplify edge container!
// find at which index the second frame observes the point
std::shared_ptr<Feature> ftr_frame2 = pt->findFrameRef(frame2);
e = createG2oEdgeSE3(v_frame2, v_pt, vk::project2d(ftr_frame2->f), true, reproj_thresh*Config::lobaRobustHuberWidth());
optimizer.addEdge(e);
edges.push_back(EdgeContainerSE3(e, frame2, ftr_frame2));
}
// Optimization
double init_error, final_error;
runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
printf("2-View BA: Error before/after = %f / %f\n", init_error, final_error);
// Update Keyframe Positions
frame1->T_f_w_.rotation_matrix() = v_frame1->estimate().rotation().toRotationMatrix();
frame1->T_f_w_.translation() = v_frame1->estimate().translation();
frame2->T_f_w_.rotation_matrix() = v_frame2->estimate().rotation().toRotationMatrix();
frame2->T_f_w_.translation() = v_frame2->estimate().translation();
// Update Mappoint Positions
for(Features::iterator it=frame1->fts_.begin(); it!=frame1->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
(*it)->point->pos_ = (*it)->point->v_pt_->estimate();
(*it)->point->v_pt_ = NULL;
}
// Find Mappoints with too large reprojection error
const double reproj_thresh_squared = reproj_thresh*reproj_thresh;
size_t n_incorrect_edges = 0;
for(list<EdgeContainerSE3>::iterator it_e = edges.begin(); it_e != edges.end(); ++it_e)
if(it_e->edge->chi2() > reproj_thresh_squared)
{
if(it_e->feature->point != NULL)
{
map->safeDeletePoint(it_e->feature->point);
it_e->feature->point = NULL;
}
++n_incorrect_edges;
}
printf("2-View BA: Wrong edges = %zu\n", n_incorrect_edges);
}
void localBA(
FramePtr center_kf,
set<FramePtr>* core_kfs,
Map* map,
size_t& n_incorrect_edges_1,
size_t& n_incorrect_edges_2,
double& init_error,
double& final_error)
{
// init g2o
g2o::SparseOptimizer optimizer;
setupG2o(&optimizer);
list<EdgeContainerSE3> edges;
set<std::shared_ptr<Point>> mps;
list<FramePtr> neib_kfs;
size_t v_id = 0;
size_t n_mps = 0;
size_t n_fix_kfs = 0;
size_t n_var_kfs = 1;
size_t n_edges = 0;
n_incorrect_edges_1 = 0;
n_incorrect_edges_2 = 0;
// Add all core keyframes
for(set<FramePtr>::iterator it_kf = core_kfs->begin(); it_kf != core_kfs->end(); ++it_kf)
{
std::shared_ptr<g2oFrameSE2> v_kf = createG2oFrameSE2(it_kf->get(), v_id++, false);
(*it_kf)->v_kf_ = v_kf;
++n_var_kfs;
assert(optimizer.addVertex(v_kf));
// all points that the core keyframes observe are also optimized:
for(Features::iterator it_pt=(*it_kf)->fts_.begin(); it_pt!=(*it_kf)->fts_.end(); ++it_pt)
if((*it_pt)->point != NULL)
mps.insert((*it_pt)->point);
}
// Now go throug all the points and add a measurement. Add a fixed neighbour
// Keyframe if it is not in the set of core kfs
double reproj_thresh_2 = Config::lobaThresh() / center_kf->cam_->errorMultiplier2();
double reproj_thresh_1 = Config::poseOptimThresh() / center_kf->cam_->errorMultiplier2();
double reproj_thresh_1_squared = reproj_thresh_1*reproj_thresh_1;
for(set<Point*>::iterator it_pt = mps.begin(); it_pt!=mps.end(); ++it_pt)
{
// Create point vertex
g2oPoint* v_pt = createG2oPoint((*it_pt)->pos_, v_id++, false);
(*it_pt)->v_pt_ = v_pt;
assert(optimizer.addVertex(v_pt));
++n_mps;
// Add edges
list<Feature*>::iterator it_obs=(*it_pt)->obs_.begin();
while(it_obs!=(*it_pt)->obs_.end())
{
Vector2d error = vk::project2d((*it_obs)->f) - vk::project2d((*it_obs)->frame->w2f((*it_pt)->pos_));
if((*it_obs)->frame->v_kf_ == NULL)
{
// frame does not have a vertex yet -> it belongs to the neib kfs and
// is fixed. create one:
std::shared_ptr<g2oFrameSE2> v_kf = createG2oFrameSE2((*it_obs)->frame, v_id++, true);
(*it_obs)->frame->v_kf_ = v_kf;
++n_fix_kfs;
assert(optimizer.addVertex(v_kf));
neib_kfs.push_back((*it_obs)->frame);
}
// create edge
std::shared_ptr<g2oEdgeSE3> e = createG2oEdgeSE3((*it_obs)->frame->v_kf_, v_pt,
vk::project2d((*it_obs)->f),
true,
reproj_thresh_2*Config::lobaRobustHuberWidth(),
1.0 / (1<<(*it_obs)->level));
assert(optimizer.addEdge(e));
edges.push_back(EdgeContainerSE3(e, (*it_obs)->frame, *it_obs));
++n_edges;
++it_obs;
}
}
// structure only
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 && v->edges().size() >= 2)
points.push_back(v);
}
structure_only_ba.calc(points, 10);
// Optimization
if(Config::lobaNumIter() > 0)
runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
// Update Keyframes
for(set<FramePtr>::iterator it = core_kfs->begin(); it != core_kfs->end(); ++it)
{
(*it)->T_f_w_ = SE3( (*it)->v_kf_->estimate().rotation(),
(*it)->v_kf_->estimate().translation());
(*it)->v_kf_ = NULL;
}
for(list<FramePtr>::iterator it = neib_kfs.begin(); it != neib_kfs.end(); ++it)
(*it)->v_kf_ = NULL;
// Update Mappoints
for(set<std::shared_ptr<Point>>::iterator it = mps.begin(); it != mps.end(); ++it)
{
(*it)->pos_ = (*it)->v_pt_->estimate();
(*it)->v_pt_ = NULL;
}
// Remove Measurements with too large reprojection error
double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
{
if(it->edge->chi2() > reproj_thresh_2_squared) //*(1<<it->feature_->level))
{
map->removePtFrameRef(it->frame, it->feature);
++n_incorrect_edges_2;
}
}
// TODO: delete points and edges!
init_error = sqrt(init_error)*center_kf->cam_->errorMultiplier2();
final_error = sqrt(final_error)*center_kf->cam_->errorMultiplier2();
}
void globalBA(Map* map)
{
// init g2o
g2o::SparseOptimizer optimizer;
setupG2o(&optimizer);
list<EdgeContainerSE3> edges;
list< pair<FramePtr,Feature*> > incorrect_edges;
// Go through all Keyframes
size_t v_id = 0;
double reproj_thresh_2 = Config::lobaThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
double reproj_thresh_1_squared = Config::poseOptimThresh() / map->lastKeyframe()->cam_->errorMultiplier2();
reproj_thresh_1_squared *= reproj_thresh_1_squared;
for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
it_kf != map->keyframes_.end(); ++it_kf)
{
// New Keyframe Vertex
std::shared_ptr<g2oFrameSE2> v_kf = createG2oFrameSE2(it_kf->get(), v_id++, false);
(*it_kf)->v_kf_ = v_kf;
optimizer.addVertex(v_kf);
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
{
// for each keyframe add edges to all observed mapoints
std::shared_ptr<Point> mp = (*it_ftr)->point;
if(mp == NULL)
continue;
g2oPoint* 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);
}
// Due to merging of mappoints it is possible that references in kfs suddenly
// have a very large reprojection error which may result in distorted results.
Vector2d error = vk::project2d((*it_ftr)->f) - vk::project2d((*it_kf)->w2f(mp->pos_));
if(error.squaredNorm() > reproj_thresh_1_squared)
incorrect_edges.push_back(pair<FramePtr,Feature*>(*it_kf, *it_ftr));
else
{
std::shared_ptr<g2oEdgeSE3> e = createG2oEdgeSE3(v_kf, v_mp, vk::project2d((*it_ftr)->f),
true,
reproj_thresh_2*Config::lobaRobustHuberWidth());
edges.push_back(EdgeContainerSE3(e, it_kf->get(), *it_ftr));
optimizer.addEdge(e);
}
}
}
// Optimization
double init_error=0.0, final_error=0.0;
if(Config::lobaNumIter() > 0)
runSparseBAOptimizer(&optimizer, Config::lobaNumIter(), init_error, final_error);
// Update Keyframe and MapPoint Positions
for(list<FramePtr>::iterator it_kf = map->keyframes_.begin();
it_kf != map->keyframes_.end(); ++it_kf)
{
(*it_kf)->T_f_w_ = SE3( (*it_kf)->v_kf_->estimate().rotation(),
(*it_kf)->v_kf_->estimate().translation());
(*it_kf)->v_kf_ = NULL;
for(Features::iterator it_ftr=(*it_kf)->fts_.begin(); it_ftr!=(*it_kf)->fts_.end(); ++it_ftr)
{
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;
}
}
// Remove Measurements with too large reprojection error
for(list< pair<FramePtr,std::shared_ptr<Feature>> >::iterator it=incorrect_edges.begin();
it!=incorrect_edges.end(); ++it)
map->removePtFrameRef(it->first.get(), it->second);
double reproj_thresh_2_squared = reproj_thresh_2*reproj_thresh_2;
for(list<EdgeContainerSE3>::iterator it = edges.begin(); it != edges.end(); ++it)
{
if(it->edge->chi2() > reproj_thresh_2_squared)
{
map->removePtFrameRef(it->frame, it->feature);
}
}
}
void setupG2o(g2o::SparseOptimizer * optimizer)
{
// TODO: What's happening with all this HEAP stuff? Memory Leak?
optimizer->setVerbose(false);
#if SCHUR_TRICK
// solver
g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
//linearSolver = new g2o::LinearSolverCSparse<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);
#else
g2o::BlockSolverX::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolverX::PoseMatrixType>();
//linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolverX::PoseMatrixType>();
g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
#endif
solver->setMaxTrialsAfterFailure(5);
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);
}
}
void
runSparseBAOptimizer(g2o::SparseOptimizer* optimizer,
unsigned int num_iter,
double& init_error, double& final_error)
{
optimizer->initializeOptimization();
optimizer->computeActiveErrors();
init_error = optimizer->activeChi2();
optimizer->optimize(num_iter);
final_error = optimizer->activeChi2();
}
g2oFrameSE3*
createG2oFrameSE3(FramePtr frame, size_t id, bool fixed)
{
g2oFrameSE3* v = new g2oFrameSE3();
v->setId(id);
v->setFixed(fixed);
v->setEstimate(g2o::SE3Quat(frame->T_f_w_.unit_quaternion(), frame->T_f_w_.translation()));
return v;
}
g2oPoint*
createG2oPoint(Vector3d pos,
size_t id,
bool fixed)
{
g2oPoint* v = new g2oPoint();
v->setId(id);
#if SCHUR_TRICK
v->setMarginalized(true);
#endif
v->setFixed(fixed);
v->setEstimate(pos);
return v;
}
g2oEdgeSE3*
createG2oEdgeSE3( g2oFrameSE3* v_frame,
g2oPoint* v_point,
const Vector2d& f_up,
bool robust_kernel,
double huber_width,
double weight)
{
g2oEdgeSE3* e = new g2oEdgeSE3();
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(2,2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber(); // TODO: memory leak
rk->setDelta(huber_width);
e->setRobustKernel(rk);
e->setParameterId(0, 0); //old: e->setId(v_point->id());
return e;
}
} // namespace svo
*/
GPU_version/vio/src/depth_filter.cpp
+1 -1
View File
@@ -261,7 +261,7 @@ namespace vio {
SE2 T(frame->T_f_w_.so2()*(*it)->ftr->frame->T_f_w_.inverse().so2(),frame->T_f_w_.se2().translation()+(*it)->ftr->frame->T_f_w_.inverse().translation());
///TODO add 15 degrees roll orientation
Quaterniond q;
q = AngleAxisd(0.0, Vector3d::UnitX())
q = AngleAxisd(-0.122173, Vector3d::UnitX())
* AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
* AngleAxisd(0.0, Vector3d::UnitZ());
SE3 T_cur_ref(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
GPU_version/vio/src/initialization.cpp
+1 -1
View File
@@ -58,7 +58,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
double disparity = vk::getMean(disparities_,min,max);
if(disparity < Config::initMinDisparity()){
auto result=ukf_->get_location();
if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);
if(fabs(result.second.se2().translation().x())>0.02 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);
#if VIO_DEBUG
fprintf(log_,"[%s] Init: px average disparity is:%f ,While minimum is: %f KLT tracked : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
disparity,