Made 'between' derivatives in the tangent space of the solution instead of tangent space of identity, this makes Pose2 an "origin-free" manifold.
Richard Roberts
parent
d0b757da48
commit
92b60a8543
|
|
@ -19,20 +19,6 @@ namespace gtsam {
|
|||
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2 Pose2::exmap(const Vector& v) const {
|
||||
return Pose2(r_.exmap(Vector_(1, v(2))), t_+Point2(v(0),v(1)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Pose2::log(const Pose2 &pose) const {
|
||||
return Vector_(3,
|
||||
pose.t().x() - t().x(),
|
||||
pose.t().y() - t().y(),
|
||||
pose.r().theta() - r().theta());
|
||||
// return between(*this, pose).vector();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Pose2 Pose2::rotate(double theta) const {
|
||||
// //return Pose2(t_, Rot2(theta)*r_);
|
||||
|
|
@ -46,7 +32,7 @@ namespace gtsam {
|
|||
|
||||
// TODO, have a combined function that returns both function value and derivative
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point) {
|
||||
Matrix dx_dt = pose.r().negtranspose();
|
||||
Matrix dx_dt = Matrix_(2,2, -1.0, 0.0, 0.0, -1.0);
|
||||
Matrix dx_dr = Dunrotate1(pose.r(), point-pose.t());
|
||||
return collect(2, &dx_dt, &dx_dr);
|
||||
}
|
||||
|
|
@ -63,16 +49,20 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2) {
|
||||
Matrix dbt_dp = Dtransform_to1(p1, p2.t());
|
||||
Matrix dbr_dp = Matrix_(1,3, 0.0, 0.0, -1.0);
|
||||
return stack(2, &dbt_dp, &dbr_dp);
|
||||
Matrix dt_dr = Dunrotate1(p1.r(), p2.t()-p1.t());
|
||||
Matrix dt_dt1 = -p2.r().invcompose(p1.r()).matrix();
|
||||
Matrix dt_dr1 = Dunrotate1(p2.r(), p2.t()-p1.t());
|
||||
return Matrix_(3,3,
|
||||
dt_dt1(0,0), dt_dt1(0,1), dt_dr1(0,0),
|
||||
dt_dt1(1,0), dt_dt1(1,1), dt_dr1(1,0),
|
||||
0.0, 0.0, -1.0);
|
||||
}
|
||||
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2) {
|
||||
Matrix db_dt2 = p1.r().transpose();
|
||||
return Matrix_(3,3,
|
||||
db_dt2.data()[0], db_dt2.data()[1], 0.0,
|
||||
db_dt2.data()[2], db_dt2.data()[3], 0.0,
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0);
|
||||
}
|
||||
|
||||
|
|
|
|||
49
cpp/Pose2.h
49
cpp/Pose2.h
|
|
@ -16,6 +16,21 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Return point coordinates in pose coordinate frame
|
||||
*/
|
||||
class Pose2;
|
||||
Point2 transform_to(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
*/
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
|
||||
|
||||
/**
|
||||
* A 2D pose (Point2,Rot2)
|
||||
*/
|
||||
|
|
@ -70,48 +85,26 @@ namespace gtsam {
|
|||
size_t dim() const { return 3; }
|
||||
|
||||
/** exponential map */
|
||||
Pose2 exmap(const Vector& v) const;
|
||||
Pose2 exmap(const Vector& v) const { return Pose2(v[0], v[1], v[2]) * (*this); }
|
||||
|
||||
/** log map */
|
||||
Vector log(const Pose2 &pose) const;
|
||||
Vector log(const Pose2 &pose) const { return between(*this, pose).vector(); }
|
||||
|
||||
/** rotate pose by theta */
|
||||
// Pose2 rotate(double theta) const;
|
||||
|
||||
/** inverse transformation */
|
||||
Pose2 inverse() const {
|
||||
return Pose2(r_, r_.unrotate(t_));
|
||||
}
|
||||
Pose2 inverse() const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); }
|
||||
|
||||
/** compose this transformation onto another (pre-multiply this*p1) */
|
||||
Pose2 compose(const Pose2& p1) const {
|
||||
return Pose2(p1.r_*r_, p1.r_*t_+p1.t_);
|
||||
}
|
||||
Pose2 compose(const Pose2& p1) const { return Pose2(p1.r_ * r_, p1.r_ * t_ + p1.t_); }
|
||||
|
||||
/** same as compose (pre-multiply this*p1) */
|
||||
Pose2 operator*(const Pose2& p1) const {
|
||||
return compose(p1);
|
||||
}
|
||||
Pose2 operator*(const Pose2& p1) const { return compose(p1); }
|
||||
|
||||
/** Return point coordinates in pose coordinate frame, same as transform_to */
|
||||
Point2 operator*(const Point2& point) const {
|
||||
return r_.unrotate(point-t_);
|
||||
}
|
||||
Point2 operator*(const Point2& point) const { return r_.unrotate(point-t_); }
|
||||
|
||||
}; // Pose2
|
||||
|
||||
/**
|
||||
* Return point coordinates in pose coordinate frame
|
||||
*/
|
||||
Point2 transform_to(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to1(const Pose2& pose, const Point2& point);
|
||||
Matrix Dtransform_to2(const Pose2& pose, const Point2& point);
|
||||
|
||||
/**
|
||||
* Return relative pose between p1 and p2, in p1 coordinate frame
|
||||
*/
|
||||
Pose2 between(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween1(const Pose2& p1, const Pose2& p2);
|
||||
Matrix Dbetween2(const Pose2& p1, const Pose2& p2);
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -24,6 +24,8 @@ private:
|
|||
std::map<std::string, Pose2> values_;
|
||||
|
||||
public:
|
||||
typedef std::map<std::string, Pose2>::const_iterator iterator;
|
||||
typedef iterator const_iterator;
|
||||
|
||||
Pose2Config() {}
|
||||
Pose2Config(const Pose2Config &config) : values_(config.values_) { }
|
||||
|
|
@ -75,5 +77,8 @@ public:
|
|||
return newConfig;
|
||||
}
|
||||
|
||||
iterator begin() const { return values_.begin(); }
|
||||
iterator end() const { return values_.end(); }
|
||||
|
||||
};
|
||||
} // namespace
|
||||
|
|
|
|||
|
|
@ -5,12 +5,33 @@
|
|||
|
||||
#include <math.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <iostream>
|
||||
#include "numericalDerivative.h"
|
||||
#include "Pose2.h"
|
||||
#include "Point2.h"
|
||||
#include "Rot2.h"
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
Matrix toManifoldDerivative(Matrix vectorDerivative, Pose2 linearizationPoint) {
|
||||
Matrix manifoldDerivative(vectorDerivative.size1(), vectorDerivative.size2());
|
||||
for(int j=0; j<vectorDerivative.size2(); j++) {
|
||||
// Find the pose implied by the vector derivative, which is usually
|
||||
// linearized about identity.
|
||||
Pose2 partialPose = Pose2().exmap(Vector_(3,
|
||||
vectorDerivative(0,j),
|
||||
vectorDerivative(1,j),
|
||||
vectorDerivative(2,j)) + linearizationPoint.vector());
|
||||
// Now convert the derivative to the tangent space of the new linearization
|
||||
// point.
|
||||
Vector manifoldPartial = linearizationPoint.log(partialPose);
|
||||
manifoldDerivative(0,j) = manifoldPartial(0);
|
||||
manifoldDerivative(1,j) = manifoldPartial(1);
|
||||
manifoldDerivative(2,j) = manifoldPartial(2);
|
||||
}
|
||||
return manifoldDerivative;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST(Pose2, print) {
|
||||
|
|
@ -29,16 +50,24 @@ TEST(Pose2, constructors) {
|
|||
/* ************************************************************************* */
|
||||
TEST(Pose2, exmap) {
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
Pose2 expected(M_PI_2+0.018, Point2(1.01, 1.985));
|
||||
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 actual = pose.exmap(Vector_(3, 0.01, -0.015, 0.018));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, exmap0) {
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 actual = Pose2().exmap(Vector_(3, 0.01, -0.015, 0.018)) * pose;
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, log) {
|
||||
Pose2 pose0(M_PI_2, Point2(1, 2));
|
||||
Pose2 pose(M_PI, Point2(1.1, 2.1));
|
||||
Vector expected = Vector_(3, 0.1, 0.1, M_PI_2);
|
||||
Pose2 pose(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Vector expected = Vector_(3, 0.01, -0.015, 0.018);
|
||||
Vector actual = pose0.log(pose);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
|
@ -52,12 +81,6 @@ TEST(Pose2, log) {
|
|||
// CHECK(assert_equal(Pose2(-s,c,0.5),p2.rotate(theta)));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//TEST(Pose2, operators) {
|
||||
// CHECK(assert_equal(Pose2(2,2,2),Pose2(1,1,1)+Pose2(1,1,1)));
|
||||
// CHECK(assert_equal(Pose2(0,0,0),Pose2(1,1,1)-Pose2(1,1,1)));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2, transform_to )
|
||||
{
|
||||
|
|
@ -66,8 +89,8 @@ TEST( Pose2, transform_to )
|
|||
|
||||
// expected
|
||||
Point2 expected(2,2);
|
||||
Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0);
|
||||
Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
|
||||
// Matrix expectedH1 = Matrix_(2,3, 0.0, -1.0, 2.0, 1.0, 0.0, -2.0);
|
||||
// Matrix expectedH2 = Matrix_(2,2, 0.0, 1.0, -1.0, 0.0);
|
||||
|
||||
// actual
|
||||
Point2 actual = transform_to(pose,point);
|
||||
|
|
@ -75,8 +98,8 @@ TEST( Pose2, transform_to )
|
|||
Matrix actualH2 = Dtransform_to2(pose,point);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH2,actualH2));
|
||||
// CHECK(assert_equal(expectedH1,actualH1));
|
||||
// CHECK(assert_equal(expectedH2,actualH2));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(transform_to, pose, point, 1e-5);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
|
@ -87,6 +110,26 @@ TEST( Pose2, transform_to )
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_a)
|
||||
{
|
||||
Pose2 pose1(M_PI/4.0, Point2(sqrt(0.5), sqrt(0.5)));
|
||||
Pose2 pose2(M_PI/2.0, Point2(0.0, 2.0));
|
||||
|
||||
Pose2 expected(3.0*M_PI/4.0, Point2(-sqrt(0.5), 3.0*sqrt(0.5)));
|
||||
Pose2 actual = pose2 * pose1;
|
||||
CHECK(assert_equal(expected, actual));
|
||||
|
||||
Point2 point(sqrt(0.5), 3.0*sqrt(0.5));
|
||||
Point2 expected_point(-1.0, -1.0);
|
||||
Point2 actual_point1 = pose2 * pose1 * point;
|
||||
Point2 actual_point2 = (pose2 * pose1) * point;
|
||||
Point2 actual_point3 = pose2 * (pose1 * point);
|
||||
CHECK(assert_equal(expected_point, actual_point1));
|
||||
CHECK(assert_equal(expected_point, actual_point2));
|
||||
CHECK(assert_equal(expected_point, actual_point3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_b)
|
||||
{
|
||||
Pose2 pose1(Rot2(M_PI/10.0), Point2(.75, .5));
|
||||
Pose2 pose2(Rot2(M_PI/4.0-M_PI/10.0), Point2(0.701289620636, 1.34933052585));
|
||||
|
|
@ -101,7 +144,7 @@ TEST(Pose2, compose_a)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2, compose_b)
|
||||
TEST(Pose2, compose_c)
|
||||
{
|
||||
Pose2 pose1(Rot2(M_PI/4.0), Point2(1.0, 1.0));
|
||||
Pose2 pose2(Rot2(M_PI/4.0), Point2(sqrt(.5), sqrt(.5)));
|
||||
|
|
@ -125,13 +168,13 @@ TEST( Pose2, between )
|
|||
// expected
|
||||
Pose2 expected(M_PI_2, Point2(2,2));
|
||||
Matrix expectedH1 = Matrix_(3,3,
|
||||
0.0,-1.0,2.0,
|
||||
0.0,-1.0,-2.0,
|
||||
1.0, 0.0,-2.0,
|
||||
0.0, 0.0,-1.0
|
||||
);
|
||||
Matrix expectedH2 = Matrix_(3,3,
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
-1.0,0.0,0.0,
|
||||
0.0, 0.0, 1.0
|
||||
);
|
||||
|
||||
|
|
@ -140,14 +183,16 @@ TEST( Pose2, between )
|
|||
Matrix actualH1 = Dbetween1(p1,p2);
|
||||
Matrix actualH2 = Dbetween2(p1,p2);
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
|
||||
numericalH1 = toManifoldDerivative(numericalH1, between(p1,p2));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
|
||||
numericalH2 = toManifoldDerivative(numericalH2, between(p1,p2));
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
CHECK(assert_equal(expectedH1,actualH1));
|
||||
CHECK(assert_equal(expectedH2,actualH2));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(between, p1, p2, 1e-5);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(between, p1, p2, 1e-5);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -8,12 +8,15 @@
|
|||
#include <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <Boost/shared_ptr.hpp>
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "Pose2Factor.h"
|
||||
#include "Pose2Graph.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
using namespace boost;
|
||||
|
||||
TEST( Pose2Factor, constructor )
|
||||
{
|
||||
|
|
@ -32,22 +35,19 @@ TEST( Pose2Factor, constructor )
|
|||
Pose2Config config;
|
||||
config.insert("p1",p1);
|
||||
config.insert("p2",p2);
|
||||
//Pose2 pose1;
|
||||
//pose1=config.get("p1");
|
||||
//pose1.print("pose1");
|
||||
|
||||
// Linearize
|
||||
boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
|
||||
|
||||
// expected
|
||||
Matrix expectedH1 = Matrix_(3,3,
|
||||
0.0,-1.0,2.1,
|
||||
0.0,-1.0,-2.1,
|
||||
1.0, 0.0,-2.1,
|
||||
0.0, 0.0,-1.0
|
||||
);
|
||||
Matrix expectedH2 = Matrix_(3,3,
|
||||
1.0, 0.0, 0.0,
|
||||
0.0, 1.0, 0.0,
|
||||
-1.0,0.0,0.0,
|
||||
0.0, 0.0, 1.0
|
||||
);
|
||||
// we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!)
|
||||
|
|
@ -59,11 +59,44 @@ TEST( Pose2Factor, constructor )
|
|||
GaussianFactor expected(
|
||||
"p1", square_root_inverse_covariance*expectedH1,
|
||||
"p2", square_root_inverse_covariance*expectedH2,
|
||||
Vector_(3,0.1,0.1,0.0), 1.0);
|
||||
Vector_(3, 0.1, -0.1, 0.0), 1.0);
|
||||
|
||||
CHECK(assert_equal(expected,*actual));
|
||||
}
|
||||
|
||||
bool poseCompare(const std::string& key,
|
||||
const gtsam::Pose2Config& feasible,
|
||||
const gtsam::Pose2Config& input) {
|
||||
return feasible.get(key).equals(input.get(key));
|
||||
}
|
||||
|
||||
TEST(Pose2Factor, optimize) {
|
||||
Pose2Graph fg;
|
||||
shared_ptr<Pose2Config> config = shared_ptr<Pose2Config>(new Pose2Config());
|
||||
Pose2Config feasible;
|
||||
feasible.insert("p0", Pose2(0,0,0));
|
||||
fg.push_back(Pose2Graph::sharedFactor(
|
||||
new NonlinearEquality<Pose2Config>("p0", feasible, Pose2().dim(), poseCompare)));
|
||||
config->insert("p0", Pose2(0,0,0));
|
||||
fg.push_back(Pose2Graph::sharedFactor(
|
||||
new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3,
|
||||
0.5, 0.0, 0.0,
|
||||
0.0, 0.5, 0.0,
|
||||
0.0, 0.0, 0.5))));
|
||||
config->insert("p1", Pose2(0,0,0));
|
||||
Ordering ordering;
|
||||
ordering.push_back("p0");
|
||||
ordering.push_back("p1");
|
||||
NonlinearOptimizer<Pose2Graph, Pose2Config> optimizer(fg, ordering, config);
|
||||
optimizer = optimizer.levenbergMarquardt(1e-10, 1e-10);
|
||||
Pose2 actual0 = optimizer.config()->get("p0");
|
||||
Pose2 actual1 = optimizer.config()->get("p1");
|
||||
Pose2 expected0 = Pose2(0,0,0);
|
||||
Pose2 expected1 = Pose2(1,2,M_PI_2);
|
||||
CHECK(assert_equal(expected0, actual0));
|
||||
CHECK(assert_equal(expected0, actual0));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
|
|
@ -56,22 +56,22 @@ TEST( Pose2Graph, linerization )
|
|||
// the expected linear factor
|
||||
GaussianFactorGraph lfg_expected;
|
||||
Matrix A1 = Matrix_(3,3,
|
||||
0.0, 2.0, -4.2,
|
||||
0.0, 2.0, 4.2,
|
||||
-2.0, 0.0, 4.2,
|
||||
0.0, 0.0, 10.0);
|
||||
|
||||
Matrix A2 = Matrix_(3,3,
|
||||
-2.0, 0.0, 0.0,
|
||||
0.0,-2.0, 0.0,
|
||||
2.0, 0.0, 0.0,
|
||||
0.0, 0.0, -10.0);
|
||||
|
||||
|
||||
double sigma = 1;
|
||||
Vector b = Vector_(3,0.1,0.1,0.0);
|
||||
Vector b = Vector_(3,0.1,-0.1,0.0);
|
||||
lfg_expected.add("x1", A1, "x2", A2, b, sigma);
|
||||
|
||||
|
||||
CHECK(lfg_expected.equals(lfg_linearized));
|
||||
CHECK(assert_equal(lfg_expected, lfg_linearized));
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue