64 lines
1.6 KiB
C++
64 lines
1.6 KiB
C++
//
|
|
// Created by darshan on 3/11/25.
|
|
//
|
|
|
|
#ifndef G_H
|
|
#define G_H
|
|
|
|
|
|
#include <gtsam/geometry/Rot3.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/base/Vector.h>
|
|
#include <vector>
|
|
|
|
using namespace gtsam;
|
|
|
|
/**
|
|
* Symmetry group (SO(3) |x so(3)) x SO(3) x ... x SO(3)
|
|
* Each element of the B list is associated with a calibration state
|
|
*/
|
|
class G {
|
|
public:
|
|
Rot3 A; // First SO(3) element
|
|
Matrix3 a; // so(3) element (skew-symmetric matrix)
|
|
std::vector<Rot3> B; // List of SO(3) elements for calibration
|
|
|
|
/**
|
|
* Initialize the symmetry group G
|
|
* @param A SO3 element
|
|
* @param a so(3) element (skew symmetric matrix)
|
|
* @param B list of SO3 elements
|
|
*/
|
|
G(const Rot3& A = Rot3::Identity(),
|
|
const Matrix3& a = Matrix3::Zero(),
|
|
const std::vector<Rot3>& B = std::vector<Rot3>());
|
|
|
|
/**
|
|
* Define the group operation (multiplication)
|
|
* @param other Another group element
|
|
* @return The product of this and other
|
|
*/
|
|
G operator*(const G& other) const;
|
|
|
|
/**
|
|
* Return the inverse element of the symmetry group
|
|
* @return The inverse of this group element
|
|
*/
|
|
G inv() const;
|
|
|
|
/**
|
|
* Return the identity of the symmetry group
|
|
* @param n Number of calibration elements
|
|
* @return The identity element with n calibration components
|
|
*/
|
|
static G identity(int n);
|
|
|
|
/**
|
|
* Return a group element X given by X = exp(x)
|
|
* @param x Vector representation of Lie algebra element
|
|
* @return Group element given by the exponential of x
|
|
*/
|
|
static G exp(const Vector& x);
|
|
};
|
|
#endif //G_H
|