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

Made retract etc take Fixed vectors in poses

release/4.3a0
dellaert 2015-05-26 00:10:03 -07:00
parent f3bb4434ab
commit 22ff9b6aef
5 changed files with 21 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose2.cpp
View File

@ -59,7 +59,7 @@ bool Pose2::equals(const Pose2& q, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector& xi, OptionalJacobian<3, 3> H) { Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) {
if (H) *H = Pose2::ExpmapDerivative(xi); if (H) *H = Pose2::ExpmapDerivative(xi);
assert(xi.size() == 3); assert(xi.size() == 3);
Point2 v(xi(0),xi(1)); Point2 v(xi(0),xi(1));
@ -130,7 +130,7 @@ Matrix3 Pose2::AdjointMap() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Pose2::adjointMap(const Vector& v) { Matrix3 Pose2::adjointMap(const Vector3& v) {
// See Chirikjian12book2, vol.2, pg. 36 // See Chirikjian12book2, vol.2, pg. 36
Matrix3 ad = zeros(3,3); Matrix3 ad = zeros(3,3);
ad(0,1) = -v[2]; ad(0,1) = -v[2];

7
gtsam/geometry/Pose2.h
View File

@ -118,7 +118,7 @@ public:
/// @{ /// @{
///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$ ///Exponential map at identity - create a rotation from canonical coordinates \f$ [T_x,T_y,\theta] \f$
static Pose2 Expmap(const Vector& xi, ChartJacobian H = boost::none); static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none); static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
@ -128,15 +128,14 @@ public:
* Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi) * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
*/ */
Matrix3 AdjointMap() const; Matrix3 AdjointMap() const;
inline Vector Adjoint(const Vector& xi) const { inline Vector3 Adjoint(const Vector3& xi) const {
assert(xi.size() == 3);
return AdjointMap()*xi; return AdjointMap()*xi;
} }
/** /**
* Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19 * Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19
*/ */
static Matrix3 adjointMap(const Vector& v); static Matrix3 adjointMap(const Vector3& v);
/** /**
* wedge for SE(2): * wedge for SE(2):

10
gtsam/geometry/Pose3.cpp
View File

@ -111,7 +111,7 @@ bool Pose3::equals(const Pose3& pose, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
/** Modified from Murray94book version (which assumes w and v normalized?) */ /** Modified from Murray94book version (which assumes w and v normalized?) */
Pose3 Pose3::Expmap(const Vector& xi, OptionalJacobian<6, 6> H) { Pose3 Pose3::Expmap(const Vector6& xi, OptionalJacobian<6, 6> H) {
if (H) { if (H) {
*H = ExpmapDerivative(xi); *H = ExpmapDerivative(xi);
} }
@ -354,7 +354,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
return boost::none; // we need at least three pairs return boost::none; // we need at least three pairs
// calculate centroids // calculate centroids
Vector cp = zero(3), cq = zero(3); Vector3 cp = Vector3::Zero(), cq = Vector3::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs) { BOOST_FOREACH(const Point3Pair& pair, pairs) {
cp += pair.first.vector(); cp += pair.first.vector();
cq += pair.second.vector(); cq += pair.second.vector();
@ -364,10 +364,10 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
cq *= f; cq *= f;
// Add to form H matrix // Add to form H matrix
Matrix3 H = Eigen::Matrix3d::Zero(); Matrix3 H = Z_3x3;
BOOST_FOREACH(const Point3Pair& pair, pairs) { BOOST_FOREACH(const Point3Pair& pair, pairs) {
Vector dp = pair.first.vector() - cp; Vector3 dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq; Vector3 dq = pair.second.vector() - cq;
H += dp * dq.transpose(); H += dp * dq.transpose();
} }

4
gtsam/geometry/Pose3.h
View File

@ -110,7 +110,7 @@ public:
/// @{ /// @{
/// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$
static Pose3 Expmap(const Vector& xi, OptionalJacobian<6, 6> H = boost::none); static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> H = boost::none);
/// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of this rotation
static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none); static Vector6 Logmap(const Pose3& p, OptionalJacobian<6, 6> H = boost::none);
@ -125,7 +125,7 @@ public:
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
*/ */
Vector Adjoint(const Vector& xi_b) const { Vector6 Adjoint(const Vector6& xi_b) const {
return AdjointMap() * xi_b; return AdjointMap() * xi_b;
} /// FIXME Not tested - marked as incorrect } /// FIXME Not tested - marked as incorrect

2
gtsam/geometry/tests/testPose2.cpp
View File

@ -104,7 +104,7 @@ TEST(Pose2, expmap3) {
Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0; Matrix A2 = A*A/2.0, A3 = A2*A/3.0, A4=A3*A/4.0;
Matrix expected = eye(3) + A + A2 + A3 + A4; Matrix expected = eye(3) + A + A2 + A3 + A4;
Vector v = Vector3(0.01, -0.015, 0.99); Vector3 v(0.01, -0.015, 0.99);
Pose2 pose = Pose2::Expmap(v); Pose2 pose = Pose2::Expmap(v);
Pose2 pose2(v); Pose2 pose2(v);
EXPECT(assert_equal(pose, pose2)); EXPECT(assert_equal(pose, pose2));