Learning diary of slam beginners

The main steps to create g2o are as follows:

1. Create a linear solver

The form of the incremental equation we require is: H △ X=-b. generally, the method we think of is to directly inverse, that is, △ X=-H^-1*b. But when the dimension of H is large, it is very difficult to invert the matrix, so we need some special methods to invert the matrix.

Linear solver cholmod: use sparse cholesky decomposition method. Inherited from LinearSolverCCS
LinearSolverCSparse: use CSparse method. Inherited from LinearSolverCCS
LinearSolverPCG: using the preconditioned reconcile gradient method, inherited from LinearSolver
LinearSolverDense: use the deny Cholesky decomposition method. Inherited from LinearSolver
LinearSolverEigen: the dependency is only eigen, which is solved by sparse Cholesky in eigen. The performance is similar to that of CSparse. Inherited from LinearSolver

2. Create BlockSolver. It is initialized with the linear solver defined above.

The BlockSolver contains the LinearSolver internally, which is initialized with the LinearSolver defined above. It is defined in the following folder:

g2o/g2o/core/block_solver.h

BlockSolver can be defined in two ways

1. One is the solver of the specified fixed variable, which is defined as follows

using BlockSolverPL = BlockSolver< BlockSolverTraits<p, l> >;

Where p represents the dimension of post (note that it must be the minimum representation under manifold), and l represents the dimension of landmark

2. The other is variable size solver, which is defined as follows

using BlockSolverX = BlockSolverPL<Eigen::Dynamic, Eigen::Dynamic>;

At the end of block ﹣ solver. H, several commonly used types are predefined, as follows:

3. Create the total solver. And select one from GN, LM and dogleg, and then initialize it with BlockSolver

In the directory of g2o/g2o/core /, there are three optimization methods for Solver: Gauss Newton method, LM (Levenberg Marquardt) method and Dogleg method, as shown in the figure below, which also match the previous figureThe GN, LM and Doglet algorithms inherit from the same class: OptimizationWithHessian.

Optimization algorithm with Hessian, in turn, inherits from optimization algorithm.

In short, at this stage, we can choose three methods:

g2o::OptimizationAlgorithmGaussNewton g2o::OptimizationAlgorithmLevenberg g2o::OptimizationAlgorithmDogleg

4. Create the ultimate large boss sparse optimizer and use the defined solver as the solution method.

Create sparse optimizer

g2o::SparseOptimizer optimizer;

Use the solver defined above as the solution method:

SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)

setVerbose is used to set the output information of optimization process

SparseOptimizer::setVerbose(bool verbose)

5. Defines the vertices and edges of a graph. And add to SparseOptimizer

First, let's look at the ① class related to vertex in the above figure: HyperGraph::Vertex, which is in the path of

g2o/core/hyper_graph.h

This HyperGraph::Vertex is an abstract vertex and must be used by derivation. As shown in the figure below

Then we look at the ② class in the G 2O class structure diagram. We see that HyperGraph::Vertex is inherited by the class optimizeablegraph, which is defined in

g2o/core/optimizable_graph.h

We found the vertex definition and found that, as expected, the optimizeablegraph inherits from HyperGraph, as shown in the following figure

However, this OptimizableGraph::Vertex is also very low-level, and it will be extended when it is used. Therefore, a general template suitable for most situations is provided in g2o. It is the ③ class corresponding to the structure diagram of G 2O class

BaseVertex<D, T>

Its path is:

g2o/core/base_vertex.h

D is of type int, representing the minimum dimension of vertex. For example, if rotation in 3D space is 3-dimensional, then D = 3

T is the data type of the vertex to be estimated. For example, if the Quaternion is used to express three-dimensional rotation, t is the Quaternion type

How do I define vertices myself?

Some commonly used vertex types are defined in g2o itself:

VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta) VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion) VertexPointXY : public BaseVertex<2, Vector2> VertexPointXYZ : public BaseVertex<3, Vector3> VertexSBAPointXYZ : public BaseVertex<3, Vector3> // Internal and external parameterization and exponential mapping of Se 3 vertices VertexSE3Expmap : public BaseVertex<6, SE3Quat> // SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. // Suppose qw is positive, otherwise there is an ambiguous rotation in qx, qy, qz VertexCam : public BaseVertex<6, SBACam> // Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion. VertexSim3Expmap : public BaseVertex<7, Sim3>

Redefining a vertex generally requires rewriting the following functions:

virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void oplusImpl(const number_t* update); virtual void setToOriginImpl();

Read, write: read disk and save disk functions respectively. In general, if there is no need to perform read / write operation, just declare it

setToOriginImpl: vertex reset function, which sets the original value of the optimized variable.

oplusImpl: vertex update function. A very important function, mainly used for the calculation of delta △ x in the optimization process. After we calculate the increment according to the increment equation, we adjust the estimated value through this function, so we must pay attention to the content of this function.

Let's take a simple example. It comes from the curve fitting in Lecture 14. The sources are as follows

ch6/g2o_curve_fitting/main.cpp

//Vertex and template parameters of curve model: optimize variable dimension and data type

class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW virtual void setToOriginImpl() // Reset { _estimate << 0,0,0; } virtual void oplusImpl( const double* update ) // To update { _estimate += Eigen::Vector3d(update); } // Save and read: leave blank virtual bool read( istream& in ) {} virtual bool write( ostream& out ) const {} };

The initial value of the vertex in the code is set to 0. When updating, the update amount is added directly because of X + △ x (because the vertex type is Eigen::Vector3d, which belongs to vector, and can be updated by adding)

But some examples don't work, such as the following complex point example: Lie algebra represents vertex se3expmap

From g2o official website

g2o/types/sba/types_six_dof_expmap.h

//brief SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSE3Expmap(); bool read(std::istream& is); bool write(std::ostream& os) const; virtual void setToOriginImpl() { _estimate = SE3Quat(); } virtual void oplusImpl(const number_t* update_) { Eigen::Map<const Vector6> update(update_); setEstimate(SE3Quat::exp(update)*estimate()); //Regeneration mode } };

The 6 and se3quat in this are:

The first parameter 6 represents the optimization variable dimension of internal storage, which is a six dimensional Lie algebra

The second parameter is the type of optimization variable, which uses the camera pose type defined by g2o: SE3Quat.

Here you can see the details of g2o/types/slam3d/se3quat.h

It uses quaternion to express rotation, and then adds displacement to store position and pose. At the same time, it supports operations on Lie algebra, such as log function, update function and so on

How do I add vertices to a graph?

It's easy to add vertices to the graph. Let's first look at the first curve fitting example, setEstimate(type) function to set the initial value; setId(int) defines the node number

// Add vertex to graph CurveFittingVertex* v = new CurveFittingVertex(); v->setEstimate( Eigen::Vector3d(0,0,0) ); v->setId(0); optimizer.addVertex( v );

This is an example of adding vertex point XYZ, which is easy to understand

/ch7/pose_estimation_3d2d.cpp

int index = 1; for ( const Point3f p:points_3d ) // landmarks { g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ(); point->setId ( index++ ); point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) ); point->setMarginalized ( true ); optimizer.addVertex ( point ); }

A preliminary understanding of the edge of G 2O

Header file related to edge:
g2o/g2o/core/hyper_graph.h
g2o/g2o/core/optimizable_graph.h
g2o/g2o/core/base_edge.h

BaseUnaryEdge, BaseBinaryEdge and BaseMultiEdge represent one, two and multiple edge respectively.
One edge can be understood as an edge connecting only one vertex, two edges can be understood as an edge connecting two vertices, and multiple edges can be understood as an edge connecting more than three vertices

Their main parameters are: D, E, VertexXi, VertexXj, respectively representing:

D is of type int, indicating the dimension of the measured value
E is the data type of the measurement
Vertex Xi and vertex XJ represent different vertex types respectively

For example, if we use an edge to represent the re projection error of a 3D point projected onto an image plane, we can set the input parameters as follows:

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

The first 2 is that the measurement value is 2D, that is, the difference of image pixel coordinates x and Y. the corresponding measurement value type is Vector2D, and the two vertices, that is, the optimization variables are three-dimensional vertex vertex sapointxyz and Lie group pose vertex sexe3expmap respectively

When defining an edge, we usually need to copy some important member functions. The edge mainly has the following important member functions

virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void computeError(); virtual void linearizeOplus();

Here's a brief explanation
Read, write: read disk and save disk functions respectively. In general, if there is no need to perform read / write operation, just declare it
Computererror function: it is very important to use the value of the current vertex to calculate the error between the measured value and the actual measured value
linearizeOplus function: it is very important that under the current vertex value, the partial derivative of the error to the optimization variable, i.e. Jacobian
In addition to the above member functions, several important member variables and functions are also explained:

How to customize the edges of g2o?

Basically, the edge in G 2O is defined as follows:

class myEdge: public g2o::BaseBinaryEdge<errorDim, errorType, Vertex1Type, Vertex2Type> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW myEdge(){} virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} virtual void computeError() override { // ... _error = _measurement - Something; } virtual void linearizeOplus() override { _jacobianOplusXi(pos, pos) = something; // ... /* _jocobianOplusXj(pos, pos) = something; ... */ } private: // data }

We can find that the most important two functions are computeError(), linearizeOplus()

Let's start with a simple example at https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o'u curve'fixing/main.cpp
This is a unary side. It mainly defines the error function. As shown below, you can see that this example is basically a lost extension of the above example

// Error model template parameters: observation dimension, type, connection vertex type class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {} // Calculation curve model error void computeError() { const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]); const Eigen::Vector3d abc = v->estimate(); _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ; } virtual bool read( istream& in ) {} virtual bool write( ostream& out ) const {} public: double _x; // x value, y value is "measurement" };

Here's a complicated example: the PnP problem of 3D-2D points, which is to minimize the re projection error. This problem is very common. The most common binary edge is used, and the basic edge related code is almost all in one

//Inheriting the BaseBinaryEdge class, the observation value is 2D, the type is Vector2D, and the vertices are 3D points and Lie group pose respectively class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; //1. Default initialization EdgeProjectXYZ2UV(); //2. Calculation error void computeError() { //Pose of Li group camera v1 const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]); // Vertex v2 const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]); //Camera parameters const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0)); //Error calculation, measured value minus estimated value, i.e. re projection error OBS cam //The estimated value is calculated by T*p. the camera coordinates are obtained, and then the pixel coordinates are obtained by camera2pixel() function. Vector2D obs(_measurement); _error = obs-cam->cam_map(v1->estimate().map(v2->estimate())); //Error = observation projection } //3. Calculation method of linear increment function, i.e. Jacobian matrix J virtual void linearizeOplus(); //4. Camera parameters CameraParameters * _cam; bool read(std::istream& is); bool write(std::ostream& os) const; };

How do I add edges to a graph?

Let's start with the simplest example: the method of adding one edge

The following code is from GitHub, which is still an example of curve fitting
slambook/ch6/g2o_curve_fitting/main.cpp

// Add edge to graph for ( int i=0; i<N; i++ ) { CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] ); edge->setId(i); edge->setVertex( 0, v ); // Set connected vertices edge->setMeasurement( y_data[i] ); // Observational value edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrix: the inverse of covariance matrix optimizer.addEdge( edge ); }

For this curve fitting, the observed value is the actual observed data point. For visual SLAM, it is usually the coordinate of feature points we observed. Here is an example. This example is a little more complicated than the one just mentioned, because it is a binary edge, and it needs to connect two vertices with an edge
Code from GitHub
slambook/ch7/pose_estimation_3d2d.cpp

index = 1; for ( const Point2f p:points_2d ) { g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV(); edge->setId ( index ); edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) ); edge->setVertex ( 1, pose ); edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) ); edge->setParameterId ( 0,0 ); edge->setInformation ( Eigen::Matrix2d::Identity() ); optimizer.addEdge ( edge ); index++; }

The p in the setMeasurement function here comes from the vector points 2D, that is, the image coordinates (x,y) of the feature points.
setVertex has two vertices of type 0 and vertexsapointxyz, and one is 1 and pose.
_vertices[0] corresponds to vertex of vertexsapointxyz type, i.e. 3D point, and "vertices[1] corresponds to vertex of VertexSE3Expmap type, i.e. pose

6. Set optimization parameters to start optimization.

Set the initialization, iterations, save results, etc. of SparseOptimizer.

Initialization

SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset)

Set the number of iterations, and then you start to perform the graph optimization.

SparseOptimizer::optimize(int iterations, bool online)

---------------

typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // The optimization variable dimension of each error item is 3, and the error value dimension is 1 // Step 1: create a linear solver Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // Step 2: create BlockSolver. And initialized with the linear solver defined above Block* solver_ptr = new Block( linearSolver ); // Step 3: create the total solver. And select one from GN, LM and dogleg, and then initialize it with BlockSolver g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr ); // Step 4: create the ultimate big boss sparse optimizer g2o::SparseOptimizer optimizer; // Graph model optimizer.setAlgorithm( solver ); // Set solver optimizer.setVerbose( true ); // Turn on debug output // Step 5: define the vertices and edges of the graph. And add to SparseOptimizer CurveFittingVertex* v = new CurveFittingVertex(); //Add vertex to graph v->setEstimate( Eigen::Vector3d(0,0,0) ); v->setId(0); optimizer.addVertex( v ); for ( int i=0; i<N; i++ ) // Add edge to graph { CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] ); edge->setId(i); edge->setVertex( 0, v ); // Set connected vertices edge->setMeasurement( y_data[i] ); // Observational value edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrix: the inverse of covariance matrix optimizer.addEdge( edge ); } // Step 6: set optimization parameters and start optimization optimizer.initializeOptimization(); optimizer.optimize(100);

reference material:
14 lectures on visual SLAM by Gao Xiang
https://blog.csdn.net/electech6/article/details/88018481
https://www.cnblogs.com/CV-life/p/10286037.html
https://www.cnblogs.com/CV-life/archive/2019/03/13/10525579.html

• Give the thumbs-up
• Collection
• share
• • Article report