61 lines
1.5 KiB
C++
61 lines
1.5 KiB
C++
//
|
|
// Created by darshan on 3/11/25.
|
|
//
|
|
#include "G.h"
|
|
#include "utilities.h"
|
|
#include <stdexcept>
|
|
|
|
G::G(const Rot3& A, const Matrix3& a, const std::vector<Rot3>& B)
|
|
: A(A), a(a), B(B) {}
|
|
|
|
G G::operator*(const G& other) const {
|
|
if (B.size() != other.B.size()) {
|
|
throw std::invalid_argument("Group elements must have the same number of calibration elements");
|
|
}
|
|
|
|
std::vector<Rot3> new_B;
|
|
for (size_t i = 0; i < B.size(); i++) {
|
|
new_B.push_back(B[i] * other.B[i]);
|
|
}
|
|
|
|
return G(A * other.A,
|
|
a + wedge(A.matrix() * vee(other.a)),
|
|
new_B);
|
|
}
|
|
|
|
G G::inv() const {
|
|
Matrix3 A_inv = A.inverse().matrix();
|
|
|
|
std::vector<Rot3> B_inv;
|
|
for (const auto& b : B) {
|
|
B_inv.push_back(b.inverse());
|
|
}
|
|
|
|
return G(A.inverse(),
|
|
-wedge(A_inv * vee(a)),
|
|
B_inv);
|
|
}
|
|
|
|
G G::identity(int n) {
|
|
std::vector<Rot3> B(n, Rot3::Identity());
|
|
return G(Rot3::Identity(), Matrix3::Zero(), B);
|
|
}
|
|
|
|
G G::exp(const Vector& x) {
|
|
if (x.size() < 6 || x.size() % 3 != 0) {
|
|
throw std::invalid_argument("Wrong size, a vector with size multiple of 3 and at least 6 must be provided");
|
|
}
|
|
|
|
int n = (x.size() - 6) / 3;
|
|
Rot3 A = Rot3::Expmap(x.head<3>());
|
|
|
|
Vector3 a_vee = Rot3::ExpmapDerivative(-x.head<3>()) * x.segment<3>(3);
|
|
Matrix3 a = wedge(a_vee);
|
|
|
|
std::vector<Rot3> B;
|
|
for (int i = 0; i < n; i++) {
|
|
B.push_back(Rot3::Expmap(x.segment<3>(6 + 3*i)));
|
|
}
|
|
|
|
return G(A, a, B);
|
|
} |