SE3cov.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.
 *
 * @brief SE3 compounding class
 * @file SE3.hpp
 *
 * @date Created on: July, 2021
 * @author Gonzalo Ferrer, Aleksei Panchenko
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#ifndef SE3COV_HPP_
#define SE3COV_HPP_

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

namespace mrob
{
    class SE3Cov : public SE3
    {
    public:
        SE3Cov(void);

        SE3Cov(const SE3 &pose, const Mat6 &cov);

        SE3Cov(const SE3Cov &pose);
        Mat6 cov() const;

        SE3Cov compound_2nd_order(const SE3Cov &pose) const;

        SE3Cov compound_2nd_order(const SE3 &pose, const Mat6 &cov) const; // does right hand side update

        SE3Cov compound_4th_order(const SE3Cov &pose) const;

        SE3Cov compound_4th_order(const SE3 &pose, const Mat6 &cov) const;

        void print();

        SE3Cov mul(const SE3Cov &rhs) const;

        SE3Cov operator*(const SE3Cov &rhs) const;


        std::string toString() const;

    protected:
        Mat6 covariance_; 
    };

    Mat6 curly_wedge(const Mat61& xi);

} // end namespace

#endif // SE3COV_HPP_