-
GAUDIN Gregory authoredGAUDIN Gregory authored
pose.hpp 25.94 KiB
#pragma once
#ifndef POSE_HPP
#define POSE_HPP 1
#define NUMCPP_NO_USE_BOOST ON
#include <iostream>
#include <numeric>
#include <iostream>
#include <algorithm> // For std::min_element, std::max_element
#include "NumCpp.hpp"
#ifdef _MSC_VER
#include <valarray>
#endif
namespace QOSM::Pose{
using MatrixX3d = nc::NdArray<double>;
/**
* @class VectorNx3
* @brief Represents a collection of N 3D vectors stored as a matrix with N rows and 3 columns.
*
* This class extends MatrixX3d and provides various constructors for initializing vectors,
* as well as operations for manipulating and processing sets of 3D vectors.
* It supports element-wise arithmetic, row and column extraction, dot and cross products,
* normalization, and transformations.
*
* @note Each row represents a distinct 3D vector, which can be a position vector, unit vector, or Poynting vector.
*/
class VectorNx3 : public MatrixX3d {
protected:
/// Number of points (number of rows)
size_type N;
public:
/**
* @brief Default constructor, initializes a 1x3 vector.
*/
VectorNx3();
/**
* @brief Constructs a vector with N rows and 3 columns, initializing all elements to a specific value.
* @param _N Number of rows (vectors).
* @param value Initial value for all components (default: 0).
*/
VectorNx3(const size_type _N, double value = 0.0);
/**
* @brief Constructs a vector from an existing matrix of size Nx3.
* @param data Matrix of values.
*/
VectorNx3(const MatrixX3d data);
/**
* @brief Constructs a vector from a pointer to an array of doubles.
* @param ptr Pointer to data.
* @param size Total number of elements (must be a multiple of 3).
*/
VectorNx3(const double* ptr, uint32_t size);
/**
* @brief Returns the number of vectors (rows).
* @return Number of vectors.
*/
inline nc::uint32 rows() const;
/**
* @brief Retrieves a column at a specific index.
* @param idx Index of the column.
* @return Column as an Nx1 array.
*/
inline auto col(nc::uint32 idx);
/**
* @brief Retrieves a row at a specific index.
* @param idx Index of the row.
* @return Row as a 1x3 array.
*/
inline auto row(nc::uint32 idx);
/**
* @brief Extracts a column and returns it as a separate VectorNx3 instance.
* @param idx Index of the column.
* @return New VectorNx3 containing the column.
*/
inline auto vcol(nc::uint32 idx);
/**
* @brief Extracts a row and returns it as a separate VectorNx3 instance.
* @param idx Index of the row.
* @param N Number of repetitions (default: 1).
* @return New VectorNx3 containing the repeated row.
*/
inline auto vrow(nc::uint32 idx, nc::uint32 N = 1);
/**
* @brief Repeats a row N times.
* @param idx Index of the row.
* @return New instance with the row repeated.
*/
inline auto rowN(nc::uint32 idx);
/**
* @brief Inserts a column vector at a specified index.
* @param idx Column index.
* @param data Column data (Nx1 matrix).
*/
inline void putCol(nc::uint32 idx, MatrixX3d &data);
/**
* @brief Inserts a row vector at a specified index.
* @param idx Row index.
* @param data Row data (1x3 matrix).
*/
inline void putRow(nc::uint32 idx, MatrixX3d &data);
/**
* @brief Creates a VectorNx3 from an existing MatrixX3d.
* @param data Matrix of values.
* @return New VectorNx3 instance.
*/
static VectorNx3 CreateVectorNx3(const MatrixX3d data);
/**
* @brief Computes the norm of each row.
* @return Nx1 matrix containing norms.
*/
inline MatrixX3d norm();
/**
* @brief Computes the squared norm of each row.
* @return Nx1 matrix containing squared norms.
*/
inline MatrixX3d norm2();
/**
* @brief Normalizes each row in place.
* @return Nx1 matrix of normalization factors.
*/
nc::NdArray<double> normalise();
/** * @brief Returns a normalized copy of the vector.
* @return New VectorNx3 instance with normalized rows.
*/
VectorNx3 normalised() const;
/**
* @brief Computes the dot product of each row with another VectorNx3.
* @warning Both vectors must have the same number of rows.
* @return Nx1 matrix of dot product values.
*/
inline MatrixX3d dot(const VectorNx3 &b);
/**
* @brief Computes the cross product of each row with another VectorNx3.
* @warning Both vectors must have the same number of rows.
* @return New VectorNx3 containing cross product results.
*/
VectorNx3 cross(const VectorNx3 &v2);
/**
* @brief Flips the direction of all vectors.
* @return New VectorNx3 with reversed directions.
*/
VectorNx3 flip();
/** @brief Scalar multiplication. */
friend VectorNx3 operator*(VectorNx3 lhs, const double &rhs);
VectorNx3 &operator*=(const double &rhs);
/** @brief Scalar division. */
friend VectorNx3 operator/(VectorNx3 lhs, const double &rhs);
VectorNx3 &operator/=(const double &rhs);
/** @brief Scalar addition. */
friend VectorNx3 operator+(VectorNx3 lhs, const double &rhs);
VectorNx3 &operator+=(const double &rhs);
/** @brief Scalar subtraction. */
friend VectorNx3 operator-(VectorNx3 lhs, const double &rhs);
VectorNx3 &operator-=(const double &rhs);
};
#ifndef _MSC_VER
typedef double v4df __attribute__ ((vector_size (32)));
#else
using v4df = std::valarray<double>;
#endif
/**
* @class Vec3
* @brief Represents a 3D vector with various arithmetic and utility operations.
* It can represent a position vector, a unit vector or a Poyinting vector.
*
* This class supports initialization from different data structures, arithmetic operations,
* and vector operations like dot product, cross product, and normalization.
*/
class Vec3 {
protected:
v4df data = {0., 0., 0., 0.};
public:
/**
* @brief Default constructor initializing the vector to (0,0,0).
*/
Vec3(double x=0., double y=0., double z=0.);
/**
* @brief Constructor initializing from an nc::NdArray.
* @param inVector Input array. * @param idx Row index to extract the vector from.
*/
Vec3(const nc::NdArray<double> &inVector, const std::size_t idx);
/**
* @brief Constructor initializing from a VectorNx3.
* @param inVector Input matrix.
* @param idx Row index to extract the vector from.
*/
Vec3(const VectorNx3 &inVector, const std::size_t idx);
/**
* @brief Constructor initializing from a 3-element std::array.
* @param v Input array.
*/
Vec3(const std::array<double, 3> &v);
/**
* @brief Constructor initializing from a std::vector.
* @param vec Input vector (must have at least 3 elements).
* @throws std::runtime_error if the vector has less than 3 elements.
*/
Vec3(const std::vector<double>& vec);
/**
* @brief Constructor initializing from an initializer list.
* @param values List of 3 values.
* @throws std::runtime_error if the list has less than 3 elements.
*/
Vec3(std::initializer_list<double> values);
/**
* @brief Assigns values using an initializer list.
* @param values List of 3 values.
* @throws std::runtime_error if the list has less than 3 elements.
*/
void assign(std::initializer_list<double> values);
/**
* @brief Computes the dot product with another vector.
* @param rhl The other vector.
* @return The dot product value.
*/
inline double dot(const Vec3 &rhl) const;
/**
* @brief Computes the squared norm of the vector.
* @return The squared norm value.
*/
inline double norm2() const;
/**
* @brief Computes the norm (magnitude) of the vector.
* @return The norm value.
*/
inline double norm() const;
/**
* @brief Normalizes the vector in place.
* @return The original norm before normalization.
*/
inline double normalise();
/**
* @brief Returns a normalized copy of the vector.
* @return A normalized vector.
*/
inline Vec3 normalised() const;
/** * @brief Computes the cross product with another vector.
* @param rhl The other vector.
* @return The resulting cross product vector.
*/
Vec3 cross(const Vec3 &rhl) const;
/**
* @brief Overloaded arithmetic operators.
*/
inline Vec3& operator+=(const Vec3& rhl);
inline Vec3& operator-=(const Vec3& rhl);
inline Vec3& operator*=(const Vec3& rhl);
inline Vec3& operator/=(const Vec3& rhl);
inline Vec3& operator+=(const double& rhl);
inline Vec3& operator-=(const double& rhl);
inline Vec3& operator*=(const double& rhl);
inline Vec3& operator/=(const double& rhl);
inline double &operator[](short dim);
inline double operator[](short dim) const;
/**
* @brief Computes the minimum component of the vector.
* @return The minimum value.
*/
double min() const;
/**
* @brief Computes the maximum component of the vector.
* @return The maximum value.
*/
double max() const;
/**
* @brief Computes the minimum/maximum between two vectors.
* @return A vector containing the element-wise min/max.
*/
Vec3 minimum(const Vec3& other) const;
Vec3 maximum(const Vec3& other) const;
/**
* @brief Finds the index of the minimum/maximum element.
* @return Index of the minimum/maximum value.
*/
int argmin() const;
int argmax() const;
/**
* @brief Converts the vector to a VectorNx3 representation.
* @param N Number of rows.
* @return A VectorNx3 with repeated rows.
*/
VectorNx3 toNd(const std::size_t N) const;
/**
* @brief Accessors for individual components.
*/
double& getx();
double& gety();
double& getz();
/**
* @brief Mutators for individual components.
*/
void setx(double &val);
void sety(double &val);
void setz(double &val);
/**
* @brief Creates a copy of the vector.
* @return A new Vec3 instance with the same values. */
Vec3 copy() const;
/**
* @brief Overloaded stream output operator.
*/
friend std::ostream& operator<<(std::ostream& os, const Vec3& o);
/**
* @brief Converts the vector to a string representation.
* @return A formatted string representing the vector.
*/
std::string __str__() const;
};
/**
* @class Quaternion
* @brief Represents a quaternion, using the European convention (w, x, y, z).
*
* Quaternions are used for representing rotations in 3D space and provide
* an efficient way to handle rotations without suffering from gimbal lock.
* This class supports various constructors, arithmetic operations, and
* rotation transformations.
*
* @note The class uses a four-element vector (v4df) to store the quaternion components.
*/
class Quaternion {
protected:
v4df data = {0., 0., 0., 0.};
public:
/**
* @brief Default constructor, initializes a quaternion.
* @param qw Scalar (real) component, default is 1.
* @param qx X-component, default is 0.
* @param qy Y-component, default is 0.
* @param qz Z-component, default is 0.
*/
Quaternion(const double& qw = 1., const double& qx = 0., const double& qy = 0., const double& qz = 0.);
/**
* @brief Constructs a quaternion from a scalar and a 3D vector.
* @param w Scalar component.
* @param v 3D vector representing the imaginary part.
*/
Quaternion(const double& w, const Vec3& v);
/**
* @brief Constructs a quaternion from an axis-angle representation.
* @param axis Rotation axis (must be normalized).
* @param angle Rotation angle in radians (or degrees if deg=true).
* @param deg Whether the angle is given in degrees (default: false).
*/
Quaternion(const Vec3& axis, const double& angle, const bool& deg = false);
/**
* @brief Constructs a quaternion from a rotation vector.
* @param rotVec A vector representing the axis and angle of rotation.
* @param deg Whether the rotation vector is in degrees (default: false).
*/
Quaternion(const Vec3& rotVec, const bool& deg = false);
/**
* @brief Constructs a quaternion from a 4-element array.
* @param q An array of 4 double values.
*/
Quaternion(const std::array<double, 4>& q);
/** * @brief Constructs a quaternion from a vector of at least 4 elements.
* @param q A std::vector of doubles (must have at least 4 elements).
* @throws std::runtime_error if the vector size is < 4.
*/
Quaternion(const std::vector<double>& q);
/**
* @brief Constructs a quaternion from an initializer list.
* @param values A list of 4 double values.
* @throws std::runtime_error if the list size is < 4.
*/
Quaternion(std::initializer_list<double> values);
/**
* @brief Assigns new values to the quaternion from an initializer list.
* @param values A list of 4 double values.
* @throws std::runtime_error if the list size is < 4.
*/
void assign(std::initializer_list<double> values);
/**
* @brief Returns the vector (imaginary) part of the quaternion.
* @return Vec3 The imaginary part (x, y, z).
*/
inline Vec3 toVec3() const;
/** @brief Accessor for the scalar (real) part of the quaternion. */
double& getw();
/** @brief Accessor for the x-component of the quaternion. */
double& getx();
/** @brief Accessor for the y-component of the quaternion. */
double& gety();
/** @brief Accessor for the z-component of the quaternion. */
double& getz();
/**
* @brief Computes the conjugate of the quaternion.
* @return Quaternion with the same real part but negated imaginary parts.
*/
inline Quaternion conj() const;
/**
* @brief Computes the dot product with another quaternion.
* @param rhl The other quaternion.
* @return Dot product value.
*/
inline double dot(const Quaternion& rhl) const;
/**
* @brief Computes the squared norm of the quaternion.
* @return The squared norm.
*/
inline double norm2() const;
/**
* @brief Computes the norm (magnitude) of the quaternion.
* @return The norm of the quaternion.
*/
inline double norm() const;
/**
* @brief Normalizes the quaternion in place.
*/
inline void normalise();
/**
* @brief Returns a normalized copy of the quaternion.
* @return A normalized quaternion.
*/
inline Quaternion normalised() const;
/**
* @brief Rotates a 3D vector using the quaternion.
* @param v The vector to rotate.
* @return The rotated vector.
*/
inline Vec3 rotate(const Vec3& v) const;
/**
* @brief Overloaded rotation method for modifying the input vector.
*/
inline Vec3 rotate(Vec3& v) const;
/**
* @brief Rotates a set of 3D vectors.
* @param v The vectors to rotate.
* @return The rotated vectors.
*/
inline VectorNx3 rotate(const VectorNx3& v) const;
/**
* @brief Overloaded rotation method for modifying a set of vectors.
*/
inline VectorNx3 rotate(VectorNx3& v) const;
};
/**
* @struct Pose
* @brief Represents a 3D pose consisting of position and orientation.
*
* The Pose structure combines a position vector (`ref_r_ref_ori`) and an
* orientation quaternion (`q_ref_ori`). It provides multiple constructors
* for initialization using initializer lists, arrays, or explicit parameters.
*/
struct Pose {
Vec3 ref_r_ref_ori; ///< Position vector (x, y, z) in reference frame.
Quaternion q_ref_ori; ///< Orientation as a quaternion.
/**
* @brief Constructs a Pose from an initializer list.
* @param values A list of 7 double values (3 for position, 4 for quaternion).
* @throws std::runtime_error if the list size is not 7.
*/
Pose(const std::initializer_list<double>& values) {
if (values.size() != 7)
throw std::runtime_error("Invalid list size (must be 7)");
auto it = values.begin();
ref_r_ref_ori = {*it++, *it++, *it++};
q_ref_ori = {*it++, *it++, *it++, *it++};
}
/**
* @brief Constructs a Pose from a 7-element array.
* @param values A std::array of 7 doubles (3 for position, 4 for quaternion).
*/
Pose(const std::array<double, 7>& values) {
auto it = values.begin();
ref_r_ref_ori = {*it++, *it++, *it++};
q_ref_ori = {*it++, *it++, *it++, *it++};
}
/**
* @brief Constructs a Pose from explicit position and quaternion values.
* @param v The position vector (default: zero vector).
* @param q The orientation quaternion (default: identity quaternion).
*/
Pose(Vec3 v = Vec3(), Quaternion q = Quaternion())
: ref_r_ref_ori(v), q_ref_ori(q) {}
};
/**
* Write the pose of b (written in frame ref) wrt the frame a
* @param pose_ref_a Pose of a wrt frame ref
* @param pose_ref_b Pose of b wrt frame ref
* @return pose_a_b Pose of b wrt frale a
*/
Pose changeFrame(const Pose &pose_ref_a, const Pose &pose_ref_b);
/**
* @brief
* Grid of point on a planar surface
*/
class MeshGrid{
private:
/// @brief Scale to apply on the points coordinates
double scale;
public:
/// @brief Matrices of points (meshgrid)
MatrixX3d U, V;
/// @brief number of point along u and v axis
long nu, nv;
/**
* Create a Meshgrid
* @param u Meshgrid for u axis
* @param v Meshgrid for v axis
* @param scale Scale to apply to the grid
*/
MeshGrid(MatrixX3d u, MatrixX3d v, const double scale=1.) : scale(scale){
nu = u.size();
nv = v.size();
auto grid = nc::meshgrid(u, v);
U = grid.first * scale;
V = grid.second * scale;
}
/**
* Get the number of points in the grid
* @return the number of points as an integer
*/
inline auto size(){
return nu * nv;
}
/**
* Get an 1-D vector of the meshgrid U
* @return the flatten vector of the mesgrid U
*/
MatrixX3d flattenU(){
return U.reshape(nu*nv, 1);
}
/**
* Get an 1-D vector of the meshgrid V
* @return the flatten vector of the mesgrid V
*/
MatrixX3d flattenV(){
return V.reshape(nu*nv, 1);
}
/**
* Return a Vector of points for an XY plane located at z
* @param z position along the z-axis of the XY plane
* @return a Vector of points lying in the XY plane
*/
VectorNx3 xyPlane(double z=0.){
VectorNx3 r_pts(nu*nv); auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, U.reshape(nu*nv, 1));
r_pts.put(rows, 1, V.reshape(nu*nv, 1));
r_pts.put(rows, 2, z * scale);
return r_pts;
}
/**
* Return a Vector of points for an XZ plane located at y
* @param y position along the y-axis of the XY plane
* @return a Vector of points lying in the XZ plane
*/
VectorNx3 xzPlane(double y=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, U.reshape(nu*nv, 1));
r_pts.put(rows, 1, y * scale);
r_pts.put(rows, 2, V.reshape(nu*nv, 1));
return r_pts;
}
/**
* Return a Vector of points for an YZ plane located at x
* @param x position along the x-axis of the XY plane
* @return a Vector of points lying in the YZ plane
*/
VectorNx3 yzPlane(double x=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, x * scale);
r_pts.put(rows, 1, U.reshape(nu*nv, 1));
r_pts.put(rows, 2, V.reshape(nu*nv, 1));
return r_pts;
}
/**
* Return a Vector of points for an YX plane located at z
* @param z position along the z-axis of the YX plane
* @return a Vector of points lying in the YX plane
*/
VectorNx3 yxPlane(double z=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, V.reshape(nu*nv, 1));
r_pts.put(rows, 1, U.reshape(nu*nv, 1));
r_pts.put(rows, 2, z * scale);
return r_pts;
}
/**
* Return a Vector of points for an ZY plane located at x
* @param x position along the z-axis of the XY plane
* @return a Vector of points lying in the ZY plane
*/
VectorNx3 zyPlane(double x=0.){
VectorNx3 r_pts(nu*nv);
auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, x * scale);
r_pts.put(rows, 1, V.reshape(nu*nv, 1));
r_pts.put(rows, 2, U.reshape(nu*nv, 1));
return r_pts;
}
/**
* Return a Vector of points for an ZX plane located at z
* @param y position along the z-axis of the ZX plane
* @return a Vector of points lying in the ZX plane
*/
VectorNx3 zxPlane(double y=0.){
VectorNx3 r_pts(nu*nv); auto rows = nc::Slice(0, nu*nv);
r_pts.put(rows, 0, V.reshape(nu*nv, 1));
r_pts.put(rows, 1, y * scale);
r_pts.put(rows, 2, U.reshape(nu*nv, 1));
return r_pts;
}
};
}
#endif