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

Alternate signs on Hat/Vee and update tests

release/4.3a0
Binit Shah 2020-05-28 05:25:33 -04:00
parent 3422c3bb66
commit 99cc2294e1
9 changed files with 128 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam.h
View File

@ -2258,8 +2258,10 @@ virtual class NonlinearOptimizerParams {
}; };
bool checkConvergence(double relativeErrorTreshold, bool checkConvergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double errorThreshold, double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError); double currentError, double newError);
bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
double currentError, double newError);
#include <gtsam/nonlinear/GaussNewtonOptimizer.h> #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
@ -2495,7 +2497,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/nonlinear/PriorFactor.h> #include <gtsam/nonlinear/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}> template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2518,7 +2520,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible); NonlinearEquality(size_t j, const T& feasible);
@ -2759,8 +2761,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr> // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
class BetweenFactorPose3s class BetweenFactorPose3s
{ {
BetweenFactorPose3s();
size_t size() const; size_t size() const;
gtsam::BetweenFactorPose3* at(size_t i) const; gtsam::BetweenFactorPose3* at(size_t i) const;
void push_back(const gtsam::BetweenFactorPose3* factor);
}; };
#include <gtsam/slam/InitializePose3.h> #include <gtsam/slam/InitializePose3.h>

4
gtsam/base/Lie.h
View File

@ -170,7 +170,7 @@ namespace internal {
/// Assumes existence of: identity, dimension, localCoordinates, retract, /// Assumes existence of: identity, dimension, localCoordinates, retract,
/// and additionally Logmap, Expmap, compose, between, and inverse /// and additionally Logmap, Expmap, compose, between, and inverse
template<class Class> template<class Class>
struct LieGroupTraits { struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
typedef lie_group_tag structure_category; typedef lie_group_tag structure_category;
/// @name Group /// @name Group
@ -186,8 +186,6 @@ struct LieGroupTraits {
typedef Eigen::Matrix<double, dimension, 1> TangentVector; typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian; typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static int GetDimension(const Class&) {return dimension;}
static TangentVector Local(const Class& origin, const Class& other, static TangentVector Local(const Class& origin, const Class& other,
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
return origin.localCoordinates(other, Horigin, Hother); return origin.localCoordinates(other, Horigin, Hother);

6
gtsam/base/Manifold.h
View File

@ -72,7 +72,7 @@ struct HasManifoldPrereqs {
/// Extra manifold traits for fixed-dimension types /// Extra manifold traits for fixed-dimension types
template<class Class, int N> template<class Class, int N>
struct ManifoldImpl { struct GetDimensionImpl {
// Compile-time dimensionality // Compile-time dimensionality
static int GetDimension(const Class&) { static int GetDimension(const Class&) {
return N; return N;
@ -81,7 +81,7 @@ struct ManifoldImpl {
/// Extra manifold traits for variable-dimension types /// Extra manifold traits for variable-dimension types
template<class Class> template<class Class>
struct ManifoldImpl<Class, Eigen::Dynamic> { struct GetDimensionImpl<Class, Eigen::Dynamic> {
// Run-time dimensionality // Run-time dimensionality
static int GetDimension(const Class& m) { static int GetDimension(const Class& m) {
return m.dim(); return m.dim();
@ -92,7 +92,7 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
/// To use this for your class type, define: /// To use this for your class type, define:
/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { }; /// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
template<class Class> template<class Class>
struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> { struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery // Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>)); BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));

8
gtsam/geometry/SO3.cpp
View File

@ -261,13 +261,13 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (std::abs(tr + 1.0) < 1e-10) { if (tr + 1.0 < 1e-10) {
if (std::abs(R33 + 1.0) > 1e-10) if (std::abs(R33 + 1.0) > 1e-5)
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33);
else if (std::abs(R22 + 1.0) > 1e-10) else if (std::abs(R22 + 1.0) > 1e-5)
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32);
else else
// if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
} else { } else {
double magnitude; double magnitude;

27
gtsam/geometry/SO4.cpp
View File

@ -33,9 +33,10 @@ using namespace std;
namespace gtsam { namespace gtsam {
// TODO(frank): is this better than SOn::Random?
// /* ************************************************************************* // /* *************************************************************************
// */ static Vector3 randomOmega(boost::mt19937 &rng) { // */ static Vector3 randomOmega(boost::mt19937 &rng) {
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI); // static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
// return Unit3::Random(rng).unitVector() * randomAngle(rng); // return Unit3::Random(rng).unitVector() * randomAngle(rng);
// } // }
@ -58,9 +59,9 @@ Matrix4 SO4::Hat(const Vector6& xi) {
Y(0, 1) = -xi(5); Y(0, 1) = -xi(5);
Y(0, 2) = +xi(4); Y(0, 2) = +xi(4);
Y(1, 2) = -xi(3); Y(1, 2) = -xi(3);
Y(0, 3) = -xi(2); Y(0, 3) = +xi(2);
Y(1, 3) = +xi(1); Y(1, 3) = -xi(1);
Y(2, 3) = -xi(0); Y(2, 3) = +xi(0);
return Y - Y.transpose(); return Y - Y.transpose();
} }
@ -72,9 +73,9 @@ Vector6 SO4::Vee(const Matrix4& X) {
xi(5) = -X(0, 1); xi(5) = -X(0, 1);
xi(4) = +X(0, 2); xi(4) = +X(0, 2);
xi(3) = -X(1, 2); xi(3) = -X(1, 2);
xi(2) = -X(0, 3); xi(2) = +X(0, 3);
xi(1) = +X(1, 3); xi(1) = -X(1, 3);
xi(0) = -X(2, 3); xi(0) = +X(2, 3);
return xi; return xi;
} }
@ -208,9 +209,9 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
if (H) { if (H) {
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2), const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
q = R.topRightCorner<3, 1>(); q = R.topRightCorner<3, 1>();
*H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, // *H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2, //
Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, // Z_3x1, q, Z_3x1, m3, Z_3x1, -m1, //
q, Z_3x1, Z_3x1, -m2, m1, Z_3x1; -q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
} }
return M; return M;
} }
@ -221,9 +222,9 @@ GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
const Matrix43 M = R.leftCols<3>(); const Matrix43 M = R.leftCols<3>();
if (H) { if (H) {
const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3); const auto &m1 = R.col(0), m2 = R.col(1), m3 = R.col(2), q = R.col(3);
*H << Z_4x1, Z_4x1, q, Z_4x1, -m3, m2, // *H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2, //
Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, // Z_4x1, q, Z_4x1, m3, Z_4x1, -m1, //
q, Z_4x1, Z_4x1, -m2, m1, Z_4x1; -q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
} }
return M; return M;
} }

9
gtsam/geometry/SOn.cpp
View File

@ -38,11 +38,12 @@ Matrix SOn::Hat(const Vector& xi) {
const size_t dmin = (n - 1) * (n - 2) / 2; const size_t dmin = (n - 1) * (n - 2) / 2;
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin)); X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
// determine sign of last element (signs alternate)
double sign = pow(-1.0, xi.size());
// Now fill last row and column // Now fill last row and column
double sign = 1.0;
for (size_t i = 0; i < n - 1; i++) { for (size_t i = 0; i < n - 1; i++) {
const size_t j = n - 2 - i; const size_t j = n - 2 - i;
X(n - 1, j) = sign * xi(i); X(n - 1, j) = -sign * xi(i);
X(j, n - 1) = -X(n - 1, j); X(j, n - 1) = -X(n - 1, j);
sign = -sign; sign = -sign;
} }
@ -67,10 +68,10 @@ Vector SOn::Vee(const Matrix& X) {
Vector xi(d); Vector xi(d);
// Fill first n-1 spots from last row of X // Fill first n-1 spots from last row of X
double sign = 1.0; double sign = pow(-1.0, xi.size());
for (size_t i = 0; i < n - 1; i++) { for (size_t i = 0; i < n - 1; i++) {
const size_t j = n - 2 - i; const size_t j = n - 2 - i;
xi(i) = sign * X(n - 1, j); xi(i) = -sign * X(n - 1, j);
sign = -sign; sign = -sign;
} }

23
gtsam/geometry/SOn.h
View File

@ -24,10 +24,11 @@
#include <Eigen/Core> #include <Eigen/Core>
#include <random> #include <iostream> // TODO(frank): how to avoid?
#include <string> #include <string>
#include <type_traits> #include <type_traits>
#include <vector> #include <vector>
#include <random>
namespace gtsam { namespace gtsam {
@ -93,6 +94,16 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
return SO(R); return SO(R);
} }
/// Named constructor from lower dimensional matrix
template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
Matrix Q = Matrix::Identity(n, n);
size_t p = R.rows();
assert(p < n && R.cols() == p);
Q.topLeftCorner(p, p) = R;
return SO(Q);
}
/// Construct dynamic SO(n) from Fixed SO<M> /// Construct dynamic SO(n) from Fixed SO<M>
template <int M, int N_ = N, typename = IsDynamic<N_>> template <int M, int N_ = N, typename = IsDynamic<N_>>
explicit SO(const SO<M>& R) : matrix_(R.matrix()) {} explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
@ -207,11 +218,11 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
* etc... For example, the vector-space isomorphic to so(5) is laid out as: * etc... For example, the vector-space isomorphic to so(5) is laid out as:
* a b c d | u v w | x y | z * a b c d | u v w | x y | z
* where the latter elements correspond to "telescoping" sub-algebras: * where the latter elements correspond to "telescoping" sub-algebras:
* 0 -z y -w d * 0 -z y w -d
* z 0 -x v -c * z 0 -x -v c
* -y x 0 -u b * -y x 0 u -b
* w -v u 0 -a * -w v -u 0 a
* -d c -b a 0 * d -c b -a 0
* This scheme behaves exactly as expected for SO(2) and SO(3). * This scheme behaves exactly as expected for SO(2) and SO(3).
*/ */
static MatrixNN Hat(const TangentVector& xi); static MatrixNN Hat(const TangentVector& xi);

88
gtsam/geometry/tests/testRot3.cpp
View File

@ -181,63 +181,70 @@ TEST( Rot3, retract)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, log) TEST(Rot3, log) {
{
static const double PI = boost::math::constants::pi<double>(); static const double PI = boost::math::constants::pi<double>();
Vector w; Vector w;
Rot3 R; Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \ #define CHECK_OMEGA(X, Y, Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ w = (Vector(3) << X, Y, Z).finished(); \
R = Rot3::Rodrigues(w); \ R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
// Check zero // Check zero
CHECK_OMEGA( 0, 0, 0) CHECK_OMEGA(0, 0, 0)
// create a random direction: // create a random direction:
double norm=sqrt(1.0+16.0+4.0); double norm = sqrt(1.0 + 16.0 + 4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm; double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
// Check very small rotation for Taylor expansion // Check very small rotation for Taylor expansion
// Note that tolerance above is 1e-12, so Taylor is pretty good ! // Note that tolerance above is 1e-12, so Taylor is pretty good !
double d = 0.0001; double d = 0.0001;
CHECK_OMEGA( d, 0, 0) CHECK_OMEGA(d, 0, 0)
CHECK_OMEGA( 0, d, 0) CHECK_OMEGA(0, d, 0)
CHECK_OMEGA( 0, 0, d) CHECK_OMEGA(0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d) CHECK_OMEGA(x * d, y * d, z * d)
// check normal rotation // check normal rotation
d = 0.1; d = 0.1;
CHECK_OMEGA( d, 0, 0) CHECK_OMEGA(d, 0, 0)
CHECK_OMEGA( 0, d, 0) CHECK_OMEGA(0, d, 0)
CHECK_OMEGA( 0, 0, d) CHECK_OMEGA(0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d) CHECK_OMEGA(x * d, y * d, z * d)
// Check 180 degree rotations // Check 180 degree rotations
CHECK_OMEGA( PI, 0, 0) CHECK_OMEGA(PI, 0, 0)
CHECK_OMEGA( 0, PI, 0) CHECK_OMEGA(0, PI, 0)
CHECK_OMEGA( 0, 0, PI) CHECK_OMEGA(0, 0, PI)
// Windows and Linux have flipped sign in quaternion mode // Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS) #if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI).finished(); w = (Vector(3) << x * PI, y * PI, z * PI).finished();
R = Rot3::Rodrigues(w); R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12)); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
#else #else
CHECK_OMEGA(x*PI,y*PI,z*PI) CHECK_OMEGA(x * PI, y * PI, z * PI)
#endif #endif
// Check 360 degree rotations // Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \ #define CHECK_OMEGA_ZERO(X, Y, Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \ w = (Vector(3) << X, Y, Z).finished(); \
R = Rot3::Rodrigues(w); \ R = Rot3::Rodrigues(w); \
EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R))); EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) CHECK_OMEGA_ZERO(2.0 * PI, 0, 0)
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) CHECK_OMEGA_ZERO(0, 2.0 * PI, 0)
CHECK_OMEGA_ZERO( 0, 0, 2.0*PI) CHECK_OMEGA_ZERO(0, 0, 2.0 * PI)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) CHECK_OMEGA_ZERO(x * 2. * PI, y * 2. * PI, z * 2. * PI)
// Check problematic case from Lund dataset vercingetorix.g2o
// This is an almost rotation with determinant not *quite* 1.
Rot3 Rlund(-0.98582676, -0.03958746, -0.16303092, //
-0.03997006, -0.88835923, 0.45740671, //
-0.16293753, 0.45743998, 0.87418537);
EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
(Vector)Rot3::Logmap(Rlund), 1e-8));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -536,16 +543,15 @@ TEST( Rot3, logmapStability ) {
TEST(Rot3, quaternion) { TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion // NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
0.271018623057411, 0.278786459830371, 0.921318086098018, 0.578529366719085, 0.717799701969298, -0.387385285854279,
0.578529366719085, 0.717799701969298, -0.387385285854279, -0.769319620053772, 0.637998195662053, 0.033250932803219);
-0.769319620053772, 0.637998195662053, 0.033250932803219).finished());
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << 0.599136268678053);
-0.207341903877828, 0.250149415542075, 0.945745528564780, Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290, 0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946).finished()); 0.424630407073532, 0.893945571198514, -0.143353873763946);
// Check creating Rot3 from quaternion // Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1))); EXPECT(assert_equal(R1, Rot3(q1)));

48
gtsam/geometry/tests/testSOn.cpp
View File

@ -46,7 +46,7 @@ TEST(SOn, SO0) {
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
EXPECT_LONGS_EQUAL(0, R.dim()); EXPECT_LONGS_EQUAL(0, R.dim());
EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R)); EXPECT_LONGS_EQUAL(0, traits<SOn>::GetDimension(R));
} }
//****************************************************************************** //******************************************************************************
@ -56,7 +56,7 @@ TEST(SOn, SO5) {
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim()); EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::Dim());
EXPECT_LONGS_EQUAL(10, R.dim()); EXPECT_LONGS_EQUAL(10, R.dim());
EXPECT_LONGS_EQUAL(-1, traits<SOn>::GetDimension(R)); EXPECT_LONGS_EQUAL(10, traits<SOn>::GetDimension(R));
} }
//****************************************************************************** //******************************************************************************
@ -95,32 +95,42 @@ TEST(SOn, Random) {
//****************************************************************************** //******************************************************************************
TEST(SOn, HatVee) { TEST(SOn, HatVee) {
Vector6 v; Vector10 v;
v << 1, 2, 3, 4, 5, 6; v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
Matrix expected2(2, 2); Matrix expected2(2, 2);
expected2 << 0, -1, 1, 0; expected2 << 0, -1, 1, 0;
const auto actual2 = SOn::Hat(v.head<1>()); const auto actual2 = SOn::Hat(v.head<1>());
CHECK(assert_equal(expected2, actual2)); EXPECT(assert_equal(expected2, actual2));
CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2))); EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
Matrix expected3(3, 3); Matrix expected3(3, 3);
expected3 << 0, -3, 2, // expected3 << 0, -3, 2, //
3, 0, -1, // 3, 0, -1, //
-2, 1, 0; -2, 1, 0;
const auto actual3 = SOn::Hat(v.head<3>()); const auto actual3 = SOn::Hat(v.head<3>());
CHECK(assert_equal(expected3, actual3)); EXPECT(assert_equal(expected3, actual3));
CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3)); EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3))); EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
Matrix expected4(4, 4); Matrix expected4(4, 4);
expected4 << 0, -6, 5, -3, // expected4 << 0, -6, 5, 3, //
6, 0, -4, 2, // 6, 0, -4, -2, //
-5, 4, 0, -1, // -5, 4, 0, 1, //
3, -2, 1, 0; -3, 2, -1, 0;
const auto actual4 = SOn::Hat(v); const auto actual4 = SOn::Hat(v.head<6>());
CHECK(assert_equal(expected4, actual4)); EXPECT(assert_equal(expected4, actual4));
CHECK(assert_equal((Vector)v, SOn::Vee(actual4))); EXPECT(assert_equal((Vector)v.head<6>(), SOn::Vee(actual4)));
Matrix expected5(5, 5);
expected5 << 0,-10, 9, 7, -4, //
10, 0, -8, -6, 3, //
-9, 8, 0, 5, -2, //
-7, 6, -5, 0, 1, //
4, -3, 2, -1, 0;
const auto actual5 = SOn::Hat(v);
EXPECT(assert_equal(expected5, actual5));
EXPECT(assert_equal((Vector)v, SOn::Vee(actual5)));
} }
//****************************************************************************** //******************************************************************************