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

Hat and Vee for SO3

release/4.3a0
Frank Dellaert 2019-05-06 18:51:32 -04:00 committed by Fan Jiang
parent a765886e5a
commit dea6b995e5
3 changed files with 42 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
gtsam/geometry/SOt.cpp
View File

@ -159,16 +159,25 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
// } // }
//****************************************************************************** //******************************************************************************
// Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); } template <>
Matrix3 SO3::Hat(const Vector3& xi) {
// skew symmetric matrix X = xi^
Matrix3 Y = Z_3x3;
Y(0, 1) = -xi(2);
Y(0, 2) = +xi(1);
Y(1, 2) = -xi(0);
return Y - Y.transpose();
}
// /* ************************************************************************* //******************************************************************************
// */ Vector3 SO3::Vee(const Matrix3& X) { template <>
// Vector3 xi; Vector3 SO3::Vee(const Matrix3& X) {
// xi(0) = -X(1, 2); Vector3 xi;
// xi(1) = X(0, 2); xi(0) = -X(1, 2);
// xi(2) = -X(0, 1); xi(1) = +X(0, 2);
// return xi; xi(2) = -X(0, 1);
// } return xi;
}
//****************************************************************************** //******************************************************************************
template <> template <>

9
gtsam/geometry/SOt.h
View File

@ -39,12 +39,15 @@ using SO3 = SO<3>;
// /// Static, named constructor that finds chordal mean = argmin_R \sum // /// Static, named constructor that finds chordal mean = argmin_R \sum
// sqr(|R-R_i|_F). static SO3 ChordalMean(const std::vector<SO3>& rotations); // sqr(|R-R_i|_F). static SO3 ChordalMean(const std::vector<SO3>& rotations);
// static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix
// static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat
// Below are all declarations of SO<3> specializations. // Below are all declarations of SO<3> specializations.
// They are *defined* in SO3.cpp. // They are *defined* in SO3.cpp.
template <>
Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix
template <>
Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat
/// Adjoint map /// Adjoint map
template <> template <>
Matrix3 SO3::AdjointMap() const { Matrix3 SO3::AdjointMap() const {

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

@ -44,10 +44,27 @@ TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
//****************************************************************************** //******************************************************************************
SO3 id; SO3 id;
Vector3 z_axis(0, 0, 1); Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
//******************************************************************************
TEST(SO3, Hat) {
// Check that Hat specialization is equal to dynamic version
EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
}
//******************************************************************************
TEST(SO3, Vee) {
// Check that Hat specialization is equal to dynamic version
auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));
EXPECT(assert_equal(SO3::Vee(X3), SOn::Vee(X3)));
}
//****************************************************************************** //******************************************************************************
TEST(SO3, Local) { TEST(SO3, Local) {
Vector3 expected(0, 0, 0.1); Vector3 expected(0, 0, 0.1);