factor2Poses2d.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.
 *
 *
 *  Created on: Jan 14, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              konstantin.pakulev@skoltech.ru
 *              Konstantin Pakulev
 *              Mobile Robotics Lab, Skoltech
 */
#ifndef MROB_FACTOR2POSES2D_H
#define MROB_FACTOR2POSES2D_H

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


namespace mrob{

    class Factor2Poses2d : public Factor
    {
    public:
        Factor2Poses2d(const Mat31 &observation, std::shared_ptr<Node> &nodeOrigin,
                       std::shared_ptr<Node> &nodeTarget, const Mat3 &obsInf, bool updateNodeTarget=false,
                       Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
        ~Factor2Poses2d() override = default;

        void evaluate_residuals() override;
        void evaluate_jacobians() override;
        void evaluate_chi2() override;

        MatRefConst get_obs() const override {return obs_;};
        VectRefConst get_residual() const override {return r_;};
        MatRefConst get_information_matrix() const override {return W_;};
        MatRefConst get_jacobian([[maybe_unused]]mrob::factor_id_t id = 0) const override {return J_;};
        void print() const override;

    protected:
        // The Jacobian's correspondent nodes are ordered on the vector<Node>
        // being [0]->J1 and [1]->J2
        // declared here but initialized on child classes
        Mat31 obs_, r_; //and residuals
        Mat3 W_;//inverse of observation covariance (information matrix)
        Mat<3,6> J_;//Joint Jacobian

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen
    };

    class Factor2Poses2dOdom : public Factor2Poses2d
    {
    public:
        Factor2Poses2dOdom(const Mat31 &observation, std::shared_ptr<Node> &nodeOrigin,
                           std::shared_ptr<Node> &nodeTarget, const Mat3 &obsInf, bool updateNodeTarget=true,
                           Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
        ~Factor2Poses2dOdom() override = default;

        void evaluate_residuals() override;
        void evaluate_jacobians() override;

    private:
        Mat31 get_odometry_prediction(Mat31 state, Mat31 motion);

    };

}

#endif //MROB_FACTOR2POSES2D_H