#include "node.h"
#include "scoped_timer.h"
#include "transformation_estimation.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/types/icp/types_icp.h"
#include "g2o/types/slam3d/se3quat.h"
#include "g2o/types/slam3d/edge_se3_pointxyz_depth.h"
#include "g2o/types/slam3d/vertex_pointxyz.h"
#include "misc2.h"
#include <Eigen/SVD>
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/hyper_dijkstra.h"
#include "g2o/core/optimization_algorithm_gauss_newton.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/robust_kernel_impl.h"
Go to the source code of this file.
Typedefs | |
typedef g2o::EdgeSE3PointXYZDepth | feature_edge_type |
typedef g2o::VertexPointXYZ | feature_vertex_type |
Functions | |
feature_edge_type * | edgeToFeature (const Node *node, unsigned int feature_id, g2o::VertexSE3 *camera_vertex, g2o::VertexPointXYZ *feature_vertex) |
void | getTransformFromMatchesG2O (const Node *earlier_node, const Node *newer_node, const std::vector< cv::DMatch > &matches, Eigen::Matrix4f &transformation_estimate, int iterations) |
Compute. | |
void | optimizerSetup (g2o::SparseOptimizer &optimizer) |
Set parameters for icp optimizer. | |
std::pair< g2o::VertexSE3 *, g2o::VertexSE3 * > | sensorVerticesSetup (g2o::SparseOptimizer &optimizer, Eigen::Matrix4f &tf_estimate) |
typedef g2o::EdgeSE3PointXYZDepth feature_edge_type |
Definition at line 30 of file transformation_estimation.cpp.
typedef g2o::VertexPointXYZ feature_vertex_type |
Definition at line 29 of file transformation_estimation.cpp.
feature_edge_type* edgeToFeature | ( | const Node * | node, |
unsigned int | feature_id, | ||
g2o::VertexSE3 * | camera_vertex, | ||
g2o::VertexPointXYZ * | feature_vertex | ||
) |
Definition at line 91 of file transformation_estimation.cpp.
void getTransformFromMatchesG2O | ( | const Node * | earlier_node, |
const Node * | newer_node, | ||
const std::vector< cv::DMatch > & | matches, | ||
Eigen::Matrix4f & | transformation_estimate, | ||
int | iterations | ||
) |
Compute.
Do sparse bundle adjustment for the node pair.
Definition at line 126 of file transformation_estimation.cpp.
void optimizerSetup | ( | g2o::SparseOptimizer & | optimizer | ) |
Set parameters for icp optimizer.
Definition at line 37 of file transformation_estimation.cpp.
std::pair<g2o::VertexSE3*, g2o::VertexSE3*> sensorVerticesSetup | ( | g2o::SparseOptimizer & | optimizer, |
Eigen::Matrix4f & | tf_estimate | ||
) |
Definition at line 64 of file transformation_estimation.cpp.