12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2052 from borglab/feature/FrobeniusFactors 

More FrobeniusFactors
release/4.3a0
Frank Dellaert 2025-03-10 23:35:58 -04:00 committed by GitHub
parent 572b11fee4 7750dec317
commit 33d2262e07
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
17 changed files with 292 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/base/Lie.h
View File

@ -229,7 +229,7 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
/// Both LieGroupTraits and Testable
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits
/// Adds LieAlgebra, Hat, and Vee to LieGroupTraits
template<class Class> struct MatrixLieGroupTraits: LieGroupTraits<Class> {
using LieAlgebra = typename Class::LieAlgebra;
using TangentVector = typename LieGroupTraits<Class>::TangentVector;
@ -351,7 +351,7 @@ template <class T> Matrix wedge(const Vector& x);
*/
template <class T>
T expm(const Vector& x, int K = 7) {
auto xhat = T::Hat(x);
const Matrix xhat = T::Hat(x);
return T(expm(xhat, K));
}

26
gtsam/geometry/Pose2.cpp
View File

@ -323,6 +323,32 @@ double Pose2::range(const Pose2& pose,
return r;
}
/* ************************************************************************* */
// Compute vectorized Lie algebra generators for SE(2)
static Matrix93 VectorizedGenerators() {
Matrix93 G;
for (size_t j = 0; j < 3; j++) {
const Matrix3 X = Pose2::Hat(Vector::Unit(3, j));
G.col(j) = Eigen::Map<const Vector9>(X.data());
}
return G;
}
Vector9 Pose2::vec(OptionalJacobian<9, 3> H) const {
// Vectorize
const Matrix3 M = matrix();
const Vector9 X = Eigen::Map<const Vector9>(M.data());
// If requested, calculate H as (I_3 \oplus M) * G.
if (H) {
static const Matrix93 G = VectorizedGenerators(); // static to compute only once
for (size_t i = 0; i < 3; i++)
H->block(i * 3, 0, 3, dimension) = M * G.block(i * 3, 0, 3, dimension);
}
return X;
}
/* *************************************************************************
* Align finds the angle using a linear method:
* a = Pose2::transformFrom(b) = t + R*b

3
gtsam/geometry/Pose2.h
View File

@ -328,6 +328,9 @@ public:
*/
static std::pair<size_t, size_t> rotationInterval() { return {2, 2}; }
/// Return vectorized SE(2) matrix in column order.
Vector9 vec(OptionalJacobian<9, 3> H = {}) const;
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose2& p);

28
gtsam/geometry/Pose3.cpp
View File

@ -534,6 +534,34 @@ Pose3 Pose3::slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx, Opti
return interpolate(*this, other, t, Hx, Hy);
}
/* ************************************************************************* */
// Compute vectorized Lie algebra generators for SE(3)
using Matrix16x6 = Eigen::Matrix<double, 16, 6>;
using Vector16 = Eigen::Matrix<double, 16, 1>;
static Matrix16x6 VectorizedGenerators() {
Matrix16x6 G;
for (size_t j = 0; j < 6; j++) {
const Matrix4 X = Pose3::Hat(Vector::Unit(6, j));
G.col(j) = Eigen::Map<const Vector16>(X.data());
}
return G;
}
Vector Pose3::vec(OptionalJacobian<16, 6> H) const {
// Vectorize
const Matrix4 M = matrix();
const Vector X = Eigen::Map<const Vector16>(M.data());
// If requested, calculate H as (I_4 \oplus M) * G.
if (H) {
static const Matrix16x6 G = VectorizedGenerators(); // static to compute only once
for (size_t i = 0; i < 4; i++)
H->block(i * 4, 0, 4, dimension) = M * G.block(i * 4, 0, 4, dimension);
}
return X;
}
/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
// Both Rot3 and Point3 have ostream definitions so we use them.

3
gtsam/geometry/Pose3.h
View File

@ -392,6 +392,9 @@ public:
Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = {},
OptionalJacobian<6, 6> Hy = {}) const;
/// Return vectorized SE(3) matrix in column order.
Vector vec(OptionalJacobian<16, 6> H = {}) const;
/// Output stream operator
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

16
gtsam/geometry/Rot2.cpp
View File

@ -157,6 +157,22 @@ Rot2 Rot2::ClosestTo(const Matrix2& M) {
return Rot2::fromCosSin(c, s);
}
/* ************************************************************************* */
Vector4 Rot2::vec(OptionalJacobian<4, 1> H) const {
// Vectorize
const Matrix2 M = matrix();
const Vector4 X = Eigen::Map<const Vector4>(M.data());
// If requested, calculate H as (I_3 \oplus M) * G.
if (H) {
static const Matrix41 G = (Matrix41() << 0, 1, -1, 0).finished();
for (size_t i = 0; i < 2; i++)
H->block(i * 2, 0, 2, dimension) = M * G.block(i * 2, 0, 2, dimension);
}
return X;
}
/* ************************************************************************* */
} // gtsam

3
gtsam/geometry/Rot2.h
View File

@ -223,6 +223,9 @@ namespace gtsam {
/** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M);
/// Return vectorized SO(2) matrix in column order.
Vector4 vec(OptionalJacobian<4, 1> H = {}) const;
/// @}
private:

2
gtsam/geometry/Rot3.cpp
View File

@ -146,6 +146,7 @@ Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
Point3 Rot3::column(int index) const{
if(index == 3)
return r3();
@ -156,6 +157,7 @@ Point3 Rot3::column(int index) const{
else
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
}
#endif
/* ************************************************************************* */
Vector3 Rot3::xyz(OptionalJacobian<3, 3> H) const {

17
gtsam/geometry/Rot3.h
View File

@ -459,9 +459,6 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
*/
Matrix3 transpose() const;
/// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const;
Point3 r1() const; ///< first column
Point3 r2() const; ///< second column
Point3 r3() const; ///< third column
@ -530,14 +527,26 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
/**
* @brief Spherical Linear intERPolation between *this and other
* @param t a value between 0 and 1
* @param other final point of iterpolation geodesic on manifold
* @param other final point of interpolation geodesic on manifold
*/
Rot3 slerp(double t, const Rot3& other) const;
/// Vee maps from Lie algebra to tangent vector
inline Vector9 vec(OptionalJacobian<9, 3> H = {}) const { return SO3(matrix()).vec(H); }
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @}
/// @name deprecated
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const;
#endif
/// @}
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION

1
gtsam/geometry/geometry.i
View File

@ -395,7 +395,6 @@ class Rot3 {
// Standard Interface
gtsam::Matrix matrix() const;
gtsam::Matrix transpose() const;
gtsam::Point3 column(size_t index) const;
gtsam::Vector xyz() const;
gtsam::Vector ypr() const;
gtsam::Vector rpy() const;

17
gtsam/geometry/tests/testPose2.cpp
View File

@ -958,6 +958,23 @@ TEST(Pose2, Print) {
EXPECT(assert_print_equal(expected2, pose, s));
}
/* ************************************************************************* */
TEST(Pose2, vec) {
// Test a simple pose
Pose2 pose(Rot2::fromAngle(M_PI / 4), Point2(1, 2));
// Test the 'vec' method
Vector9 expected_vec = Eigen::Map<Vector9>(pose.matrix().data());
Matrix93 actualH;
Vector9 actual_vec = pose.vec(actualH);
EXPECT(assert_equal(expected_vec, actual_vec));
// Verify Jacobian with numerical derivatives
std::function<Vector9(const Pose2&)> f = [](const Pose2& p) { return p.vec(); };
Matrix93 numericalH = numericalDerivative11<Vector9, Pose2>(f, pose);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

15
gtsam/geometry/tests/testPose3.cpp
View File

@ -1401,6 +1401,21 @@ TEST(Pose3, ExpmapChainRule) {
EXPECT(assert_equal<Matrix6>(expected2, analytic, 1e-5)); // note tolerance
}
/* ************************************************************************* */
TEST(Pose3, vec) {
// Test the 'vec' method
using Vector16 = Eigen::Matrix<double, 16, 1>;
Vector16 expected_vec = Eigen::Map<Vector16>(T.matrix().data());
Matrix actualH;
Vector16 actual_vec = T.vec(actualH);
EXPECT(assert_equal(expected_vec, actual_vec));
// Verify Jacobian with numerical derivatives
std::function<Vector16(const Pose3&)> f = [](const Pose3& p) { return p.vec(); };
Matrix numericalH = numericalDerivative11<Vector16, Pose3>(f, T);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;

14
gtsam/geometry/tests/testRot2.cpp
View File

@ -174,6 +174,20 @@ TEST( Rot2, relativeBearing )
CHECK(assert_equal(expectedH,actualH));
}
/* ************************************************************************* */
TEST(Rot2, vec) {
// Test the 'vec' method
Vector4 expected_vec = Eigen::Map<Vector4>(R.matrix().data());
Matrix41 actualH;
Vector4 actual_vec = R.vec(actualH);
EXPECT(assert_equal(expected_vec, actual_vec));
// Verify Jacobian with numerical derivatives
std::function<Vector4(const Rot2&)> f = [](const Rot2& p) { return p.vec(); };
Matrix41 numericalH = numericalDerivative11<Vector4, Rot2>(f, R);
EXPECT(assert_equal(numericalH, actualH, 1e-9));
}
//******************************************************************************
namespace {
Rot2 id;

2
gtsam/geometry/tests/testUnit3.cpp
View File

@ -67,7 +67,7 @@ static Unit3 rotate_(const Rot3& R, const Unit3& p) {
TEST(Unit3, rotate) {
Rot3 R = Rot3::Yaw(0.5);
Unit3 p(1, 0, 0);
Unit3 expected = Unit3(R.column(1));
Unit3 expected = Unit3(R.matrix().col(0));
Unit3 actual = R * p;
EXPECT(assert_equal(expected, actual, 1e-5));
Matrix actualH, expectedH;

84
gtsam/slam/FrobeniusFactor.h
View File

@ -33,11 +33,10 @@ namespace gtsam {
* model with sigma=1.0.
* If not, we we check if the d-dimensional noise model on rotations is
* isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an
* error.
* If the noise model is a robust error model, we use the sigmas of the
* error. If the noise model is a robust error model, we use the sigmas of the
* underlying noise model.
*
* If defaultToUnit == false throws an exception on unexepcted input.
* If defaultToUnit == false throws an exception on unexpected input.
*/
GTSAM_EXPORT SharedNoiseModel
ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
@ -47,29 +46,30 @@ ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
* FrobeniusPrior calculates the Frobenius norm between a given matrix and an
* element of SO(3) or SO(4).
*/
template <class Rot>
class FrobeniusPrior : public NoiseModelFactorN<Rot> {
inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
using MatrixNN = typename Rot::MatrixNN;
template <class T>
class FrobeniusPrior : public NoiseModelFactorN<T> {
inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
inline constexpr static auto Dim = N * N;
using MatrixNN = Eigen::Matrix<double, N, N>;
Eigen::Matrix<double, Dim, 1> vecM_; ///< vectorized matrix to approximate
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor1<Rot>::evaluateError;
using NoiseModelFactor1<T>::evaluateError;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor
FrobeniusPrior(Key j, const MatrixNN& M,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
: NoiseModelFactorN<T>(ConvertNoiseModel(model, Dim), j) {
vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
}
/// Error is just Frobenius norm between Rot element and vectorized matrix M.
Vector evaluateError(const Rot& R, OptionalMatrixType H) const override {
return R.vec(H) - vecM_; // Jacobian is computed only when needed.
/// Error is just Frobenius norm between T element and vectorized matrix M.
Vector evaluateError(const T& g, OptionalMatrixType H) const override {
return g.vec(H) - vecM_; // Jacobian is computed only when needed.
}
};
@ -77,24 +77,24 @@ class FrobeniusPrior : public NoiseModelFactorN<Rot> {
* FrobeniusFactor calculates the Frobenius norm between rotation matrices.
* The template argument can be any fixed-size SO<N>.
*/
template <class Rot>
class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
template <class T>
class FrobeniusFactor : public NoiseModelFactorN<T, T> {
inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
inline constexpr static auto Dim = N * N;
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Rot, Rot>::evaluateError;
using NoiseModelFactor2<T, T>::evaluateError;
/// Constructor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
j2) {}
: NoiseModelFactorN<T, T>(ConvertNoiseModel(model, Dim), j1, j2) {}
/// Error is just Frobenius norm between rotation matrices.
Vector evaluateError(const Rot& R1, const Rot& R2,
Vector evaluateError(const T& T1, const T& T2,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
Vector error = R2.vec(H2) - R1.vec(H1);
Vector error = T2.vec(H2) - T1.vec(H1);
if (H1) *H1 = -*H1;
return error;
}
@ -106,17 +106,18 @@ class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
* Logmap of the error). This factor is only defined for fixed-dimension types,
* and in fact only SO3 and SO4 really work, as we need SO<N>::AdjointMap.
*/
template <class Rot>
class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
Rot R12_; ///< measured rotation between R1 and R2
Eigen::Matrix<double, Rot::dimension, Rot::dimension>
R2hat_H_R1_; ///< fixed derivative of R2hat wrpt R1
inline constexpr static auto Dim = Rot::VectorN2::RowsAtCompileTime;
template <class T>
class FrobeniusBetweenFactor : public NoiseModelFactorN<T, T> {
T T12_; ///< measured rotation between T1 and T2
Eigen::Matrix<double, T::dimension, T::dimension>
T2hat_H_T1_; ///< fixed derivative of T2hat wrpt T1
inline constexpr static auto N = T::LieAlgebra::RowsAtCompileTime;
inline constexpr static auto Dim = N * N;
public:
// Provide access to the Matrix& version of evaluateError:
using NoiseModelFactor2<Rot, Rot>::evaluateError;
using NoiseModelFactor2<T, T>::evaluateError;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@ -124,12 +125,11 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
/// @{
/// Construct from two keys and measured rotation
FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
FrobeniusBetweenFactor(Key j1, Key j2, const T& T12,
const SharedNoiseModel& model = nullptr)
: NoiseModelFactorN<Rot, Rot>(
ConvertNoiseModel(model, Dim), j1, j2),
R12_(R12),
R2hat_H_R1_(R12.inverse().AdjointMap()) {}
: NoiseModelFactorN<T, T>(ConvertNoiseModel(model, Dim), j1, j2),
T12_(T12),
T2hat_H_T1_(T12.inverse().AdjointMap()) {}
/// @}
/// @name Testable
@ -139,10 +139,10 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
void
print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(T).name())
<< ">(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
traits<Rot>::Print(R12_, " R12: ");
traits<T>::Print(T12_, " T12: ");
this->noiseModel_->print(" noise model: ");
}
@ -150,21 +150,21 @@ class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
bool equals(const NonlinearFactor &expected,
double tol = 1e-9) const override {
auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
traits<Rot>::Equals(this->R12_, e->R12_, tol);
return e != nullptr && NoiseModelFactorN<T, T>::equals(*e, tol) &&
traits<T>::Equals(this->T12_, e->T12_, tol);
}
/// @}
/// @name NoiseModelFactorN methods
/// @{
/// Error is Frobenius norm between R1*R12 and R2.
Vector evaluateError(const Rot& R1, const Rot& R2,
/// Error is Frobenius norm between T1*T12 and T2.
Vector evaluateError(const T& T1, const T& T2,
OptionalMatrixType H1, OptionalMatrixType H2) const override {
const Rot R2hat = R1.compose(R12_);
Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
const T T2hat = T1.compose(T12_);
Eigen::Matrix<double, Dim, T::dimension> vec_H_T2hat;
Vector error = T2.vec(H2) - T2hat.vec(H1 ? &vec_H_T2hat : nullptr);
if (H1) *H1 = -vec_H_T2hat * T2hat_H_T1_;
return error;
}
/// @}

12
gtsam/slam/slam.i
View File

@ -403,21 +403,21 @@ gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
size_t d);
template <T = {gtsam::SO3, gtsam::SO4}>
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
FrobeniusFactor(size_t key1, size_t key2, gtsam::noiseModel::Base* model);
gtsam::Vector evaluateError(const T& R1, const T& R2);
gtsam::Vector evaluateError(const T& T1, const T& T2);
};
template <T = {gtsam::SO3, gtsam::SO4}>
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
virtual class FrobeniusBetweenFactor : gtsam::NoiseModelFactor {
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12);
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& R12,
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& T12);
FrobeniusBetweenFactor(size_t key1, size_t key2, const T& T12,
gtsam::noiseModel::Base* model);
gtsam::Vector evaluateError(const T& R1, const T& R2);
gtsam::Vector evaluateError(const T& T1, const T& T2);
};
#include <gtsam/slam/TriangulationFactor.h>

110
gtsam/slam/tests/testFrobeniusFactor.cpp
View File

@ -9,18 +9,20 @@
* -------------------------------------------------------------------------- */
/**
/**
* testFrobeniusFactor.cpp
*
* @file testFrobeniusFactor.cpp
* @date March 2019
* @author Frank Dellaert
* @brief Check evaluateError for various Frobenius norm
* @brief Check evaluateError for various Frobenius norms
*/
#include <gtsam/base/lieProxies.h>
#include <gtsam/base/testLie.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/SO3.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@ -30,17 +32,16 @@
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
/* ************************************************************************* */
namespace so3 {
SO3 id;
Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1);
Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2);
SO3 R12 = R1.between(R2);
SO3 id;
Vector3 v1 = (Vector(3) << 0.1, 0, 0).finished();
SO3 R1 = SO3::Expmap(v1);
Vector3 v2 = (Vector(3) << 0.01, 0.02, 0.03).finished();
SO3 R2 = SO3::Expmap(v2);
SO3 R12 = R1.between(R2);
} // namespace so3
/* ************************************************************************* */
@ -89,7 +90,7 @@ TEST(FrobeniusPriorSO3, ChordalL2mean) {
SO3 expected; // identity
Matrix3 M = R1.matrix() + R1.matrix().transpose();
EXPECT(assert_equal(expected, SO3::ClosestTo(M), 1e-6));
EXPECT(assert_equal(expected, SO3::ChordalMean({R1, R1.inverse()}), 1e-6));
EXPECT(assert_equal(expected, SO3::ChordalMean({ R1, R1.inverse() }), 1e-6));
// manifold optimization gets same result as ChordalMean
NonlinearFactorGraph graph;
@ -148,13 +149,13 @@ TEST(FrobeniusBetweenFactorSO3, evaluateError) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
//******************************************************************************
/* ************************************************************************* */
namespace so4 {
SO4 id;
Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
SO4 Q2 = SO4::Expmap(v2);
SO4 id;
Vector6 v1 = (Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
SO4 Q1 = SO4::Expmap(v1);
Vector6 v2 = (Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
SO4 Q2 = SO4::Expmap(v2);
} // namespace so4
/* ************************************************************************* */
@ -174,7 +175,7 @@ TEST(FrobeniusFactorSO4, evaluateError) {
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorSO4, evaluateError) {
using namespace ::so4;
Matrix4 M{I_4x4};
Matrix4 M{ I_4x4 };
M.topLeftCorner<3, 3>() = ::so3::R12.matrix();
auto factor = FrobeniusBetweenFactor<SO4>(1, 2, Q1.between(Q2));
Matrix H1, H2;
@ -188,6 +189,79 @@ TEST(FrobeniusBetweenFactorSO4, evaluateError) {
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
namespace pose2 {
Pose2 id;
Pose2 P1 = Pose2(0.1, 0.2, 0.3);
Pose2 P2 = Pose2(0.4, 0.5, 0.6);
} // namespace pose2
/* ************************************************************************* */
TEST(FrobeniusFactorPose2, evaluateError) {
using namespace ::pose2;
auto factor = FrobeniusFactor<Pose2>(1, 2, noiseModel::Unit::Create(3));
Vector actual = factor.evaluateError(P1, P2);
Vector expected = P2.vec() - P1.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, P1);
values.insert(2, P2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorPose2, evaluateError) {
using namespace ::pose2;
auto factor = FrobeniusBetweenFactor<Pose2>(1, 2, P1.between(P2));
Matrix H1, H2;
Vector actual = factor.evaluateError(P1, P2, H1, H2);
Vector expected = Vector9::Zero();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, P1);
values.insert(2, P2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
namespace pose3 {
Pose3 id;
Pose3 P1 = Pose3(Rot3::Expmap(Vector3(0.1, 0.2, 0.3)), Vector3(0.4, 0.5, 0.6));
Pose3 P2 = Pose3(Rot3::Expmap(Vector3(0.2, 0.3, 0.4)), Vector3(0.7, 0.8, 0.9));
} // namespace pose3
/* ************************************************************************* */
TEST(FrobeniusFactorPose3, evaluateError) {
using namespace ::pose3;
auto factor = FrobeniusFactor<Pose3>(1, 2, noiseModel::Unit::Create(12));
Vector actual = factor.evaluateError(P1, P2);
Vector expected = P2.vec() - P1.vec();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, P1);
values.insert(2, P2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(FrobeniusBetweenFactorPose3, evaluateError) {
using namespace ::pose3;
auto factor = FrobeniusBetweenFactor<Pose3>(1, 2, P1.between(P2));
Matrix H1, H2;
Vector actual = factor.evaluateError(P1, P2, H1, H2);
Vector expected(16);
expected.setZero();
EXPECT(assert_equal(expected, actual, 1e-9));
Values values;
values.insert(1, P1);
values.insert(2, P2);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;