# Learning diary of slam beginners

Learning SLAM for the first time, because one note cannot be used in ubuntu, I decided to use CSDN to record my learning notes. The purpose of publishing is to share and exchange progress with you. Most of the notes come from the experience of the big guys. I will also attach a link. If there is infringement, please let me know. 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:

```Block solver ﹣ 6 ﹣ 3: indicates that the post is 6-dimensional and the observation point is 3-dimensional. BA for 3D SLAM
Blocksolver? 7? 3: there is an additional scale based on blocksolver? 6? 3
Block solver ﹣ 3 ﹣ 2: indicates that post is 3D and observation point is 2D
```

## 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 figure The 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 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);
```

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 );
}
```

A preliminary understanding of the edge of G 2O

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:

```_measurement: store observations
_Error: stores the error computed by the computeError() function
_Vertices []: for storing vertex information, such as binary edges, the size of [vertices [] is 2, and the storage order is related to the set int (0 or 1) when setVertex(int, vertex) is called
setId(int): to define the number of the edge (determines the position in the H matrix)
setMeasurement(type) function to define observations
setVertex(int, vertex) to define vertices
setInformation() to define the inverse of covariance matrix
```

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 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()

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);
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);
// Vertex v2
const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices);
//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 write(std::ostream& os) const;
};
```

How do I add edges to a graph?

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
}
```

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() );
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 corresponds to vertex of vertexsapointxyz type, i.e. 3D point, and "vertices 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);
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
}

// 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  Published 0 original articles, won 0 praise, visited 6

Tags: github Ubuntu

Posted on Fri, 14 Feb 2020 02:00:26 -0500 by grantf