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

fixed the (*) to -> and code some more code beautification.

release/4.3a0
nsrinivasan7 2014-12-03 11:27:18 -05:00
parent 7116661a2e
commit deff8b1e25
6 changed files with 26 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/Point3.cpp
View File

@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
(*H1) = I_3x3;
*H1 = I_3x3;
if (H2)
(*H2) = I_3x3;
*H2 = I_3x3;
return *this - q;
}
@ -112,8 +112,8 @@ Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
// 3*3 Derivative
double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
(*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;
}

12
gtsam/geometry/Point3.h
View File

@ -95,8 +95,8 @@ namespace gtsam {
inline Point3 compose(const Point3& p2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if (H1) (*H1).setIdentity();
if (H2) (*H2).setIdentity();
if (H1) H1->setIdentity();
if (H2) H2->setIdentity();
return *this + p2;
}
@ -107,8 +107,8 @@ namespace gtsam {
inline Point3 between(const Point3& p2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if(H1) (*H1) = (-Eigen::Matrix3d::Identity());
if(H2) (*H2).setIdentity();
if(H1) *H1 = _I_3x3;
if(H2) H2->setIdentity();
return p2 - *this;
}
@ -167,12 +167,12 @@ namespace gtsam {
double d = (p2 - *this).norm();
if (H1) {
*H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
(*H1) = (*H1) *(1./d);
*H1 = *H1 *(1./d);
}
if (H2) {
*H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
(*H2) = (*H2) *(1./d);
*H2 = *H2 *(1./d);
}
return d;
}

8
gtsam/geometry/Pose2.cpp
View File

@ -163,8 +163,8 @@ Point2 Pose2::transform_from(const Point2& p,
Matrix21 Drotate1;
Drotate1 << -q.y(), q.x();
if (H1) {
(*H1).block<2,2>(0,0) = R; // [R R_{pi/2}q]
(*H1).block<2,1>(0,2) = Drotate1;
H1->block<2,2>(0,0) = R; // [R R_{pi/2}q]
H1->block<2,1>(0,2) = Drotate1;
}
if (H2) *H2 = R; // R
}
@ -226,8 +226,8 @@ Rot2 Pose2::bearing(const Pose2& pose,
Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
if (H2) {
Matrix12 H2_ = D2 * pose.r().matrix();
(*H2).setZero();
(*H2).block<1,2>(0,0) = H2_;
H2->setZero();
H2->block<1,2>(0,0) = H2_;
}
return result;
}

12
gtsam/geometry/Pose3.cpp
View File

@ -65,13 +65,13 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
(*H).setZero();
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix6 Gi = adjointMap(dxi);
(*H).col(i) = Gi * y;
H->col(i) = Gi * y;
}
}
return adjointMap(xi) * y;
@ -87,7 +87,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
dxi.setZero();
dxi(i) = 1.0;
Matrix6 GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi * y;
H->col(i) = GTi * y;
}
}
return adjointMap(xi).transpose() * y;
@ -297,7 +297,7 @@ Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
if (H1)
*H1 = -result.inverse().AdjointMap();
if (H2)
(*H2) = I_6x6;
*H2 = I_6x6;
return result;
}
@ -327,8 +327,8 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix13 H2_ = D2 * pose.rotation().matrix();
(*H2).setZero();
(*H2).block<1,3>(0,3) = H2_;
H2->setZero();
H2->block<1,3>(0,3) = H2_;
}
return r;
}

6
gtsam/geometry/Rot2.cpp
View File

@ -102,13 +102,11 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if(fabs(n) > 1e-5) {
if (H) {
(*H) << -y / d2, x / d2;
*H << -y / d2, x / d2;
}
return Rot2::fromCosSin(x / n, y / n);
} else {
if (H) {
(*H) << 0.0, 0.0;
}
if (H) *H << 0.0, 0.0;
return Rot2();
}
}

12
gtsam/geometry/Rot3.cpp
View File

@ -73,10 +73,8 @@ Unit3 Rot3::rotate(const Unit3& p,
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix() * Dp;
if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew();
if (Hp) *Hp << q.basis().transpose() * matrix() * Dp;
if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
return q;
}
@ -85,10 +83,8 @@ Unit3 Rot3::unrotate(const Unit3& p,
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(unrotate(p.point3(Dp)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * Dp;
if (HR)
(*HR) = q.basis().transpose() * q.skew();
if (Hp) *Hp << q.basis().transpose() * matrix().transpose () * Dp;
if (HR) *HR = q.basis().transpose() * q.skew();
return q;
}