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


#ifndef SO3_HPP_
#define SO3_HPP_


#include "mrob/matrix_base.hpp"




namespace mrob{


class SO3
{
public:
    SO3(const Mat3 &R = Mat3::Identity());
    SO3(const Mat31 &w);
    SO3(const SO3 &R);
    template<typename OtherDerived>
    SO3(const Eigen::MatrixBase<OtherDerived>& rhs);
    SO3& operator=(const SO3& rhs);

    SO3 operator*(const SO3& rhs) const;

    SO3 mul(const SO3& rhs) const;

    void update_lhs(const Mat31 &dw);
    void update_rhs(const Mat31 &dw);
    void exp(const Mat3 &w_hat);
    Mat3 ln(double *o = nullptr) const;
    Mat31 ln_vee() const;
    SO3 inv(void) const;
    Mat3 adj() const;
    Mat3 R() const;
    Mat3& ref2R();

    double distance(const SO3 &rhs) const;

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


    std::string toString() const;


protected:
    Mat3 R_;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};


Mat3 hat3(const Mat31 &w);
Mat31 vee3(const Mat3 &w_hat);

bool isSO3(const Mat3 &R);


Mat3 quat_to_so3(const Eigen::Ref<const Mat41> v);


Mat41 so3_to_quat(const Eigen::Ref<const Mat3> R);

Mat3 rpy_to_so3(const Eigen::Ref<const Mat31> v);

}// end namespace
#endif /* SO3_HPP_ */