Implementing own solvers

Note that this page is also in documentation, with much better source code formatting and with hyperlinks to all the objects referenced in the code and in the text.

Implementing own solvers

In order to implement solver for your problem of desired dimensionality and with appropriate jacobians, one needs to execute the following steps:

• implement a new parser code if required (the parser now only processes 2D and 3D types)
  • note that this might not be required if you are going to use your own means of passing data to the solver
• create a new header file for the new code
• implement new vertex types (contain "smart" plus code)
• implement new edge types (contain jacobians code)
• implement edge type traits to connect to parse loop if using the builtin parser, or implement your own parse loop
• write specialization of a system that would use your new types
• add a new branch in Main.cpp to call your new code, or call the optimizer yourself

Implement a new parser code

In case the edge types can not be parsed using the existing code, you need to implement parser for them. Note that this might not be required if you are going to use your own means of passing data to the solver (such as e.g. from a video stream if implementing visual SLAM).

Nevertheless if you are going to use the built-in parser, you need to implement storage structure for your new edge, let's start by copying the CParseEntity_XYT_Edge_2D type that can be found in Parser.h in the class CParserBase (copy it just below it):

struct CParseEntity_XYT_Edge_2D : public CParseEntity { // change name here (and also below)
    int m_n_node_0;
    int m_n_node_1;
    Eigen::Vector3d m_v_delta;
    Eigen::Matrix3d m_t_inv_sigma; // don't change names of variables unless you must, just change the types from 3d to whatever you need
    inline CParseEntity_XYT_Edge_2D(int n_node_0, int n_node_1,
        double f_delta_x, double f_delta_y, double f_delta_theta,
        const double *p_upper_matrix_3x3) // you can change the parameters as you wish
        :m_n_node_0(n_node_0), m_n_node_1(n_node_1), // fill the indices of edge vertices (these should be zero-based)
        m_v_delta(f_delta_x, f_delta_y, f_delta_theta) // fill the measurement vector
    {
        m_t_inv_sigma <<
            p_upper_matrix_3x3[0], p_upper_matrix_3x3[1], p_upper_matrix_3x3[2],
            p_upper_matrix_3x3[1], p_upper_matrix_3x3[3], p_upper_matrix_3x3[4],
            p_upper_matrix_3x3[2], p_upper_matrix_3x3[4], p_upper_matrix_3x3[5]; // fill the inverse sigma matrix
    }
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // this is imposed by the use of eigen, copy it
};

Once you have parsed type, you need to implement code to parse it. Start by making a copy of CIgnoreParsePrimitive in ParsePrimitives.h:

class MyNewEdgeParsePrimitive {
public:
    static void EnumerateTokens(std::map<std::string, int> &r_token_name_map, int n_assigned_id) // throws(std::bad_alloc)
    {
        r_token_name_map["MY-NEW-TOKEN-NAME-IN-UPPERCASE"] = n_assigned_id;
        r_token_name_map["MY-OTHER-NEW-TOKEN-NAME-IN-UPPERCASE"] = n_assigned_id;
        // associate all the tokens you want to handle in this parse primitive
    }

    template <class _TyParseLoop>
    static bool Parse_and_Dispatch(size_t n_line_no, const std::string &r_s_line,
    const std::string &UNUSED(r_s_token), _TyParseLoop &r_parse_loop)
    {
        // here, a primitive of type r_s_token should be parsed from r_s_line
        // and if successful, passed to r_parse_loop
        return true;
    }
};

You will need to associate the token names inside EnumerateTokens(). Note that the names should all point to the same primitive (e.g. it is ok to put "EDGE:SE2" and "EDGE2", but not so ok to put "EDGE" and "LANDMARK" together as these carry different data). If handling of more types is required, more parse primitives need to be written.

In the second function, you need to implement the actual parsing and passing of the parsed structure to the parse loop for processing. Add your custom code for the parsing itself:

template <class _TyParseLoop>
static bool Parse_and_Dispatch(size_t n_line_no, const std::string &r_s_line,
    const std::string &UNUSED(r_s_token), _TyParseLoop &r_parse_loop)
{
    int p_pose_idx[2];
    double p_measurement[3];
    double p_matrix[6];
    // data that are stored on the line (change the measurement and matrix dimensions as required)

    if(sscanf(r_s_line.c_str(), "%d %d %lf %lf %lf %lf %lf %lf %lf %lf %lf", // modify the numbers of %lf to reflect number of numbers you need to read from the line
       p_pose_idx, p_pose_idx + 1, p_measurement, p_measurement + 1, p_measurement + 2,
       p_matrix, p_matrix + 1, p_matrix + 2, p_matrix + 3, p_matrix + 4, // put all the matrix and measurement elements, in the order they come
       p_matrix + 5) != 2 + 3 + 6) { // modify the condition
        _ASSERTE(n_line_no < SIZE_MAX);
        fprintf(stderr, "error: line " PRIsize ": line is truncated\n", n_line_no + 1);
        return false;
    }
    // read the individual numbers

    CParseEntity_XYT_Edge_2D edge(p_pose_idx[0], p_pose_idx[1],
        p_measurement[0], p_measurement[1], p_measurement[2], p_matrix); // this creates an object of type that you introduced to Parser.h (rename it)
    // process the measurement

    r_parse_loop.AppendSystem(edge);
    // append the measurement to the system, or something

    return true;
}

This adds handling of the new edge type. If you want to extend the code and make your new type one of the default handled types, you need to add your new type to the list of standard types at the end of ParsePrimitives.h:

typedef MakeTypelist_Safe((CEdge2DParsePrimitive, CLandmark2DParsePrimitive,
    CEdge3DParsePrimitive, CVertex2DParsePrimitive, CVertex3DParsePrimitive,
    CIgnoreParsePrimitive, MyNewEdgeParsePrimitive)) CStandardParsedPrimitives;

If you did that, you also need to change the CParserBase::CParserAdaptor class, back in Parser.h to include your new type(s). Create a variant of the CParserBase::CParserAdaptor::AppendSystem() function for every new type added (just copy-paste and rename). This also needs to be done in TDatasetPeeker in Main.cpp since it inherits from parser adaptor.

This is, however, not always neccessary. If you just want to solve your specific problem, you can leave CStandardParsedPrimitives and CParserBase::CParserAdaptor as is.

Create a new header file for the new code

To accomplish the second step, create a new file, it should be in the include/slam folder and it should be named Something_Types.h, where "Something" is replaced by something that makes more sense (e.g. SE2 types are stored in SE2_Types.h). The file should be blank and it should contain the following include directives:

/**
 *   @file include/slam/Something_Types.h // change this to your name
 *   @author <your name here>
 *   @date 2012
 *   @brief <brief description of what types are being defined here and what problem it can solve>
 */
#ifndef __SOMETHING_TYPES_INCLUDED
#define __SOMETHING_TYPES_INCLUDED // change this to your name

#include "slam/BaseTypes.h" // this needs to be included for base type implementations, required by the solvers
#include "slam/SE2_Types.h" // include this as well to have some common classes (type traits)

// you can start implementing stuff here

#endif // __SOMETHING_TYPES_INCLUDED

Note that modifications to CMakeFiles.txt are not required since it is only a header file.

Also note that for SE2 and SE3 types there was the convention of making "xDSolverBase.h" file (such as 2DSolverBase.h and 3DSolverBase.h) with some common functions. This is not completely neccessary as it was only required by the legacy code and this file is not really needed for anything anymore.

Implement new vertex types

In order to start implementing your types, it is best to begin by exploring of what is in slam/SE2_Types.h. Note the CVertexPose2D class:

class CVertexPose2D : public CSEBaseVertexImpl<CVertexPose2D, 3> { // this says to use base vertex implementation for class with name CVertexPose2D, while the vertex has 3 dimensions; this will generate member variable m_v_state ("member vector" state), which will be Eigen dense column vector with the given number of dimensions
public:
    __SE2_TYPES_ALIGN_OPERATOR_NEW // imposed by the use of eigen, copy this
    inline CVertexPose2D() // copy this
    {}

    inline CVertexPose2D(const Eigen::Vector3d &r_v_state) // copy this, change the dimension of the vector to appropriate
        :CSEBaseVertexImpl<CVertexPose2D, 3>(r_v_state) // change the dimension here as well
    {}

    inline void Operator_Plus(const Eigen::VectorXd &r_v_delta) // "smart" plus
    {
        m_v_state += r_v_delta.segment<3>(m_n_order); // pick part of the delta vector, belonging to this vertex, apply +
        m_v_state(2) = CBase2DSolver::C2DJacobians::f_ClampAngle_2Pi(m_v_state(2)); // clamp angle
    }

    inline void Operator_Minus(const Eigen::VectorXd &r_v_delta) // "smart" minus
    {
        m_v_state -= r_v_delta.segment<3>(m_n_order); // pick part of the delta vector, belonging to this vertex, apply -
        m_v_state(2) = CBase2DSolver::C2DJacobians::f_ClampAngle_2Pi(m_v_state(2)); // clamp angle
    }
};

Basically what you need to do is to change the name of the class (don't delete this one, copy it to your Something_Types.h and edit it there), set the number of dimensions and implement Operator_Plus(). You can leave operator minus empty, it is not really used, or at least do this:

inline void Operator_Minus(const Eigen::VectorXd &r_v_delta) // "smart" minus
{
    Operator_Plus(-r_v_delta); // call plus with negative delta, that should do the trick
}

That gives you your new shiny vertex type. Once you have that, you need to implement the edge type(s).

Implement new edge types

Now since the parser is ready, you can implement your edge type; let's begin by copying CEdgePose2D from SE2_Types.h to your Something_Types.h:

class CEdgePose2D : public CSEBaseEdgeImpl<CEdgePose2D, CVertexPose2D, CVertexPose2D, 3> { // again, this tells that base implementation for base edge for type that will be called CEdgePose2D, and it will be an edge between two vertices of type CVertexPose2D, and the measurement will have 3 dimensions (in order to have hyperedges, you need to provide your own version of CSEBaseEdgeImpl, that is an advanced topic)
public:
    class CRelative_to_Absolute_XYT_Initializer { // this is an object which is used to lazy initialize vertices (copy it)
    protected:
        const Eigen::Vector3d &m_r_v_pose1; // this is a reference to the first vertex state (change the dimensions if required)
        const CParserBase::CParseEntity_XYT_Edge_2D &m_r_edge; // this is a reference to parser entity, containing the current edge (change the type to your type)

    public:
        inline CRelative_to_Absolute_XYT_Initializer(const Eigen::Vector3d &r_v_vertex1,
            const CParserBase::CParseEntity_XYT_Edge_2D &r_edge) // just change the types, same as above
            :m_r_v_pose1(r_v_vertex1), m_r_edge(r_edge)
        {}

        inline operator CVertexPose2D() const // this function calculates initial prior from the state of the first vertex m_r_v_pose1 and from the edge measurement m_r_edge
        {
            Eigen::Vector3d v_pose2;
            CBase2DSolver::C2DJacobians::Relative_to_Absolute(m_r_v_pose1, m_r_edge.m_v_delta, v_pose2); // implement your own equation here

            return CVertexPose2D(v_pose2);
        }
    };

public:
    __SE2_TYPES_ALIGN_OPERATOR_NEW // imposed by the use of eigen, just copy this

    inline CEdgePose2D() // copy this
    {}

    template <class CSystem>
    CEdgePose2D(const CParserBase::CParseEntity_XYT_Edge_2D &r_t_edge, CSystem &r_system) // this is edge constructor how it is called in the parse loop; you need to change type of the edge
        :CSEBaseEdgeImpl<CEdgePose2D, CVertexPose2D, CVertexPose2D, 3>(r_t_edge.m_n_node_0,
        r_t_edge.m_n_node_1, r_t_edge.m_v_delta, r_t_edge.m_t_inv_sigma) // this just calls the base edge implementation, you need to change types and maintain the parameters if required (these are: index of first and of second vertex, the measurement vector and the inverse sigma matrix)
    {
        m_p_vertex0 = &r_system.template r_Get_Vertex<CVertexPose2D>(r_t_edge.m_n_node_0, CInitializeNullVertex());
        m_p_vertex1 = &r_system.template r_Get_Vertex<CVertexPose2D>(r_t_edge.m_n_node_1, CRelative_to_Absolute_XYT_Initializer(m_p_vertex0->v_State(), r_t_edge)); // rename your initializer if required
        // get vertices (initialize if required)
        // the strange syntax with "template" is required by g++, otherwise gives "expected primary-expression before '>' token"

        _ASSERTE(((CSEBaseVertex*)m_p_vertex0)->n_Dimension() == 3);
        _ASSERTE(((CSEBaseVertex*)m_p_vertex1)->n_Dimension() == 3); // change the dimensionality here, if required (full type control currently not possible to make sure that the indices in the edge really point to vertices that are of the expected type. e.g. the edge might expect both vertices to be poses, but if the dataset is not "nice", one of the vertices can very well be a landmark)
        // make sure the dimensionality is correct (might not be)
    }

    inline void Calculate_Jacobians_Expectation_Error(Eigen::Matrix3d &r_t_jacobian0,
        Eigen::Matrix3d &r_t_jacobian1, Eigen::Vector3d &r_v_expectation,
        Eigen::Vector3d &r_v_error) const // change dimensionality of eigen types, if required
    {
        CBase2DSolver::C2DJacobians::Absolute_to_Relative(m_p_vertex0->v_State(),
            m_p_vertex1->v_State(), r_v_expectation, r_t_jacobian0, r_t_jacobian1); // write your jacobian calculation code here (vertex state vectors are inputs, the rest are the outputs
        that you need to fill)
        // calculates the expectation and the jacobians

        r_v_error = m_v_measurement - r_v_expectation;
        r_v_error(2) = CBase2DSolver::C2DJacobians::f_ClampAngularError_2Pi(r_v_error(2)); // write your error calculation code here
        // calculates error (possibly re-calculates, if running A-SLAM)
    }

    inline double f_Chi_Squared_Error() const // this function should mostly work as is, you just need to change dimensions of the vectors and matrices
    {
        Eigen::Matrix3d p_jacobi[2];
        Eigen::Vector3d v_expectation, v_error;
        Calculate_Jacobians_Expectation_Error(p_jacobi[0], p_jacobi[1], v_expectation, v_error);
        // calculates the expectation, error and the jacobians

        return (v_error.transpose() * m_t_sigma_inv).dot(v_error); // ||h_i(O_i) - z_i||^2 lambda_i
    }
};

Implement edge type traits to connect to parse loop

This chapter only applies if using the built-in parser (otherwise see the next chapter).

And that is it, you have an edge! At this point, it is still a little bit to do. In case you will be using the built-in parser, you need to hook up the edges with the parse loop. For every edge type you've created, you need to write trait class specialization. These traits help the parse loop decide which edge type to create from which parsed structure (or whether to ignore / fail):

template <class CParsedStructure>
class CMyNewEdgeTraits { // replace "MyNew" with something (e.g. SE2 types have CSE2EdgeTraits)
    typedef CFailOnEdgeType _TyEdge; // it should fail on unknown edge types

    static const char *p_s_Reason()
    { return "unknown edge type occured"; }
};

template <>
class CMyNewEdgeTraits<CParserBase::TOdometry2D> { // this is a trait for *parsed* type CParserBase::TOdometry2D
public:
    typedef CEdgePose2D _TyEdge; // here we say to create CEdgePose2D type
};

template <>
class CSE2OnlyPoseEdgeTraits<CParserBase::TLandmark2D> { // an example, don't write "CSE2OnlyPoseEdgeTraits", that is already defined
public:
    typedef CFailOnEdgeType _TyEdge; // it should fail on this edge type

    static const char *p_s_Reason()
    {
        return "landmark edges not permitted in pose-only solver"; // tell us why
    }
};

You need to fill traits with all the types you want to permit in the system. You can add also the edges which you don't want to handle to provide more meaningful error messages than "unknown edge type occured", but it is not mandatory.

With all that, the parser knows how to parse your new edge types, the parse loop knows how to redirect those to the system and how to call optimizer.

Implement your own parse loop

If you don't want to use the built-in parser, look inside ParseLoop.h and see how the edges are passed to the system for optimization. You need to write your own code that does the same. Basically you need to:

while(have more edges) {
    TParsedType edge_in;
    // fill this with the new measurement somehow

    CYourEdgeType &r_internal_edge_rep = system.r_Add_Edge(CYourEdgeType(edge_in, system));
    // add the edge to the system ("convert" parsed edge to internal representation)

    solver.Incremental_Step(r_internal_edge_rep);
    // call solver with the new edge
}

Note that at this point you don't have system and you don't have solver. The next chapters contain information on where to get those.

Write specialization of a system

Once you have that, finally we need to make a solver to solve using your jacobians and system to contain your edges and vertices. You write:

typedef MakeTypelist_Safe((CVertexPose2D, CVertexLandmark2D)) TVertexTypelist; // just put your vertex types in the list (note the double parentheses are required)
typedef MakeTypelist_Safe((CEdgePose2D, CEdgePoseLandmark2D)) TEdgeTypelist; // just put your edge types in the list (note the double parentheses are required)
typedef CFlatSystem<CSEBaseVertex, TVertexTypelist,
    CSEBaseEdge, TEdgeTypelist> CSystemType; // the Base types can be your types in case you are not using CBaseSEVertexImpl / CBaseSEEdgeImpl
// make a system permitting SE(2) vertex and edge types

Calling the new system

CSystemType system;
// declare the system

typedef CLinearSolver_CSparse CLinearSolverType; // use CSparse as linear solver (or any other)

CLinearSolverType linear_solver;
// you need a linear solver

typedef CNonlinearSolver_Lambda CNonlinearSolverType; // or use A, these two are rather stable and we're confident they will give correct results
typedef CNonlinearSolverType<CSystemType, CLinearSolverType> CSpecializedNonlinearSolverType;
CSpecializedNonlinearSolverType nonlinear_solver(system, n_linear_solve_each_n_steps,
    n_nonlinear_solve_each_n_steps, n_max_nonlinear_solve_iteration_num,
    f_nonlinear_solve_error_threshold, b_verbose, linear_solver); // initialize nonlinear solver
// prepare nonlinear solver

typedef CMyNewEdgeTraits CEdgeTraitsType; // use any edge traits appropriate
typedef CParseLoop<CSystemType, CSpecializedNonlinearSolverType,
    CEdgeTraitsType> CSpecializedParseLoopType;
CSpecializedParseLoopType parse_loop(system, nonlinear_solver);
// prepare parse loop

typedef MakeTypelist_Safe((MyNewEdgeParsePrimitive)) CMyParsedPrimitives; // list of the new parsed primitives
typedef typename CConcatTypelist<CMyParsedPrimitives,
    CStandardParsedPrimitives>::_TyResult CParsePrimitivesList; // list of parsed primitives
// this is only required if you did not modify CStandardParsedPrimitives already
// also, for handling your custom problems, CMyParsedPrimitives might be just enough (no concatenation required)

CParserTemplate<CSpecializedParseLoopType, CParsePrimitivesList> p;
if(!p.Parse(p_s_input_file, &parse_loop, n_max_lines_to_process)) {
    fprintf(stderr, "error: failed to parse input file\n");
    exit(-1);
}
// run the parser

system.Plot2D("initial.tga");
system.Dump("initial.txt");
// save the solution

nonlinear_solver.Optimize(n_max_final_optimization_iteration_num, f_final_optimization_threshold);
// perform the final optimization

system.Plot2D("solution.tga");
system.Dump("solution.txt");
// save the solution

And as they say, you have optimized.

Modifying Main.cpp to use your new code via commandline

In case you want to update the SLAM_plus_plus program with your new types then you don't have to write all this, you just find the variable b_10k_opts and follow it arround, mirroring everything that is done for it - that should also include declaring new system type with your list of vertices and edges per every solver type you want to support (you might e.g. want to write b_bundle_adjustment).

Final Thoughts

There is kind of a lot of duplicate editing in vertex and edge types, some of it might be handled using some #defines but i prefer to keep the code clean. It is not so much extra work, just use "find and replace all", making sure to tick "Case Sensitive" and "Whole Words Only".

If you run in any problems while implementing your SLAM, let me know (to help you and to improve this tutorial / the code).