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

Done with Point3 and fixed size matrices. make check works

release/4.3a0
nsrinivasan7 2014-11-30 17:49:01 -05:00
parent d9b6aed23c
commit 186b01fd71
2 changed files with 31 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/geometry/Point3.cpp
View File

@ -63,22 +63,22 @@ Point3 Point3::operator/(double s) const {
}
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
*H1 = eye(3, 3);
(*H1).setIdentity();
if (H2)
*H2 = eye(3, 3);
(*H2).setIdentity();
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
*H1 = eye(3, 3);
(*H1).setIdentity();
if (H2)
*H2 = -eye(3, 3);
(*H2).setIdentity();
return *this - q;
}
@ -106,15 +106,14 @@ double Point3::norm(OptionalJacobian<1,3> H) const {
}
/* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> H) const {
Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
Point3 normalized = *this / norm();
if (H) {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
H->resize(3, 3);
*H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
*H /= pow(x2 + y2 + z2, 1.5);
(*H) << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
(*H) /= pow(x2 + y2 + z2, 1.5);
}
return normalized;
}

38
gtsam/geometry/Point3.h
View File

@ -93,10 +93,10 @@ namespace gtsam {
/// "Compose" - just adds coordinates of two points
inline Point3 compose(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) *H1 = eye(3);
if (H2) *H2 = eye(3);
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if (H1) (*H1).setIdentity();
if (H2) (*H2).setIdentity();
return *this + p2;
}
@ -105,10 +105,10 @@ namespace gtsam {
/** Between using the default implementation */
inline Point3 between(const Point3& p2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(3);
if(H2) *H2 = eye(3);
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if(H1) (*H1) = (-Eigen::Matrix3d::Identity());
if(H2) (*H2).setIdentity();
return p2 - *this;
}
@ -142,13 +142,13 @@ namespace gtsam {
static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
/// Left-trivialized derivative of the exponential map
static Matrix dexpL(const Vector& v) {
return eye(3);
static Matrix3 dexpL(const Vector& v) {
return Eigen::Matrix3d::Identity();
}
/// Left-trivialized derivative inverse of the exponential map
static Matrix dexpInvL(const Vector& v) {
return eye(3);
static Matrix3 dexpInvL(const Vector& v) {
return Eigen::Matrix3d::Identity();
}
/// @}
@ -163,14 +163,16 @@ namespace gtsam {
/** distance between two points */
inline double distance(const Point3& p2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
double d = (p2 - *this).norm();
if (H1) {
*H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d);
*H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
(*H1) = (*H1) *(1./d);
}
if (H2) {
*H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d);
*H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
(*H2) = (*H2) *(1./d);
}
return d;
}
@ -184,7 +186,7 @@ namespace gtsam {
double norm(OptionalJacobian<1,3> H = boost::none) const;
/** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
/** cross product @return this x q */
Point3 cross(const Point3 &q) const;
@ -213,11 +215,11 @@ namespace gtsam {
/** add two points, add(this,q) is same as this + q */
Point3 add (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
/** subtract two points, sub(this,q) is same as this - q */
Point3 sub (const Point3 &q,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
/// @}