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

FrobeniusFactors now work for any Matrix Lie group with vec

release/4.3a0
Frank Dellaert 2025-03-10 21:30:50 -04:00
parent 63257bcf19
commit 60f9385006
3 changed files with 149 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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;