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

use static free function and fix comments

release/4.3a0
thduynguyen 2014-12-25 09:07:21 -05:00
parent 6feb06bc91
commit 65bbb5e12d
2 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Pose3.cpp
View File

@ -202,7 +202,15 @@ Vector6 Pose3::localCoordinates(const Pose3& T,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Pose3::computeQforExpmapDerivative(const Vector6& xi) { /**
* Compute the 3x3 bottom-left block Q of the SE3 Expmap derivative matrix
* J(xi) = [J_(w) Z_3x3;
* Q J_(w)]
* where J_(w) is the SO3 Expmap derivative.
* (see Chirikjian11book2, pg 44, eq 10.95.
* The closed-form formula is similar to formula 102 in Barfoot14tro)
*/
static Matrix3 computeQforExpmapDerivative(const Vector6& xi) {
Vector3 w(sub(xi, 0, 3)); Vector3 w(sub(xi, 0, 3));
Vector3 v(sub(xi, 3, 6)); Vector3 v(sub(xi, 3, 6));
Matrix3 V = skewSymmetric(v); Matrix3 V = skewSymmetric(v);

7
gtsam/geometry/Pose3.h
View File

@ -204,15 +204,12 @@ public:
*/ */
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none); static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
private:
static Matrix3 computeQforExpmapDerivative(const Vector6& xi);
public: public:
/// Left-trivialized derivative of the exponential map /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);
/// Left-trivialized inverse derivative of the exponential map /// Derivative of Logmap
static Matrix6 LogmapDerivative(const Vector6& xi); static Matrix6 LogmapDerivative(const Vector6& xi);
/** /**