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

expmap now returns Matrix3

release/4.3a0
Frank Dellaert 2025-01-15 18:09:01 -05:00
parent d6581cdef9
commit b0e9b16cc4
3 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/geometry/SO3.cpp
View File

@ -92,7 +92,7 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
init(nearZeroApprox); init(nearZeroApprox);
} }
SO3 ExpmapFunctor::expmap() const { return SO3(I_3x3 + A * W + B * WW); } Matrix3 ExpmapFunctor::expmap() const { return I_3x3 + A * W + B * WW; }
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) { : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
template <> template <>
GTSAM_EXPORT GTSAM_EXPORT
SO3 SO3::AxisAngle(const Vector3& axis, double theta) { SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
return so3::ExpmapFunctor(axis, theta).expmap(); return SO3(so3::ExpmapFunctor(axis, theta).expmap());
} }
//****************************************************************************** //******************************************************************************
@ -251,11 +251,11 @@ template <>
GTSAM_EXPORT GTSAM_EXPORT
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) { if (H) {
so3::DexpFunctor impl(omega); so3::DexpFunctor local(omega);
*H = impl.dexp(); *H = local.dexp();
return impl.expmap(); return SO3(local.expmap());
} else { } else {
return so3::ExpmapFunctor(omega).expmap(); return SO3(so3::ExpmapFunctor(omega).expmap());
} }
} }

2
gtsam/geometry/SO3.h
View File

@ -150,7 +150,7 @@ struct GTSAM_EXPORT ExpmapFunctor {
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
/// Rodrigues formula /// Rodrigues formula
SO3 expmap() const; Matrix3 expmap() const;
protected: protected:
void init(bool nearZeroApprox = false); void init(bool nearZeroApprox = false);

8
gtsam/geometry/tests/testSO3.cpp
View File

@ -163,22 +163,22 @@ TEST(SO3, ExpmapFunctor) {
// axis angle version // axis angle version
so3::ExpmapFunctor f1(axis, angle); so3::ExpmapFunctor f1(axis, angle);
SO3 actual1 = f1.expmap(); SO3 actual1(f1.expmap());
CHECK(assert_equal(expected, actual1.matrix(), 1e-5)); CHECK(assert_equal(expected, actual1.matrix(), 1e-5));
// axis angle version, negative angle // axis angle version, negative angle
so3::ExpmapFunctor f2(axis, angle - 2*M_PI); so3::ExpmapFunctor f2(axis, angle - 2*M_PI);
SO3 actual2 = f2.expmap(); SO3 actual2(f2.expmap());
CHECK(assert_equal(expected, actual2.matrix(), 1e-5)); CHECK(assert_equal(expected, actual2.matrix(), 1e-5));
// omega version // omega version
so3::ExpmapFunctor f3(axis * angle); so3::ExpmapFunctor f3(axis * angle);
SO3 actual3 = f3.expmap(); SO3 actual3(f3.expmap());
CHECK(assert_equal(expected, actual3.matrix(), 1e-5)); CHECK(assert_equal(expected, actual3.matrix(), 1e-5));
// omega version, negative angle // omega version, negative angle
so3::ExpmapFunctor f4(axis * (angle - 2*M_PI)); so3::ExpmapFunctor f4(axis * (angle - 2*M_PI));
SO3 actual4 = f4.expmap(); SO3 actual4(f4.expmap());
CHECK(assert_equal(expected, actual4.matrix(), 1e-5)); CHECK(assert_equal(expected, actual4.matrix(), 1e-5));
} }