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

Expmap to .cpp file

release/4.3a0
Frank Dellaert 2025-04-15 09:44:09 -04:00
parent 7cb169f12c
commit b0fb28f087
3 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Rot3.h
View File

@ -374,20 +374,13 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
using LieAlgebra = Matrix3; using LieAlgebra = Matrix3;
/** /**
* Exponential map at identity - create a rotation from canonical coordinates * Exponential map - create a rotation from canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
*/ */
static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {}) { static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = {});
if(H) *H = Rot3::ExpmapDerivative(v);
#ifdef GTSAM_USE_QUATERNIONS
return traits<gtsam::Quaternion>::Expmap(v);
#else
return Rot3(traits<SO3>::Expmap(v));
#endif
}
/** /**
* Log map at identity - returns the canonical coordinates * Log map - returns the canonical coordinates
* \f$ [R_x,R_y,R_z] \f$ of this rotation * \f$ [R_x,R_y,R_z] \f$ of this rotation
*/ */
static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {}); static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = {});

6
gtsam/geometry/Rot3M.cpp
View File

@ -153,7 +153,11 @@ Point3 Rot3::rotate(const Point3& p,
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation Rot3 Rot3::Expmap(const Vector3& v, OptionalJacobian<3,3> H) {
return Rot3(SO3::Expmap(v, H));
}
/* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) {
return SO3::Logmap(R.rot_,H); return SO3::Logmap(R.rot_,H);
} }

6
gtsam/geometry/Rot3Q.cpp
View File

@ -116,6 +116,12 @@ namespace gtsam {
return Point3(r.x(), r.y(), r.z()); return Point3(r.x(), r.y(), r.z());
} }
/* ************************************************************************* */
Rot3 Rot3::Expmap(const Vector3& v, OptionalJacobian<3,3> H) {
if(H) *H = Rot3::ExpmapDerivative(v);
return traits<gtsam::Quaternion>::Expmap(v);
}
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) {
return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H); return traits<gtsam::Quaternion>::Logmap(R.quaternion_, H);