pose_lib.h

Go to the documentation of this file.

 
#pragma once
 
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
 
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <string>
#include <vector>
 
#include "rotation_lib.h"
 
namespace MoveG
{
class Pose
{
public:
    // Constructors
 
    Pose() noexcept;
 
    Pose(const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation);
 
    Pose(const Eigen::Vector3d &position, const Eigen::Matrix3d &rotation_matrix);
 
    explicit Pose(const Eigen::Affine3d &transformation);
 
    Pose(const Eigen::Vector3d &position, const Rotation &orientation);
 
    explicit Pose(const Eigen::Matrix4d &homogeneousT);
 
    ~Pose();
 
    Pose(const Pose &other);
 
    Pose(Pose &&other) noexcept;
 
    Pose &operator=(const Pose &other);
 
    Pose &operator=(Pose &&other) noexcept;
 
    friend std::ostream &operator<<(std::ostream &os, const Pose &pose);
 
    // Getters
 
    [[nodiscard]] Eigen::Vector3d getPosition() const;
 
    [[nodiscard]] Eigen::Quaterniond getQuaternion() const;
 
    [[nodiscard]] Eigen::Matrix3d getRotationMatrix() const;
 
    [[nodiscard]] Eigen::Affine3d getAffineTransformation() const;
 
    [[nodiscard]] Eigen::Matrix4d getHomogeneousT() const;
 
    [[nodiscard]] double getX() const;
 
    [[nodiscard]] double getY() const;
 
    [[nodiscard]] double getZ() const;
 
    [[nodiscard]] double getQx() const;
 
    [[nodiscard]] double getQy() const;
 
    [[nodiscard]] double getQz() const;
 
    [[nodiscard]] double getQw() const;
 
    // Setters
 
    void setPosition(const Eigen::Vector3d &position);
 
    void setOrientation(const Eigen::Quaterniond &orientation);
 
    void setRotationMatrix(const Eigen::Matrix3d &rotation_matrix);
 
    void setAffineTransformation(const Eigen::Affine3d &transformation);
 
    void setHomogeneousT(const Eigen::Matrix4d &homogeneousT);
 
    // Distance metrics
 
    [[nodiscard]] double positionDistance(const Pose &other) const;
 
    [[nodiscard]] double orientationDistance(const Pose &other) const;
 
    // Operations
 
    [[nodiscard]] Pose operator*(const Pose &other) const; // Compose poses
 
    [[nodiscard]] Pose inverse() const;
 
    [[nodiscard]] Pose transformPose(const Pose &transform) const;
 
    [[nodiscard]] Eigen::Vector3d localToGlobal(const Eigen::Vector3d &local_point) const;
 
    [[nodiscard]] Eigen::Vector3d globalToLocal(const Eigen::Vector3d &global_point) const;
 
private:
    Eigen::Vector3d position_;       
    Eigen::Quaterniond orientation_; 
 
    static void validateHomogeneousMatrix(const Eigen::Matrix4d &homogeneousT);
};
 
} // namespace MoveG