110 lines
3.1 KiB
C++
110 lines
3.1 KiB
C++
// 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/>.
|
|
|
|
#ifndef SVO_BUNDLE_ADJUSTMENT_H_
|
|
#define SVO_BUNDLE_ADJUSTMENT_H_
|
|
|
|
#include <svo/global.h>
|
|
|
|
namespace g2o {
|
|
class EdgeProjectXYZ2UV;
|
|
class SparseOptimizer;
|
|
class VertexSE3Expmap;
|
|
class VertexSBAPointXYZ;
|
|
}
|
|
|
|
namespace svo {
|
|
|
|
typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3;
|
|
typedef g2o::VertexSE3Expmap g2oFrameSE3;
|
|
typedef g2o::VertexSBAPointXYZ g2oPoint;
|
|
|
|
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)
|
|
{}
|
|
};
|
|
|
|
/// Optimize two camera frames and their observed 3D points.
|
|
/// Is used after initialization.
|
|
void twoViewBA(Frame* frame1, 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);
|
|
|
|
/// Global bundle adjustment.
|
|
/// Optimizes the whole map. Is currently not used in SVO.
|
|
void globalBA(Map* map);
|
|
|
|
/// Initialize g2o with solver type, optimization strategy and camera model.
|
|
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);
|
|
|
|
/// Create a g2o vertice from a keyframe object.
|
|
g2oFrameSE3* createG2oFrameSE3(
|
|
Frame* kf,
|
|
size_t id,
|
|
bool fixed);
|
|
|
|
/// Creates a g2o vertice from a mappoint object.
|
|
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);
|
|
|
|
} // namespace ba
|
|
} // namespace svo
|
|
|
|
#endif // SVO_BUNDLE_ADJUSTMENT_H_
|