factor_graph_solve.hpp

/* Copyright (c) 2022, Gonzalo Ferrer
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * factor_graph_solve.hpp
 *
 *  Created on: Mar 23, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#ifndef SRC_FACTOR_GRAPH_SOLVE_HPP_
#define SRC_FACTOR_GRAPH_SOLVE_HPP_


#include "mrob/factor_graph.hpp"
#include "mrob/time_profiling.hpp"
#include <unordered_map>

namespace mrob {


class FGraphSolve: public FGraph
{
public:
    enum matrixMethod{ADJ=0, SCHUR};
    enum optimMethod{GN=0, LM, LM_ELLIPS};

    FGraphSolve(matrixMethod method = ADJ);
    virtual ~FGraphSolve();

    uint_t solve(optimMethod method = GN, uint_t maxIters = 20, matData_t lambda = 1e-6, matData_t solutionTolerance = 1e-2);
    matData_t chi2(bool evaluateResidualsFlag = true);
    std::vector<MatX> get_estimated_state();

    void set_build_matrix_method(matrixMethod method) {matrixMethod_ = method;};
    matrixMethod get_build_matrix_method() { return matrixMethod_;};

    SMatCol get_information_matrix() { return L_;}
    SMatCol get_adjacency_matrix() { return A_;}
    SMatCol get_W_matrix() { return W_;}
    MatX1 get_vector_b() { return b_;}
    MatX1 get_chi2_array();

protected:
    void build_problem(bool useLambda = false);
    void build_adjacency();
    void build_info_adjacency();
    void build_info_EF();
    void build_schur(); // TODO

    void optimize_gauss_newton(bool useLambda = false);

    uint_t optimize_levenberg_marquardt(uint_t maxIters);

    void update_nodes();

    void synchronize_nodes_state();

    void synchronize_nodes_auxiliary_state();

    void build_index_nodes_matrix();

    // Variables for solving the FGraph
    matrixMethod matrixMethod_;
    optimMethod optimMethod_;


    factor_id_t N_; // total number of state variables
    factor_id_t M_; // total number of observation variables

    std::unordered_map<factor_id_t, factor_id_t > indNodesMatrix_;

    SMatRow A_; //Adjacency matrix, as a Row sparse matrix. The reason is for filling in row-fashion for each factor
    SMatRow W_; //A block diagonal information matrix. For types Adjacency it calculates its block transposed squared root
    MatX1 r_; // Residuals as given by the factors

    SMatCol L_; //Information matrix. For Eigen Cholesky AMD Ordering it is necessary Col convention for compilation.
    MatX1 b_; // Post-processed residuals, A'*W*r

    // Correction deltas
    MatX1 dx_;

    // Particular parameters for Levenberg-Marquard
    matData_t lambda_; // current value of lambda
    matData_t solutionTolerance_;
    MatX1 diagL_; //diagonal matrix (vector) of L to update it efficiently


    // Methods for handling Eigen factors. If not used, no problem
    SMatCol hessianEF_;
    MatX1 gradientEF_;
    bool buildAdjacencyFlag_;

    // time profiling
    TimeProfiling time_profiles_;
};


}


#endif /* SRC_FACTOR_GRAPH_SOLVE_HPP_ */