Definitions of Lie::between, Pose2::compose, expmap, and derivatives were not correct. Fixed, but changes some behavior (iSAM and CitySLAM already fixed), will send email about this change.
Richard Roberts
parent
74828669b5
commit
b82d054aaa
|
|
@ -25,15 +25,15 @@ namespace gtsam {
|
|||
|
||||
// Compute l1 s.t. l2=l1*l0
|
||||
template<class T>
|
||||
inline T between(const T& l0, const T& l2) { return l2*inverse(l0); }
|
||||
inline T between(const T& l1, const T& l2) { return inverse(l1)*l2; }
|
||||
|
||||
// Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp
|
||||
template<class T>
|
||||
inline Vector logmap(const T& l0, const T& lp) { return logmap(between(l0,lp)); }
|
||||
|
||||
/* Exponential map centered at l0, s.t. exp(l0,v) = exp(v)*l0 */
|
||||
/* Exponential map centered at l0, s.t. exp(t,d) = t*exp(d) */
|
||||
template<class T>
|
||||
inline T expmap(const T& l0, const Vector& v) { return expmap<T>(v)*l0; }
|
||||
inline T expmap(const T& t, const Vector& d) { return t * expmap<T>(d); }
|
||||
|
||||
/**
|
||||
* Base class for Lie group type
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ namespace gtsam {
|
|||
pose.r().unrotate(Point2(-pose.t().x(), -pose.t().y()))); }
|
||||
|
||||
/** compose this transformation onto another (pre-multiply this*p1) */
|
||||
inline Pose2 compose(const Pose2& p1, const Pose2& p0) {
|
||||
inline Pose2 compose(const Pose2& p0, const Pose2& p1) {
|
||||
return Pose2(p0.r()*p1.r(), p0.t() + p0.r()*p1.t()); }
|
||||
|
||||
/** exponential and log maps around identity */
|
||||
|
|
|
|||
|
|
@ -122,9 +122,8 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Matrix Dtransform_to1(const Pose3& pose, const Point3& p) {
|
||||
Point3 q = transform_to(pose,p);
|
||||
Matrix Rt = pose.rotation().transpose();
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()) * Rt;
|
||||
Matrix DT = - Rt; // negative because of sub
|
||||
Matrix DR = skewSymmetric(q.x(), q.y(), q.z());
|
||||
Matrix DT = - pose.rotation().transpose(); // negative because of sub
|
||||
return collect(2,&DR,&DT);
|
||||
}
|
||||
|
||||
|
|
@ -137,7 +136,7 @@ namespace gtsam {
|
|||
// compose = Pose3(compose(R1,R2),transform_from(p1,t2);
|
||||
|
||||
Matrix Dcompose1(const Pose3& p1, const Pose3& p2) {
|
||||
static const Matrix DR_R1 = eye(3);
|
||||
Matrix DR_R1 = p2.rotation().transpose();
|
||||
Matrix DR_t1 = zeros(3, 3);
|
||||
Matrix DR = collect(2, &DR_R1, &DR_t1);
|
||||
Matrix Dt = Dtransform_from1(p1, p2.translation());
|
||||
|
|
@ -145,8 +144,10 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
Matrix Dcompose2(const Pose3& p1, const Pose3& p2) {
|
||||
Matrix R1 = p1.rotation().matrix(), Z3 = zeros(3, 3);
|
||||
Matrix DR = collect(2, &R1, &Z3);
|
||||
Matrix R1 = p1.rotation().matrix();
|
||||
const static Matrix I = eye(3,3);
|
||||
const static Matrix Z3 = zeros(3, 3);
|
||||
Matrix DR = collect(2, &I, &Z3);
|
||||
Matrix Dt = collect(2, &Z3, &R1);
|
||||
return stack(2, &DR, &Dt);
|
||||
}
|
||||
|
|
@ -155,10 +156,10 @@ namespace gtsam {
|
|||
// inverse = Pose3(inverse(R),-unrotate(R,t));
|
||||
Matrix Dinverse(const Pose3& p) {
|
||||
Matrix Rt = p.rotation().transpose();
|
||||
Matrix DR_R1 = -Rt;
|
||||
Matrix DR_R1 = -p.rotation().matrix();
|
||||
Matrix DR_t1 = zeros(3, 3);
|
||||
Matrix DR = collect(2, &DR_R1, &DR_t1);
|
||||
Matrix Dt_R1 = -(skewSymmetric(Rt * p.translation().vector()) * Rt);
|
||||
Matrix Dt_R1 = -skewSymmetric(unrotate(p.rotation(),p.translation()).vector());
|
||||
Matrix Dt_t1 = -Rt;
|
||||
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
|
||||
return stack(2, &DR, &Dt);
|
||||
|
|
@ -169,9 +170,9 @@ namespace gtsam {
|
|||
Pose3 between(const Pose3& p1, const Pose3& p2, boost::optional<Matrix&> H1,
|
||||
boost::optional<Matrix&> H2) {
|
||||
Pose3 invp1 = inverse(p1);
|
||||
Pose3 result = compose(p2, invp1);
|
||||
if (H1) *H1 = Dcompose2(p2, invp1) * Dinverse(p1);
|
||||
if (H2) *H2 = Dcompose1(p2, invp1);
|
||||
Pose3 result = compose(invp1, p2);
|
||||
if (H1) *H1 = Dcompose1(invp1, p2) * Dinverse(p1);
|
||||
if (H2) *H2 = Dcompose2(invp1, p2);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
|
|||
13
cpp/Rot3.cpp
13
cpp/Rot3.cpp
|
|
@ -171,8 +171,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix Drotate1(const Rot3& R, const Point3& p) {
|
||||
Point3 q = R * p;
|
||||
return skewSymmetric(-q.x(), -q.y(), -q.z());
|
||||
return R.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -194,7 +193,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
Matrix Dunrotate1(const Rot3 & R, const Point3 & p) {
|
||||
Point3 q = unrotate(R,p);
|
||||
return skewSymmetric(q.x(), q.y(), q.z()) * R.transpose();
|
||||
return skewSymmetric(q.x(), q.y(), q.z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -204,22 +203,22 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix Dcompose1(const Rot3& R1, const Rot3& R2){
|
||||
return eye(3);
|
||||
return R2.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Dcompose2(const Rot3& R1, const Rot3& R2){
|
||||
return R1.matrix();
|
||||
return eye(3);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Dbetween1(const Rot3& R1, const Rot3& R2){
|
||||
return -between(R1,R2).matrix();
|
||||
return -(R2.transpose()*R1.matrix());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Dbetween2(const Rot3& R1, const Rot3& R2){
|
||||
return eye(3);
|
||||
return eye(3);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -171,6 +171,9 @@ namespace gtsam {
|
|||
R.r3().x(), R.r3().y(), R.r3().z());
|
||||
}
|
||||
|
||||
// and its derivative
|
||||
inline Matrix Dinverse(Rot3 R) { return -R.matrix();}
|
||||
|
||||
/**
|
||||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
|
|
|
|||
|
|
@ -51,8 +51,14 @@ pair<string, boost::optional<SharedDiagonal> > dataset(const string& dataset, c
|
|||
|
||||
/* ************************************************************************* */
|
||||
|
||||
pair<sharedPose2Graph, sharedPose2Config> load2D(
|
||||
pair<string, boost::optional<SharedDiagonal> > dataset,
|
||||
int maxID, bool addNoise, bool smart) {
|
||||
return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
|
||||
}
|
||||
|
||||
pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
|
||||
int maxID, boost::optional<SharedDiagonal> model, bool addNoise, bool smart) {
|
||||
boost::optional<SharedDiagonal> model, int maxID, bool addNoise, bool smart) {
|
||||
cout << "Will try to read " << filename << endl;
|
||||
ifstream is(filename.c_str());
|
||||
if (!is) {
|
||||
|
|
@ -108,7 +114,7 @@ pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
|
|||
if (maxID && (id1 >= maxID || id2 >= maxID)) continue;
|
||||
|
||||
|
||||
measured = inverse(Pose2(x, y, yaw));
|
||||
measured = Pose2(x, y, yaw);
|
||||
|
||||
// SharedGaussian noise = noiseModel::Gaussian::Covariance(m, smart);
|
||||
// hack use diagonal for now !
|
||||
|
|
@ -123,7 +129,7 @@ pair<sharedPose2Graph, sharedPose2Config> load2D(const string& filename,
|
|||
|
||||
// Insert vertices if pure odometry file
|
||||
if (!poses->exists(id1)) poses->insert(id1, Pose2());
|
||||
if (!poses->exists(id2)) poses->insert(id2, measured * poses->at(id1));
|
||||
if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * measured);
|
||||
|
||||
Pose2Graph::sharedFactor factor(new Pose2Factor(id1, id2,measured,*model));
|
||||
graph->push_back(factor);
|
||||
|
|
|
|||
|
|
@ -33,9 +33,12 @@ std::pair<std::string, boost::optional<gtsam::SharedDiagonal> >
|
|||
* @param smart: try to reduce complexity of covariance to cheapest model
|
||||
*/
|
||||
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Config> > load2D(
|
||||
const std::string& filename, int maxID = 0,
|
||||
std::pair<std::string, boost::optional<SharedDiagonal> > dataset,
|
||||
int maxID = 0, bool addNoise=false, bool smart=true);
|
||||
std::pair<boost::shared_ptr<gtsam::Pose2Graph>, boost::shared_ptr<gtsam::Pose2Config> > load2D(
|
||||
const std::string& filename,
|
||||
boost::optional<gtsam::SharedDiagonal> model = boost::optional<gtsam::SharedDiagonal>(),
|
||||
bool addNoise=false, bool smart=true);
|
||||
int maxID = 0, bool addNoise=false, bool smart=true);
|
||||
|
||||
/** save 2d graph */
|
||||
void save2D(const gtsam::Pose2Graph& graph, const gtsam::Pose2Config& config, const gtsam::SharedDiagonal model,
|
||||
|
|
|
|||
|
|
@ -20,14 +20,22 @@ namespace gtsam {
|
|||
namespace pose3SLAM {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Config circle(size_t n, double R) {
|
||||
Config circle(size_t n, double radius) {
|
||||
Config x;
|
||||
double theta = 0, dtheta = 2 * M_PI / n;
|
||||
// Vehicle at p0 is looking towards y axis
|
||||
Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
for (size_t i = 0; i < n; i++, theta += dtheta)
|
||||
x.insert(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),
|
||||
sin(theta), 0)));
|
||||
// We use aerospace/navlab convention, X forward, Y right, Z down
|
||||
// First pose will be at (R,0,0)
|
||||
// ^y ^ X
|
||||
// | |
|
||||
// z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer)
|
||||
// Vehicle at p0 is looking towards y axis (X-axis points towards world y)
|
||||
Rot3 gR0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
for (size_t i = 0; i < n; i++, theta += dtheta) {
|
||||
Point3 gti(radius*cos(theta), radius*sin(theta), 0);
|
||||
Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down !
|
||||
Pose3 gTi(gR0 * _0Ri, gti);
|
||||
x.insert(i, gTi);
|
||||
}
|
||||
return x;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -31,10 +31,10 @@ TEST(Pose2, manifold) {
|
|||
Pose2 origin;
|
||||
Vector d12 = logmap(t1,t2);
|
||||
CHECK(assert_equal(t2, expmap(t1,d12)));
|
||||
CHECK(assert_equal(t2, expmap(origin,d12)*t1));
|
||||
CHECK(assert_equal(t2, t1*expmap(origin,d12)));
|
||||
Vector d21 = logmap(t2,t1);
|
||||
CHECK(assert_equal(t1, expmap(t2,d21)));
|
||||
CHECK(assert_equal(t1, expmap(origin,d21)*t2));
|
||||
CHECK(assert_equal(t1, t2*expmap(origin,d21)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -51,7 +51,7 @@ TEST(Pose2, expmap0) {
|
|||
//cout << "expmap0" << endl;
|
||||
Pose2 pose(M_PI_2, Point2(1, 2));
|
||||
Pose2 expected(M_PI_2+0.018, Point2(1.015, 2.01));
|
||||
Pose2 actual = expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.018)) * pose;
|
||||
Pose2 actual = pose * expmap<Pose2>(Vector_(3, 0.01, -0.015, 0.018));
|
||||
CHECK(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
|
|
@ -105,12 +105,12 @@ TEST(Pose2, compose_a)
|
|||
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;
|
||||
Pose2 actual = pose1 * pose2;
|
||||
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 = transform_to(pose2 * pose1, point);
|
||||
Point2 actual_point1 = transform_to(pose1 * pose2, point);
|
||||
Point2 actual_point2 = transform_to(pose2, transform_to(pose1, point));
|
||||
CHECK(assert_equal(expected_point, actual_point1));
|
||||
CHECK(assert_equal(expected_point, actual_point2));
|
||||
|
|
@ -125,8 +125,8 @@ TEST(Pose2, compose_b)
|
|||
|
||||
Pose2 pose_expected(Rot2(M_PI/4.0), Point2(1.0, 2.0));
|
||||
|
||||
Pose2 pose_actual_op = pose2 * pose1;
|
||||
Pose2 pose_actual_fcn = compose(pose2,pose1);
|
||||
Pose2 pose_actual_op = pose1 * pose2;
|
||||
Pose2 pose_actual_fcn = compose(pose1, pose2);
|
||||
|
||||
CHECK(assert_equal(pose_expected, pose_actual_op));
|
||||
CHECK(assert_equal(pose_expected, pose_actual_fcn));
|
||||
|
|
@ -141,8 +141,8 @@ TEST(Pose2, compose_c)
|
|||
|
||||
Pose2 pose_expected(Rot2(M_PI/2.0), Point2(1.0, 2.0));
|
||||
|
||||
Pose2 pose_actual_op = pose2 * pose1;
|
||||
Pose2 pose_actual_fcn = compose(pose2,pose1);
|
||||
Pose2 pose_actual_op = pose1 * pose2;
|
||||
Pose2 pose_actual_fcn = compose(pose1,pose2);
|
||||
|
||||
CHECK(assert_equal(pose_expected, pose_actual_op));
|
||||
CHECK(assert_equal(pose_expected, pose_actual_fcn));
|
||||
|
|
@ -215,8 +215,7 @@ TEST( Pose2, compose_matrix )
|
|||
Pose2 gT1(M_PI_2, Point2(1,2)); // robot at (1,2) looking towards y
|
||||
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
|
||||
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
|
||||
CHECK(assert_equal(gM1*_1M2,matrix(compose(_1T2,gT1)))); // WRONG CHECKS OUT !
|
||||
// CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); RIGHT DOES NOT
|
||||
CHECK(assert_equal(gM1*_1M2,matrix(compose(gT1,_1T2)))); // RIGHT DOES NOT
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -276,7 +275,7 @@ TEST( Pose2, round_trip )
|
|||
{
|
||||
Pose2 p1(1.23, 2.30, 0.2);
|
||||
Pose2 odo(0.53, 0.39, 0.15);
|
||||
Pose2 p2 = compose(odo, p1);
|
||||
Pose2 p2 = compose(p1, odo);
|
||||
CHECK(assert_equal(odo, between(p1, p2)));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -245,29 +245,6 @@ TEST( Pose3, transformPose_to)
|
|||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3, composeTransform )
|
||||
{
|
||||
// known transform
|
||||
Rot3 R1 = rodriguez(0, 0, -1.570796);
|
||||
Pose3 expected(R1, Point3(1, 2, 3));
|
||||
|
||||
// current
|
||||
Rot3 R2 = rodriguez(0, 0, 0.698131701);
|
||||
Pose3 current(R2, Point3(21., 32., 13.));
|
||||
|
||||
// target
|
||||
Pose3 target(rodriguez(0, 0, 2.26892803), Point3(-30., 20., 10.));
|
||||
|
||||
// calculate transform
|
||||
// todo: which should this be?
|
||||
//Pose3 actual = compose(current, target);
|
||||
Pose3 actual = between<Pose3> (target, current);
|
||||
|
||||
//verify
|
||||
CHECK(assert_equal(expected, actual, 0.001));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose3, manifold)
|
||||
{
|
||||
|
|
@ -311,7 +288,7 @@ TEST( Pose3, between )
|
|||
Point3 t(3.5,-8.2,4.2);
|
||||
Pose3 T(R,t);
|
||||
|
||||
Pose3 expected = pose1 * inverse(T);
|
||||
Pose3 expected = inverse(T) * pose1;
|
||||
Matrix actualH1,actualH2;
|
||||
Pose3 actual = between(T, pose1, actualH1,actualH2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
|
|
|||
|
|
@ -31,23 +31,26 @@ static Matrix covariance = eye(6);
|
|||
TEST(Pose3Graph, optimizeCircle) {
|
||||
|
||||
// Create a hexagon of poses
|
||||
Pose3Config hexagon = pose3SLAM::circle(6,1.0);
|
||||
Pose3 p0 = hexagon[0], p1 = hexagon[1];
|
||||
double radius = 10;
|
||||
Pose3Config hexagon = pose3SLAM::circle(6,radius);
|
||||
Pose3 gT0 = hexagon[0], gT1 = hexagon[1];
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
shared_ptr<Pose3Graph> fg(new Pose3Graph);
|
||||
fg->addHardConstraint(0, p0);
|
||||
Pose3 delta = between(p0,p1);
|
||||
fg->addConstraint(0,1, delta, covariance);
|
||||
fg->addConstraint(1,2, delta, covariance);
|
||||
fg->addConstraint(2,3, delta, covariance);
|
||||
fg->addConstraint(3,4, delta, covariance);
|
||||
fg->addConstraint(4,5, delta, covariance);
|
||||
fg->addConstraint(5,0, delta, covariance);
|
||||
fg->addHardConstraint(0, gT0);
|
||||
Pose3 _0T1 = between(gT0,gT1); // inv(gT0)*gT1
|
||||
double theta = M_PI/3.0;
|
||||
CHECK(assert_equal(Pose3(Rot3::yaw(-theta),Point3(radius*sin(theta),-radius*cos(theta),0)),_0T1));
|
||||
fg->addConstraint(0,1, _0T1, covariance);
|
||||
fg->addConstraint(1,2, _0T1, covariance);
|
||||
fg->addConstraint(2,3, _0T1, covariance);
|
||||
fg->addConstraint(3,4, _0T1, covariance);
|
||||
fg->addConstraint(4,5, _0T1, covariance);
|
||||
fg->addConstraint(5,0, _0T1, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose3Config> initial(new Pose3Config());
|
||||
initial->insert(0, p0);
|
||||
initial->insert(0, gT0);
|
||||
initial->insert(1, expmap(hexagon[1],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial->insert(2, expmap(hexagon[2],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial->insert(3, expmap(hexagon[3],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
|
|
@ -67,10 +70,10 @@ TEST(Pose3Graph, optimizeCircle) {
|
|||
Pose3Config actual = *optimizer.config();
|
||||
|
||||
// Check with ground truth
|
||||
CHECK(assert_equal(hexagon, actual,1e-5));
|
||||
CHECK(assert_equal(hexagon, actual,1e-4));
|
||||
|
||||
// Check loop closure
|
||||
CHECK(assert_equal(delta,between(actual[5],actual[0]),1e-5));
|
||||
CHECK(assert_equal(_0T1,between(actual[5],actual[0]),1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -143,17 +143,17 @@ TEST(Rot3, log)
|
|||
/* ************************************************************************* */
|
||||
TEST(Rot3, manifold)
|
||||
{
|
||||
Rot3 t1 = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 t2 = rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 gR1 = rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = logmap(t1, t2);
|
||||
CHECK(assert_equal(t2, expmap(t1,d12)));
|
||||
CHECK(assert_equal(t2, expmap<Rot3>(d12)*t1));
|
||||
Vector d21 = logmap(t2, t1);
|
||||
CHECK(assert_equal(t1, expmap(t2,d21)));
|
||||
CHECK(assert_equal(t1, expmap<Rot3>(d21)*t2));
|
||||
Vector d12 = logmap(gR1, gR2);
|
||||
CHECK(assert_equal(gR2, expmap(gR1,d12)));
|
||||
CHECK(assert_equal(gR2, gR1*expmap<Rot3>(d12)));
|
||||
Vector d21 = logmap(gR2, gR1);
|
||||
CHECK(assert_equal(gR1, expmap(gR2,d21)));
|
||||
CHECK(assert_equal(gR1, gR2*expmap<Rot3>(d21)));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
CHECK(assert_equal(d12,-d21));
|
||||
|
|
@ -237,6 +237,21 @@ TEST( Rot3, compose )
|
|||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
TEST( Rot3, inverse )
|
||||
{
|
||||
Rot3 R = rodriguez(0.1, 0.2, 0.3);
|
||||
|
||||
Rot3 I;
|
||||
CHECK(assert_equal(I,R*inverse(R)));
|
||||
CHECK(assert_equal(I,inverse(R)*R));
|
||||
|
||||
Matrix numericalH = numericalDerivative11<Rot3,Rot3>(inverse, R, 1e-5);
|
||||
Matrix actualH = Dinverse(R);
|
||||
CHECK(assert_equal(numericalH,actualH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3, between )
|
||||
{
|
||||
|
|
@ -248,7 +263,7 @@ TEST( Rot3, between )
|
|||
Rot3 R1 = rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3 expected = R2*inverse(R1);
|
||||
Rot3 expected = inverse(R1)*R2;
|
||||
Rot3 actual = between(R1, R2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue