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

changed << to =. gives error because range() was removed from PinholeCamera.h ?

release/4.3a0
nsrinivasan7 2014-12-04 09:36:00 -05:00
parent f7ebe4bfc4
commit bd6f210b87
8 changed files with 39 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Cal3_S2.h
View File

@ -173,8 +173,8 @@ public:
inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 << -I_5x5;
if(H2) *H2 << I_5x5;
if(H1) *H1 = -I_5x5;
if(H2) *H2 = I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}

12
gtsam/geometry/Point3.cpp
View File

@ -65,20 +65,16 @@ Point3 Point3::operator/(double s) const {
/* ************************************************************************* */
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
*H1 << I_3x3;
if (H2)
*H2 << I_3x3;
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this + q;
}
/* ************************************************************************* */
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
*H1 << I_3x3;
if (H2)
*H2 << I_3x3;
if (H1) *H1 = I_3x3;
if (H2) *H2 = I_3x3;
return *this - q;
}

4
gtsam/geometry/Point3.h
View File

@ -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 << -I_3x3;
if(H2) *H2 << I_3x3;
if(H1) *H1 = -I_3x3;
if(H2) *H2 = I_3x3;
return p2 - *this;
}

22
gtsam/geometry/Pose2.cpp
View File

@ -125,7 +125,7 @@ Matrix3 Pose2::AdjointMap() const {
/* ************************************************************************* */
Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 << -AdjointMap();
if (H1) *H1 = -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
@ -148,8 +148,8 @@ Point2 Pose2::transform_to(const Point2& point,
Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
// TODO: inline and reuse?
if(H1) *H1 << p2.inverse().AdjointMap();
if(H2) *H2 << I_3x3;
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I_3x3;
return (*this)*p2;
}
@ -163,7 +163,7 @@ Point2 Pose2::transform_from(const Point2& p,
Matrix21 Drotate1;
Drotate1 << -q.y(), q.x();
if (H1) *H1 << R, Drotate1;
if (H2) *H2 << R; // R
if (H2) *H2 = R; // R
}
return q + t_;
}
@ -197,7 +197,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 << I_3x3;
if (H2) *H2 = I_3x3;
return Pose2(R,t);
}
@ -211,8 +211,8 @@ Rot2 Pose2::bearing(const Point2& point,
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix12 D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 << D_result_d * (D1);
if (H2) *H2 << D_result_d * (D2);
if (H1) *H1 = D_result_d * (D1);
if (H2) *H2 = D_result_d * (D2);
return result;
}
@ -238,9 +238,9 @@ double Pose2::range(const Point2& point,
Matrix23 H1_;
H1_ << -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 << H * H1_;
*H1 = H * H1_;
}
if (H2) *H2 << H;
if (H2) *H2 = H;
return r;
}
@ -257,14 +257,14 @@ double Pose2::range(const Pose2& pose,
H1_ <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 << H * H1_;
*H1 = H * H1_;
}
if (H2) {
Matrix23 H2_;
H2_ <<
pose.r_.c(), -pose.r_.s(), 0.0,
pose.r_.s(), pose.r_.c(), 0.0;
*H2 << H * H2_;
*H2 = H * H2_;
}
return r;
}

16
gtsam/geometry/Pose3.cpp
View File

@ -274,14 +274,14 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
if (H1) *H1 << p2.inverse().AdjointMap();
if (H2) *H2 << I_6x6;
if (H1) *H1 = p2.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
if (H1) *H1 << -AdjointMap();
if (H1) *H1 = -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
@ -291,8 +291,8 @@ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) 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 << I_6x6;
if (H1) *H1 = -result.inverse().AdjointMap();
if (H2) *H2 = I_6x6;
return result;
}
@ -309,10 +309,8 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
n = sqrt(d2);
Matrix13 D_result_d;
D_result_d << x / n, y / n, z / n;
if (H1)
*H1 = D_result_d * D1;
if (H2)
*H2 = D_result_d * D2;
if (H1) *H1 = D_result_d * D1;
if (H2) *H2 = D_result_d * D2;
return n;
}
}

8
gtsam/geometry/Rot2.h
View File

@ -118,8 +118,8 @@ namespace gtsam {
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << 1; // set to 1x1 identity matrix
if (H2) *H2 << 1; // set to 1x1 identity matrix
if (H1) *H1 = I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
@ -131,8 +131,8 @@ namespace gtsam {
/** Between using the default implementation */
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << -1; // set to 1x1 identity matrix
if (H2) *H2 << 1; // set to 1x1 identity matrix
if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix
if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}

4
gtsam/geometry/Rot3.cpp
View File

@ -73,7 +73,7 @@ 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 (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
return q;
}
@ -83,7 +83,7 @@ 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 (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
if (HR) *HR = q.basis().transpose() * q.skew();
return q;
}

14
gtsam/geometry/Rot3M.cpp
View File

@ -141,8 +141,8 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
if (H1) *H1 << R2.transpose();
if (H2) *H2 << I_3x3;
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I_3x3;
return *this * R2;
}
@ -159,15 +159,15 @@ Matrix3 Rot3::transpose() const {
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
if (H1) *H1 << -rot_;
if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 << -(R2.transpose()*rot_);
if (H2) *H2 << I_3x3;
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I_3x3;
Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
}
@ -176,8 +176,8 @@ Rot3 Rot3::between (const Rot3& R2,
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1 || H2) {
if (H1) *H1 << rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 << rot_;
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
}
return Point3(rot_ * p.vector());
}