EigenFactorPlane.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.
 *
 *
 * EigenFactorPlane.hpp
 *
 *  Created on: Aug 16, 2019
 *  Created on: July 10, 2021 (for real)
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab.
 */

#ifndef EIGENFACTORPLANE_HPP_
#define EIGENFACTORPLANE_HPP_


#include "mrob/factor.hpp"
#include <unordered_map>
#include <deque>
#include <Eigen/StdVector>


namespace mrob{

class EigenFactorPlane: public EigenFactor{
public:
    EigenFactorPlane(Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
    ~EigenFactorPlane() override = default;
    void evaluate_residuals() override;
    void evaluate_jacobians() override;
    void evaluate_chi2() override;

    void print() const;

    MatRefConst get_obs() const
            {assert(0 && "EigenFactorPlane:get_obs: method should not be called");return Mat31::Zero();}
    VectRefConst get_residual() const
            {assert(0 && "EigenFactorPlane::get_resigual: method should not be called");return Mat31::Zero();}
    MatRefConst get_information_matrix() const
            {assert(0 && "EigenFactorPlane::get_inform method should not be called");return Mat4::Zero();}

    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const override;
    MatRefConst get_hessian(mrob::factor_id_t id = 0) const override;


    // NEW functions added to the base class factor.hpp
    VectRefConst get_state(void) const override
                {return planeEstimation_;}
    void add_point(const Mat31& p, std::shared_ptr<Node> &node, matData_t &W) override;
    void add_points_array(const MatX &P, std::shared_ptr<Node> &node, mrob::matData_t &W) override;
    void add_points_S_matrix(const Mat4 &S, std::shared_ptr<Node> &node, mrob::matData_t &W) override;
    Mat31 get_mean_point(factor_id_t id);


protected:
    virtual void estimate_plane();
    void calculate_all_matrices_S();
    void calculate_all_matrices_Q();
    std::deque<factor_id_t> nodeIds_;
    std::unordered_map<factor_id_t, uint_t> reverseNodeIds_;
    std::deque<Mat61, Eigen::aligned_allocator<Mat61>> J_;
    std::deque<Mat6, Eigen::aligned_allocator<Mat6>> H_;
    std::deque<Mat4, Eigen::aligned_allocator<Mat4>> S_, Q_;
    Mat4 accumulatedQ_;//Q matrix of accumulated values for the incremental update of the error.

    Mat41 planeEstimation_, planeEstimationCenter_;
    //matData_t planeError_; //this is chi2 scaled by the covariance of point measurement.

    // subset of pointcloud for the given plane
    //std::unordered_map<factor_id_t, std::vector<Mat31> > allPlanePoints_;
    std::deque<std::deque<Mat31, Eigen::aligned_allocator<Mat31>> > allPlanePoints_;
    std::deque<std::deque<matData_t> > allPointsInformation_;
    uint_t numberPoints_;

    matData_t planeError_;

    Mat4 Tcenter_; // Used to move the Q matrix close to have a d~0, to avoid scale

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen
};

}
#endif /* EigenFactorPlane_HPP_ */