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

moved relative_bearing to Rot2, changed derivatives to new-style

release/4.3a0
Frank Dellaert 2010-01-14 16:57:48 +00:00
parent 53a03a0021
commit 5c0cd093fd
6 changed files with 181 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
cpp/BearingFactor.h
View File

@ -12,26 +12,6 @@
namespace gtsam { namespace gtsam {
/**
* Calculate relative bearing to a landmark in local coordinate frame
* @param point 2D location of landmark
* @param H optional reference for Jacobian
* @return 2D rotation \in SO(2)
*/
Rot2 relativeBearing(const Point2& d) {
double n = d.norm();
return Rot2(d.x() / n, d.y() / n);
}
/**
* Calculate relative bearing and optional derivative
*/
Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
return Rot2(x / n, y / n);
}
/** /**
* Calculate bearing to a landmark * Calculate bearing to a landmark
* @param pose 2D pose of robot * @param pose 2D pose of robot

18
cpp/Pose2.cpp
View File

@ -46,18 +46,18 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dbetween1(const Pose2& p0, const Pose2& p2) { Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
Matrix dt_dr = Dunrotate1(p0.r(), p2.t()-p0.t()); Point2 dt = p2.t()-p1.t();
Matrix dt_dt1 = -invcompose(p2.r(), p0.r()).matrix(); Matrix dT1 = -invcompose(p2.r(), p1.r()).matrix();
Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p0.t()); Matrix dR1;
unrotate(p2.r(), dt, dR1);
return Matrix_(3,3, return Matrix_(3,3,
dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0), dT1(0,0), dT1(0,1), dR1(0,0),
dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0), dT1(1,0), dT1(1,1), dR1(1,0),
0.0, 0.0, -1.0); 0.0, 0.0, -1.0);
} }
Matrix Dbetween2(const Pose2& p0, const Pose2& p2) { Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
Matrix db_dt2 = p0.r().transpose();
return Matrix_(3,3, return Matrix_(3,3,
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,

120
cpp/Rot2.cpp
View File

@ -12,77 +12,77 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
template class Lie<Rot2>; template class Lie<Rot2> ;
/* ************************************************************************* */ /* ************************************************************************* */
void Rot2::print(const string& s) const { void Rot2::print(const string& s) const {
cout << s << ":" << theta() << endl; cout << s << ":" << theta() << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Rot2::equals(const Rot2& R, double tol) const { bool Rot2::equals(const Rot2& R, double tol) const {
return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol; return fabs(c_ - R.c_) <= tol && fabs(s_ - R.s_) <= tol;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Rot2::matrix() const { Matrix Rot2::matrix() const {
return Matrix_(2, 2, c_, -s_, s_, c_); return Matrix_(2, 2, c_, -s_, s_, c_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Rot2::transpose() const { Matrix Rot2::transpose() const {
return Matrix_(2, 2, c_, s_, -s_, c_); return Matrix_(2, 2, c_, s_, -s_, c_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Rot2::negtranspose() const { Matrix Rot2::negtranspose() const {
return Matrix_(2, 2, -c_, -s_, s_, -c_); return Matrix_(2, 2, -c_, -s_, s_, -c_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Rot2::unrotate(const Point2& p) const { Point2 Rot2::rotate(const Point2& p) const {
return Point2( return Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
c_ * p.x() + s_ * p.y(), }
-s_ * p.x() + c_ * p.y()
);
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 rotate(const Rot2& R, const Point2& p) { Point2 Rot2::unrotate(const Point2& p) const {
return Point2( return Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
R.c() * p.x() - R.s() * p.y(), }
R.s() * p.x() + R.c() * p.y());
}
/* ************************************************************************* */ /* ************************************************************************* */
// see libraries/caml/geometry/math.ml // see libraries/caml/geometry/math.ml
Matrix Drotate1(const Rot2& R, const Point2& p) { Point2 rotate(const Rot2 & R, const Point2& p,
Point2 q = R*p; boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
return Matrix_(2, 1, -q.y(), q.x()); Point2 q = R * p;
} if (H1) *H1 = Matrix_(2, 1, -q.y(), q.x());
if (H2) *H2 = R.matrix();
return q;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Drotate2(const Rot2& R) { // see libraries/caml/geometry/math.lyx, derivative of unrotate
return R.matrix(); Point2 unrotate(const Rot2 & R, const Point2& p,
} boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) {
Point2 q = R.unrotate(p);
if (H1) *H1 = Matrix_(2, 1, q.y(), -q.x());
if (H2) *H2 = R.transpose();
return q;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 unrotate(const Rot2& R, const Point2& p) { Rot2 relativeBearing(const Point2& d) {
return R.unrotate(p); double n = d.norm();
} return Rot2(d.x() / n, d.y() / n);
}
/* ************************************************************************* */ /* ************************************************************************* */
/** see libraries/caml/geometry/math.lyx, derivative of unrotate */ Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
/* ************************************************************************* */ double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
Matrix Dunrotate1(const Rot2 & R, const Point2 & p) { if (H) *H = Matrix_(1, 2, -y / d2, x / d2);
Point2 q = R.unrotate(p); return Rot2(x / n, y / n);
return Matrix_(2, 1, q.y(), -q.x()); }
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Dunrotate2(const Rot2 & R) {
return R.transpose();
}
} // gtsam } // gtsam

34
cpp/Rot2.h
View File

@ -8,6 +8,7 @@
#ifndef ROT2_H_ #ifndef ROT2_H_
#define ROT2_H_ #define ROT2_H_
#include <boost/optional.hpp>
#include "Testable.h" #include "Testable.h"
#include "Point2.h" #include "Point2.h"
#include "Matrix.h" #include "Matrix.h"
@ -58,12 +59,12 @@ namespace gtsam {
/** return 2*2 negative transpose */ /** return 2*2 negative transpose */
Matrix negtranspose() const; Matrix negtranspose() const;
/** rotate from world to rotated = R*p */
Point2 rotate(const Point2& p) const;
/** rotate from world to rotated = R'*p */ /** rotate from world to rotated = R'*p */
Point2 unrotate(const Point2& p) const; Point2 unrotate(const Point2& p) const;
/** friends */
friend Matrix Dunrotate1(const Rot2& R, const Point2& p);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
@ -122,20 +123,29 @@ namespace gtsam {
* rotate point from rotated coordinate frame to * rotate point from rotated coordinate frame to
* world = R*p * world = R*p
*/ */
Point2 rotate(const Rot2& R, const Point2& p); inline Point2 operator*(const Rot2& R, const Point2& p) {return R.rotate(p);}
inline Point2 operator*(const Rot2& R, const Point2& p) { Point2 rotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
return rotate(R,p); boost::none, boost::optional<Matrix&> H2 = boost::none);
}
Matrix Drotate1(const Rot2& R, const Point2& p);
Matrix Drotate2(const Rot2& R); // does not depend on p !
/** /**
* rotate point from world to rotated * rotate point from world to rotated
* frame = R'*p * frame = R'*p
*/ */
Point2 unrotate(const Rot2& R, const Point2& p); Point2 unrotate(const Rot2 & R, const Point2& p, boost::optional<Matrix&> H1 =
Matrix Dunrotate1(const Rot2& R, const Point2& p); boost::none, boost::optional<Matrix&> H2 = boost::none);
Matrix Dunrotate2(const Rot2& R); // does not depend on p !
/**
* Calculate relative bearing to a landmark in local coordinate frame
* @param point 2D location of landmark
* @param H optional reference for Jacobian
* @return 2D rotation \in SO(2)
*/
Rot2 relativeBearing(const Point2& d);
/**
* Calculate relative bearing and optional derivative
*/
Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H);
} // gtsam } // gtsam

62
cpp/testBearingRange.cpp Normal file
View File

@ -0,0 +1,62 @@
/**
* @file testBearingRange.cpp
* @authors Frank Dellaert
**/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/BearingFactor.h>
#include <gtsam/RangeFactor.h>
#include <gtsam/TupleConfig.h>
#include <gtsam/FactorGraph-inl.h>
#include <gtsam/NonlinearFactorGraph-inl.h>
using namespace std;
using namespace gtsam;
// typedefs
typedef Symbol<Pose2, 'x'> PoseKey;
typedef Symbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
typedef BearingFactor<Config, PoseKey, PointKey> BearingMeasurement;
typedef RangeFactor<Config, PoseKey, PointKey> RangeMeasurement;
typedef NonlinearFactorGraph<Config> Graph;
// some shared test values
Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
TEST( BearingRange, constructor )
{
// create config
Config c;
c.insert(2, x2);
c.insert(3, l3);
// create graph
Graph G;
// Create bearing factor
Rot2 z1(M_PI_4 + 0.1); // h(x) - z = -0.1
double sigma1 = 0.1;
Graph::sharedFactor factor1(new BearingMeasurement(z1, sigma1, 2, 3));
CHECK(assert_equal(Vector_(1,-0.1),factor1->error_vector(c)));
G.push_back(factor1);
// Create range factor
double z2(sqrt(2) - 0.22); // h(x) - z = 0.22
double sigma2 = 0.1;
Graph::sharedFactor factor2(new RangeMeasurement(z2, sigma2, 2, 3));
CHECK(assert_equal(Vector_(1,0.22),factor2->error_vector(c)));
G.push_back(factor2);
CHECK(assert_equal(Vector_(2,-0.1,0.22),G.error_vector(c)));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

71
cpp/testRot2.cpp
View File

@ -28,20 +28,20 @@ TEST( Rot2, transpose)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot2, negtranspose) TEST( Rot2, negtranspose)
{ {
CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose())); CHECK(assert_equal(-inverse(R).matrix(),R.negtranspose()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot2, compose) TEST( Rot2, compose)
{ {
CHECK(assert_equal(Rot2(0.45), Rot2(0.2)*Rot2(0.25))); CHECK(assert_equal(Rot2(0.45), Rot2(0.2)*Rot2(0.25)));
CHECK(assert_equal(Rot2(0.45), Rot2(0.25)*Rot2(0.2))); CHECK(assert_equal(Rot2(0.45), Rot2(0.25)*Rot2(0.2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot2, invcompose) TEST( Rot2, invcompose)
{ {
CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45)))); CHECK(assert_equal(Rot2(0.2), invcompose(Rot2(0.25),Rot2(0.45))));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -62,54 +62,39 @@ TEST( Rot2, expmap)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot2, logmap) TEST(Rot2, logmap)
{ {
Rot2 rot0(M_PI_2); Rot2 rot0(M_PI_2);
Rot2 rot(M_PI); Rot2 rot(M_PI);
Vector expected = Vector_(1, M_PI_2); Vector expected = Vector_(1, M_PI_2);
Vector actual = logmap(rot0,rot); Vector actual = logmap(rot0, rot);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// rotate derivatives // rotate and derivatives
inline Point2 rotate_(const Rot2 & R, const Point2& p) {return R.rotate(p);}
TEST( Rot2, Drotate1) TEST( Rot2, rotate)
{ {
Matrix computed = Drotate1(R, P); Matrix H1, H2;
Matrix numerical = numericalDerivative21(rotate, R, P); Point2 actual = rotate(R, P, H1, H2);
CHECK(assert_equal(numerical,computed)); CHECK(assert_equal(actual,R*P));
} Matrix numerical1 = numericalDerivative21(rotate_, R, P);
CHECK(assert_equal(numerical1,H1));
TEST( Rot2, Drotate2_DNrotate2) Matrix numerical2 = numericalDerivative22(rotate_, R, P);
{ CHECK(assert_equal(numerical2,H2));
Matrix computed = Drotate2(R);
Matrix numerical = numericalDerivative22(rotate, R, P);
CHECK(assert_equal(numerical,computed));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// unrotate // unrotate and derivatives
inline Point2 unrotate_(const Rot2 & R, const Point2& p) {return R.unrotate(p);}
TEST( Rot2, unrotate) TEST( Rot2, unrotate)
{ {
Point2 w = R * P; Matrix H1, H2;
CHECK(assert_equal(unrotate(R,w),P)); Point2 w = R * P, actual = unrotate(R, w, H1, H2);
} CHECK(assert_equal(actual,P));
Matrix numerical1 = numericalDerivative21(unrotate_, R, w);
/* ************************************************************************* */ CHECK(assert_equal(numerical1,H1));
// unrotate derivatives Matrix numerical2 = numericalDerivative22(unrotate_, R, w);
CHECK(assert_equal(numerical2,H2));
TEST( Rot2, Dunrotate1)
{
Matrix computed = Dunrotate1(R, P);
Matrix numerical = numericalDerivative21(unrotate, R, P);
CHECK(assert_equal(numerical,computed));
}
TEST( Rot2, Dunrotate2_DNunrotate2)
{
Matrix computed = Dunrotate2(R);
Matrix numerical = numericalDerivative22(unrotate, R, P);
CHECK(assert_equal(numerical,computed));
} }
/* ************************************************************************* */ /* ************************************************************************* */