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

error is a signed, vector-valued error signal

release/4.3a0
Frank Dellaert 2013-12-18 04:07:33 +00:00
parent e69fbbb925
commit e0c0cb751e
3 changed files with 41 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/geometry/Sphere2.cpp
View File

@ -80,17 +80,24 @@ Sphere2 Sphere2::Rotate(const Rot3& R, const Sphere2& p,
/* ************************************************************************* */
Sphere2 operator*(const Rot3& R, const Sphere2& p) {
return Sphere2::Rotate(R,p);
return Sphere2::Rotate(R, p);
}
/* ************************************************************************* */
double Sphere2::distance(const Sphere2& other,
boost::optional<Matrix&> H) const {
Vector Sphere2::error(const Sphere2& q, boost::optional<Matrix&> H) const {
Matrix Bt = basis().transpose();
Vector xi = Bt * other.p_.vector();
Vector xi = Bt * q.p_.vector();
if (H)
*H = Bt * q.basis();
return xi;
}
/* ************************************************************************* */
double Sphere2::distance(const Sphere2& q, boost::optional<Matrix&> H) const {
Vector xi = error(q,H);
double theta = xi.norm();
if (H)
*H = (xi.transpose() / theta) * Bt * other.basis();
*H = (xi.transpose() / theta) * (*H);
return theta;
}

6
gtsam/geometry/Sphere2.h
View File

@ -84,8 +84,12 @@ public:
/// Rotate sugar
friend Sphere2 operator*(const Rot3& R, const Sphere2& p);
/// Signed, vector-valued error between two directions
Vector error(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const;
/// Distance between two directions
double distance(const Sphere2& other,
double distance(const Sphere2& q,
boost::optional<Matrix&> H = boost::none) const;
/// @}

24
gtsam/geometry/tests/testSphere2.cpp
View File

@ -51,6 +51,30 @@ TEST(Sphere2, rotate) {
}
}
//*******************************************************************************
TEST(Sphere2, error) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //
r = p.retract((Vector(2) << 0.8, 0));
EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8));
EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5));
EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5));
Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian
{
expected = numericalDerivative11<Sphere2>(
boost::bind(&Sphere2::error, &p, _1, boost::none), q);
p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
{
expected = numericalDerivative11<Sphere2>(
boost::bind(&Sphere2::error, &p, _1, boost::none), r);
p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
}
}
//*******************************************************************************
TEST(Sphere2, distance) {
Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), //