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


#ifndef SE3_HPP_
#define SE3_HPP_


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




namespace mrob{


class SE3
{
public:
    SE3(const Mat4 &T = Mat4::Identity() );
    SE3(const Mat61 &xi);
    SE3(const SE3 &T);
    SE3(const SO3 &R, const Mat31 &t);
    SE3(const Mat3 &R, const Mat31 &t);
    template<typename OtherDerived>
    SE3(const Eigen::MatrixBase<OtherDerived>& rhs);
    SE3& operator=(const SE3& rhs);

    SE3 operator*(const SE3& rhs) const;
    SE3 mul(const SE3& rhs) const;

    void update_lhs(const Mat61 &dxi);
    void update_rhs(const Mat61 &dxi);
    void exp(const Mat4 &xi_hat);
    Mat4 ln(void) const;
    Mat61 ln_vee() const;
    Mat31 transform(const Mat31 & p) const;
    MatX transform_array(const MatX &P) const;
    SE3 inv(void) const;
    Mat6 adj() const;
    //Mat4 T() const;
    const Eigen::Ref<const Mat4> T() const;
    Mat4& ref2T();
    Mat3 R() const;
    Mat31 t() const;
    double distance(const SE3 &rhs=SE3()) const;
    double distance_rotation(const SE3 &rhs=SE3()) const;
    double distance_trans(const SE3 &rhs=SE3()) const;
    void regenerate();

    Mat41 transform_plane(const Mat41 &pi);


    void print(void) const;
    void print_lie(void) const;

    std::string toString() const;

protected:
    Mat4 T_;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};


Mat4 hat6(const Mat61 &xi);
Mat61 vee6(const Mat4 &xi_hat);

bool isSE3(const Mat4 &T);

Mat4 SE3GenerativeMatrix(uint_t coordinate);
//Mat4 SE3DoubleGenerativeMatrix(uint_t i, uint_t j);

extern const std::vector<Mat4> LieGenerative;
extern const std::vector< Mat4> LieDoubleGenerative;

}// end namespace
#endif /* SE3_HPP_ */