factor.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.hpp
 *
 *  Created on: Feb 12, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech 
 */

#ifndef FACTOR_HPP_
#define FACTOR_HPP_

#include <vector>
#include <memory>

#include "mrob/matrix_base.hpp"
#include "mrob/node.hpp"

namespace mrob{

class Node;

class Factor{
public:
    enum robustFactorType{QUADRATIC = 0, HUBER, CAUCHY, MCCLURE, RANSAC};
    Factor(uint_t dim, uint_t allNodesDim, robustFactorType factor_type = QUADRATIC, uint_t potNumberNodes = 5);
    virtual ~Factor();
    virtual void evaluate_residuals() = 0;
    virtual void evaluate_jacobians() = 0;
    virtual void evaluate_chi2() = 0;
    matData_t get_chi2() const { return chi2_;}
    virtual void print() const {}
    virtual MatRefConst get_obs() const = 0;
    virtual VectRefConst get_residual() const = 0;
    virtual MatRefConst get_information_matrix() const = 0;
    virtual MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const = 0;


    factor_id_t get_id() const {return id_;}
    void set_id(factor_id_t id) {id_ = id;}
    uint_t get_dim_obs() const {return dim_;}
    void set_dim_obs(uint_t dim) {dim_ = dim;}
    uint_t get_all_nodes_dim() const{ return allNodesDim_;}
    void set_all_nodes_dim(uint_t dim) {allNodesDim_ = dim;}
    const std::vector<std::shared_ptr<Node> >*
            get_neighbour_nodes(void) const {return &neighbourNodes_;}

    matData_t evaluate_robust_weight(matData_t u, matData_t params = 0.0);

protected:
    factor_id_t id_;
    std::vector<std::shared_ptr<Node> > neighbourNodes_;
    uint_t dim_;//dimension of the observation
    uint_t allNodesDim_;//summation of all the nodes that the factor affects
    matData_t chi2_;

    // Robust factor weighting the "iteratively weighted LSQ"
    robustFactorType robust_type_;
    matData_t robust_weight_; // dp/du 1/u or influence by the inverse of the distance u

    //matData_t ransac_significance_level_;

    // variables to declare on child Factor, for instance of dim 6
    //Mat61 obs_, r_; //and residuals
    //Mat<Z,N> J_;//Jacobians
    //Mat6 W_;//inverse of observation covariance (information matrix)

};

class EigenFactor : public Factor
{
public:
    EigenFactor(robustFactorType factor_type = QUADRATIC, uint_t potNumberNodes = 5);
    virtual ~EigenFactor() = default;
    virtual VectRefConst get_state() const = 0;
    virtual void add_point(const Mat31& p, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;
    virtual MatRefConst get_hessian(mrob::factor_id_t id = 0) const = 0;
    virtual void add_points_array(const MatX &P, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;
    virtual void add_points_S_matrix(const Mat4 &S, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;

};

}

#endif /* FACTOR_HPP_ */