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

#ifndef PLANE_HPP_
#define PLANE_HPP_


#include <vector>
#include <memory>
#include "mrob/SE3.hpp"
#include <Eigen/StdVector>



namespace mrob{

class Plane{
  public:
    Plane(uint_t timeLength);
    ~Plane();

    void reserve(uint_t d, uint_t t);//XXX not used
    Mat41 get_plane(void) {return planeEstimation_;};
    void push_back_point(Mat31 &point, uint_t t);
    std::vector<Mat31>& get_points(uint_t t);
    uint_t get_number_points(uint_t t) {return allPlanePoints_[t].size();};
    uint_t get_total_number_points() {return numberPoints_;};
    void clear_points();
    void set_trajectory(const std::shared_ptr<const std::vector<SE3>> &trajectory) {trajectory_ = trajectory;};

    double estimate_plane();
    double estimate_plane_incrementally(uint_t t);
    double get_error() const {return lambda_;};
    double get_error_incremental(uint_t t) const;

    void calculate_all_matrices_S(bool reset=false);
    Mat31 get_mean_point(uint_t t);
    void calculate_all_matrices_Q();
    Mat61 calculate_gradient(uint_t t);

    Mat6 calculate_hessian(uint_t t);


    void reset();
    void print() const;


  protected:
    // max index of time, or trajectory length
    uint_t timeLength_;
    //std::vector<uint_t> timeIndex_;// not used now, but it would be useful for non-consecutive obs

    // Overparametrized plane, as a transformation in SE3 TODO needed?
    //SE3 plane_;
    Mat41 planeEstimation_;
    double lambda_ {};
    bool isPlaneEstimated_;// XXX used or deprecated?

    // subset of pointcloud for the given plane
    std::vector< std::vector<Mat31> > allPlanePoints_;
    uint_t numberPoints_;

    // Transformations. We shared it across all planes that need it.
    std::shared_ptr<const std::vector<SE3>> trajectory_;

    // gradient calculation Q
    // This container is aligned since we use cpp17 (o.w. it would not be)
    std::vector<Mat4, Eigen::aligned_allocator<Mat4> > matrixS_, matrixQ_;
    Mat4 accumulatedQ_;//Q matrix of accumulated values for the incremental update of the error.

    // Store last gradients calculated (for Hessian)
    std::vector<Mat4, Eigen::aligned_allocator<Mat4> > gradQ_, lieGenerativeMatrices_;

  public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
}


#endif /* PLANE_HPP_ */