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

RotateFactor now takes full rotations, old one that takes directions is RotateDirectionsFactor (in same header) 

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20421 898a188c-9671-0410-8e00-e3fd810bbb7f
release/4.3a0
Frank Dellaert 2013-12-21 15:06:39 +00:00 committed by Richard Roberts
parent 9636ace3bb
commit 046da246ba
2 changed files with 142 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
gtsam/slam/RotateFactor.h
View File

@ -13,10 +13,53 @@
namespace gtsam {
/**
* Factor that evaluates distance between two rotated directions
* Factor on unknown rotation iRC that relates two incremental rotations
* c1Rc2 = iRc' * i1Ri2 * iRc
* Which we can write (see doc/math.lyx)
* e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p])
* with z and p measured and predicted angular velocities, and hence
* p = iRc * z
*/
class RotateFactor: public NoiseModelFactor1<Rot3> {
Point3 p_, z_; ///< Predicted and measured directions, p = iRc * z
typedef NoiseModelFactor1<Rot3> Base;
public:
/// Constructor
RotateFactor(Key key, const Rot3& P, const Rot3& Z,
const SharedNoiseModel& model) :
Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
}
/// print
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << "RotateFactor:" << std::endl;
p_.print("p");
z_.print("z");
}
/// vector of errors returns 2D vector
Vector evaluateError(const Rot3& R,
boost::optional<Matrix&> H = boost::none) const {
// predict p_ as q = R*z_, derivative H will be filled if not none
Point3 q = R.rotate(z_,H);
// error is just difference, and note derivative of that wrpt q is I3
return Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z();
}
};
/**
* Factor on unknown rotation R that relates two directions p_i = iRc * z_c
* Directions provide less constraints than a full rotation
*/
class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
Sphere2 p_, z_; ///< Predicted and measured directions, p = iRc * z
typedef NoiseModelFactor1<Rot3> Base;
@ -24,7 +67,7 @@ class RotateFactor: public NoiseModelFactor1<Rot3> {
public:
/// Constructor
RotateFactor(Key key, const Sphere2& p, const Sphere2& z,
RotateDirectionsFactor(Key key, const Sphere2& p, const Sphere2& z,
const SharedNoiseModel& model) :
Base(model, key), p_(p), z_(z) {
}
@ -33,6 +76,7 @@ public:
virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << "RotateDirectionsFactor:" << std::endl;
p_.print("p");
z_.print("z");
}
@ -50,19 +94,6 @@ public:
return e;
}
/// Obsolete: vector of errors returns 1D vector
Vector evaluateError1(const Rot3& R,
boost::optional<Matrix&> H = boost::none) const {
Sphere2 q = R * z_;
double e = p_.distance(q, H);
if (H) {
Matrix DR;
R.rotate(z_, DR);
*H = (*H) * DR;
}
return (Vector(1) << e);
}
};
} // gtsam

109
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -26,23 +26,42 @@ using namespace gtsam;
// And camera is looking forward.
Point3 cameraX(0, 1, 0), cameraY(0, 0, 1), cameraZ(1, 0, 0);
Rot3 iRc(cameraX, cameraY, cameraZ);
// Now, let's create some rotations around IMU frame
Sphere2 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1);
Rot3 gRi1 = Rot3::Expmap(Vector3(1, 0, 0));
Rot3 i1Ri2 = Rot3::rodriguez(p1, 1), //
i2Ri3 = Rot3::rodriguez(p2, 1), //
i3Ri4 = Rot3::rodriguez(p3, 1);
// The corresponding rotations in the camera frame
Sphere2 z1 = iRc.inverse() * p1, z2 = iRc.inverse() * p2, //
Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, //
c2Zc3 = iRc.inverse() * i2Ri3 * iRc, //
c3Zc4 = iRc.inverse() * i3Ri4 * iRc;
// The corresponding rotated directions in the camera frame
Sphere2 z1 = iRc.inverse() * p1, //
z2 = iRc.inverse() * p2, //
z3 = iRc.inverse() * p3;
// noise model
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2, 0.01);
typedef noiseModel::Isotropic::shared_ptr Model;
//*************************************************************************
TEST (RotateFactor, checkMath) {
EXPECT(assert_equal(c1Zc2, Rot3::rodriguez(z1, 1)));
EXPECT(assert_equal(c2Zc3, Rot3::rodriguez(z2, 1)));
EXPECT(assert_equal(c3Zc4, Rot3::rodriguez(z3, 1)));
}
//*************************************************************************
TEST (RotateFactor, test) {
RotateFactor f(1, p1, z1, model);
Model model = noiseModel::Isotropic::Sigma(3, 0.01);
RotateFactor f(1, i1Ri2, c1Zc2, model);
EXPECT(assert_equal(zero(3), f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
EXPECT(assert_equal((Vector(2)<<0,0), f.evaluateError(iRc), 1e-8));
EXPECT(
assert_equal((Vector(2)<<-0.08867, -0.20197), f.evaluateError(R), 1e-5));
Vector expectedE = (Vector(3) << -0.0246305, 0.20197, -0.08867);
EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
@ -63,9 +82,10 @@ TEST (RotateFactor, test) {
TEST (RotateFactor, minimization) {
// Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph;
graph.add(RotateFactor(1, p1, z1, model));
graph.add(RotateFactor(1, p2, z2, model));
graph.add(RotateFactor(1, p3, z3, model));
Model model = noiseModel::Isotropic::Sigma(3, 0.01);
graph.add(RotateFactor(1, i1Ri2, c1Zc2, model));
graph.add(RotateFactor(1, i2Ri3, c2Zc3, model));
graph.add(RotateFactor(1, i3Ri4, c3Zc4, model));
// Check error at ground truth
Values truth;
@ -74,8 +94,71 @@ TEST (RotateFactor, minimization) {
// Check error at initial estimate
Values initial;
double degree = M_PI/180;
Rot3 initialE = iRc.retract(degree*(Vector(3) << 20, -20, 20));
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
EXPECT_DOUBLES_EQUAL(3349, graph.error(initial), 1);
// Optimize
LevenbergMarquardtParams parameters;
//parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
Rot3 actual = result.at<Rot3>(1);
EXPECT(assert_equal(iRc, actual,1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
TEST (RotateDirectionsFactor, test) {
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
RotateDirectionsFactor f(1, p1, z1, model);
EXPECT(assert_equal(zero(2), f.evaluateError(iRc), 1e-8));
Rot3 R = iRc.retract((Vector(3) << 0.1, 0.2, 0.1));
Vector expectedE = (Vector(2) << -0.08867, -0.20197);
EXPECT( assert_equal(expectedE, f.evaluateError(R), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), iRc);
f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
{
expected = numericalDerivative11<Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), R);
f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9));
}
}
//*************************************************************************
TEST (RotateDirectionsFactor, minimization) {
// Let's try to recover the correct iRc by minimizing
NonlinearFactorGraph graph;
Model model = noiseModel::Isotropic::Sigma(2, 0.01);
graph.add(RotateDirectionsFactor(1, p1, z1, model));
graph.add(RotateDirectionsFactor(1, p2, z2, model));
graph.add(RotateDirectionsFactor(1, p3, z3, model));
// Check error at ground truth
Values truth;
truth.insert(1, iRc);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
double degree = M_PI / 180;
Rot3 initialE = iRc.retract(degree * (Vector(3) << 20, -20, 20));
initial.insert(1, initialE);
EXPECT_DOUBLES_EQUAL(3162, graph.error(initial), 1);