EigenFactorPlaneCenter.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.
 *
 *
 * EigenFactorPlaneCenter.hpp
 *
 *  Created on: Oct 7 2022
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab.
 */

// XXX this duplicate should disappear
#ifndef EIGENFACTORPLANECENTER_HPP_
#define EIGENFACTORPLANECENTER_HPP_


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


namespace mrob{

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


protected:
    void estimate_plane() override;

    Mat4 accumulatedCenterQ_;//Q matrix of accumulated values for the incremental update of the error.
    Mat41 planeEstimationUnit_;
    Mat4 Tcenter_;

    // subset of pointcloud for the given plane
    //std::unordered_map<factor_id_t, std::vector<Mat31> > allPlanePoints_;

};

}
#endif /* EigenFactorPlaneCenter_HPP_ */