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);
}
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)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
@ -193,7 +193,7 @@ Vector3 DexpFunctor::applyLeftJacobianInverse(const Vector3& v,
template <>
GTSAM_EXPORT
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
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
if (H) {
so3::DexpFunctor impl(omega);
*H = impl.dexp();
return impl.expmap();
so3::DexpFunctor local(omega);
*H = local.dexp();
return SO3(local.expmap());
} 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);
/// Rodrigues formula
SO3 expmap() const;
Matrix3 expmap() const;
protected:
void init(bool nearZeroApprox = false);

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

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