partially fixed Pose3(). Also adressed some of frank's comments from previous commits
nsrinivasan7
parent
ccda2e1b3b
commit
1b2d86929a
|
|
@ -45,7 +45,7 @@ Matrix3 Pose2::matrix() const {
|
|||
Matrix31 T;
|
||||
T << t_.x(), t_.y(), 1.0;
|
||||
Matrix3 RT_;
|
||||
RT_.block<3,2<(0,0) = R0;
|
||||
RT_.block<3,2>(0,0) = R0;
|
||||
RT_.block<3,1>(0,2) = T;
|
||||
return RT_;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ public:
|
|||
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
|
||||
|
||||
/** Constructor from 3*3 matrix */
|
||||
Pose2(const Matrix &T) : // TODO : Change this to Optional Jacobian ??
|
||||
Pose2(const Matrix &T) :
|
||||
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
|
||||
assert(T.rows() == 3 && T.cols() == 3);
|
||||
}
|
||||
|
|
@ -169,12 +169,10 @@ public:
|
|||
* @return xihat, 3*3 element of Lie algebra that can be exponentiated
|
||||
*/
|
||||
static inline Matrix3 wedge(double vx, double vy, double w) {
|
||||
Matrix3 wedge_;
|
||||
wedge_ <<
|
||||
return (Matrix(3,3) <<
|
||||
0.,-w, vx,
|
||||
w, 0., vy,
|
||||
0., 0., 0.;
|
||||
return wedge_;
|
||||
0., 0., 0.).finished();
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -289,7 +287,7 @@ private:
|
|||
|
||||
/** specialization for pose2 wedge function (generic template in Lie.h) */
|
||||
template <>
|
||||
inline Matrix wedge<Pose2>(const Vector& xi) { // TODO : Convert to Optional Jacobian ?
|
||||
inline Matrix wedge<Pose2>(const Vector& xi) {
|
||||
return Pose2::wedge(xi(0),xi(1),xi(2));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -32,8 +32,7 @@ INSTANTIATE_LIE(Pose3);
|
|||
/** instantiate concept checks */
|
||||
GTSAM_CONCEPT_POSE_INST(Pose3);
|
||||
|
||||
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
|
||||
static const Matrix6 I6 = eye(6);
|
||||
static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3;
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3::Pose3(const Pose2& pose2) :
|
||||
|
|
@ -55,7 +54,7 @@ Matrix6 Pose3::AdjointMap() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::adjointMap(const Vector& xi) {
|
||||
Matrix6 Pose3::adjointMap(const Vector6& xi) {
|
||||
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
|
||||
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
|
||||
Matrix6 adj;
|
||||
|
|
@ -65,14 +64,15 @@ Matrix6 Pose3::adjointMap(const Vector& xi) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjoint(const Vector& xi, const Vector& y,
|
||||
boost::optional<Matrix&> H) {
|
||||
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
|
||||
OptionalJacobian<6,6> H) {
|
||||
if (H) {
|
||||
*H = zeros(6, 6);
|
||||
(*H).setZero();
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector dxi = zero(6);
|
||||
Vector6 dxi;
|
||||
dxi.setZero();
|
||||
dxi(i) = 1.0;
|
||||
Matrix Gi = adjointMap(dxi);
|
||||
Matrix6 Gi = adjointMap(dxi);
|
||||
(*H).col(i) = Gi * y;
|
||||
}
|
||||
}
|
||||
|
|
@ -80,29 +80,31 @@ Vector Pose3::adjoint(const Vector& xi, const Vector& y,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
|
||||
boost::optional<Matrix&> H) {
|
||||
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
|
||||
OptionalJacobian<6,6> H) {
|
||||
if (H) {
|
||||
*H = zeros(6, 6);
|
||||
(*H).setZero();
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
Vector dxi = zero(6);
|
||||
Vector6 dxi;
|
||||
dxi.setZero();
|
||||
dxi(i) = 1.0;
|
||||
Matrix GTi = adjointMap(dxi).transpose();
|
||||
(*H).col(i) = GTi * y;
|
||||
}
|
||||
}
|
||||
Matrix adjT = adjointMap(xi).transpose();
|
||||
return adjointMap(xi).transpose() * y;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
|
||||
Matrix6 Pose3::dExpInv_exp(const Vector6& xi) {
|
||||
// Bernoulli numbers, from Wikipedia
|
||||
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
|
||||
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
|
||||
static const int N = 5; // order of approximation
|
||||
Matrix res = I6;
|
||||
Matrix6 ad_i = I6;
|
||||
Matrix6 res;
|
||||
res.setIdentity();
|
||||
Matrix6 ad_i;
|
||||
ad_i.setIdentity();
|
||||
Matrix6 ad_xi = adjointMap(xi);
|
||||
double fac = 1.0;
|
||||
for (int i = 1; i < N; ++i) {
|
||||
|
|
@ -225,7 +227,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T,
|
|||
Matrix4 Pose3::matrix() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
const Vector3 T = t_.vector();
|
||||
Eigen::Matrix<double, 1, 4> A14;
|
||||
Matrix14 A14;
|
||||
A14 << 0.0, 0.0, 0.0, 1.0;
|
||||
Matrix4 mat;
|
||||
mat << R, T, A14;
|
||||
|
|
@ -240,12 +242,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
|
||||
boost::optional<Matrix&> Dpoint) const {
|
||||
Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
|
||||
OptionalJacobian<3,3> Dpoint) const {
|
||||
if (Dpose) {
|
||||
const Matrix3 R = R_.matrix();
|
||||
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
Dpose->resize(3, 6);
|
||||
(*Dpose) << DR, R;
|
||||
}
|
||||
if (Dpoint)
|
||||
|
|
@ -273,17 +274,17 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
|
||||
OptionalJacobian<6,6> H2) const {
|
||||
if (H1)
|
||||
*H1 = p2.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
(*H2).setIdentity();
|
||||
return (*this) * p2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
|
||||
if (H1)
|
||||
*H1 = -AdjointMap();
|
||||
Rot3 Rt = R_.inverse();
|
||||
|
|
@ -292,40 +293,44 @@ Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// between = compose(p2,inverse(p1));
|
||||
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
|
||||
OptionalJacobian<6,6> H2) const {
|
||||
Pose3 result = inverse() * p2;
|
||||
if (H1)
|
||||
*H1 = -result.inverse().AdjointMap();
|
||||
if (H2)
|
||||
*H2 = I6;
|
||||
(*H2).setIdentity();
|
||||
return result;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1,
|
||||
OptionalJacobian<1,3> H2) const {
|
||||
if (!H1 && !H2)
|
||||
return transform_to(point).norm();
|
||||
Point3 d = transform_to(point, H1, H2);
|
||||
Matrix36 D1;
|
||||
Matrix3 D2;
|
||||
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
|
||||
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
|
||||
d2);
|
||||
Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished();
|
||||
Matrix13 D_result_d ;
|
||||
D_result_d << x / n, y / n, z / n;
|
||||
if (H1)
|
||||
*H1 = D_result_d * (*H1);
|
||||
*H1 = D_result_d * (D1);
|
||||
if (H2)
|
||||
*H2 = D_result_d * (*H2);
|
||||
*H2 = D_result_d * (D2);
|
||||
return n;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) const {
|
||||
double r = range(point.translation(), H1, H2);
|
||||
double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
|
||||
OptionalJacobian<1,6> H2) const {
|
||||
Matrix13 D2;
|
||||
double r = range(pose.translation(), H1, H2? &D2 : 0);
|
||||
if (H2) {
|
||||
Matrix H2_ = *H2 * point.rotation().matrix();
|
||||
*H2 = zeros(1, 6);
|
||||
insertSub(*H2, H2_, 0, 3);
|
||||
Matrix13 H2_ = D2 * pose.rotation().matrix();
|
||||
(*H2).setZero();
|
||||
(*H2).block<1,3>(0,3) = H2_;
|
||||
}
|
||||
return r;
|
||||
}
|
||||
|
|
@ -347,7 +352,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
|
|||
cq *= f;
|
||||
|
||||
// Add to form H matrix
|
||||
Matrix H = zeros(3, 3);
|
||||
Matrix3 H = Eigen::Matrix3d::Zero();
|
||||
BOOST_FOREACH(const Point3Pair& pair, pairs){
|
||||
Vector dp = pair.first.vector() - cp;
|
||||
Vector dq = pair.second.vector() - cq;
|
||||
|
|
|
|||
|
|
@ -99,11 +99,11 @@ public:
|
|||
}
|
||||
|
||||
/// inverse transformation with derivatives
|
||||
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
|
||||
Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const;
|
||||
|
||||
///compose this transformation onto another (first *this and then p2)
|
||||
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
|
||||
OptionalJacobian<6, 6> H2 = boost::none) const;
|
||||
|
||||
/// compose syntactic sugar
|
||||
Pose3 operator*(const Pose3& T) const {
|
||||
|
|
@ -114,8 +114,8 @@ public:
|
|||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
* as well as optionally the derivatives
|
||||
*/
|
||||
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
|
||||
OptionalJacobian<6, 6> H2 = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
|
|
@ -186,17 +186,17 @@ public:
|
|||
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
|
||||
*
|
||||
*/
|
||||
static Matrix6 adjointMap(const Vector& xi);
|
||||
static Matrix6 adjointMap(const Vector6 &xi);
|
||||
|
||||
/**
|
||||
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
|
||||
*/
|
||||
static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
|
||||
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none);
|
||||
|
||||
/**
|
||||
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
|
||||
*/
|
||||
static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
|
||||
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
|
||||
|
||||
/**
|
||||
* Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
|
||||
|
|
@ -208,7 +208,7 @@ public:
|
|||
* Ernst Hairer, et al., Geometric Numerical Integration,
|
||||
* Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
|
||||
*/
|
||||
static Matrix6 dExpInv_exp(const Vector& xi);
|
||||
static Matrix6 dExpInv_exp(const Vector6 &xi);
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
|
|
@ -237,7 +237,7 @@ public:
|
|||
* @return point in world coordinates
|
||||
*/
|
||||
Point3 transform_from(const Point3& p,
|
||||
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
|
||||
OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const;
|
||||
|
||||
/** syntactic sugar for transform_from */
|
||||
inline Point3 operator*(const Point3& p) const { return transform_from(p); }
|
||||
|
|
@ -284,8 +284,8 @@ public:
|
|||
* @return range (double)
|
||||
*/
|
||||
double range(const Point3& point,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
OptionalJacobian<1,6> H1=boost::none,
|
||||
OptionalJacobian<1,3> H2=boost::none) const;
|
||||
|
||||
/**
|
||||
* Calculate range to another pose
|
||||
|
|
@ -293,8 +293,8 @@ public:
|
|||
* @return range (double)
|
||||
*/
|
||||
double range(const Pose3& pose,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
OptionalJacobian<1,6> H1=boost::none,
|
||||
OptionalJacobian<1,6> H2=boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
|
|
|
|||
Loading…
Reference in New Issue