Merge pull request #325 from borglab/feature/alternate_hatvee_signs
Alternate signs on Hat/Vee and update testsrelease/4.3a0
Akshay Krishnan
GitHub
commit
8ce40a3584
12
gtsam.h
12
gtsam.h
|
|
@ -2292,8 +2292,10 @@ virtual class NonlinearOptimizerParams {
|
|||
};
|
||||
|
||||
bool checkConvergence(double relativeErrorTreshold,
|
||||
double absoluteErrorTreshold, double errorThreshold,
|
||||
double currentError, double newError);
|
||||
double absoluteErrorTreshold, double errorThreshold,
|
||||
double currentError, double newError);
|
||||
bool checkConvergence(const gtsam::NonlinearOptimizerParams& params,
|
||||
double currentError, double newError);
|
||||
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
|
||||
|
|
@ -2529,7 +2531,7 @@ class NonlinearISAM {
|
|||
#include <gtsam/geometry/StereoPoint2.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 {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
T prior() const;
|
||||
|
|
@ -2552,7 +2554,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
|||
|
||||
|
||||
#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 {
|
||||
// Constructor - forces exact evaluation
|
||||
NonlinearEquality(size_t j, const T& feasible);
|
||||
|
|
@ -2793,8 +2795,10 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
|
|||
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
|
||||
class BetweenFactorPose3s
|
||||
{
|
||||
BetweenFactorPose3s();
|
||||
size_t size() const;
|
||||
gtsam::BetweenFactorPose3* at(size_t i) const;
|
||||
void push_back(const gtsam::BetweenFactorPose3* factor);
|
||||
};
|
||||
|
||||
#include <gtsam/slam/InitializePose3.h>
|
||||
|
|
|
|||
|
|
@ -170,7 +170,7 @@ namespace internal {
|
|||
/// Assumes existence of: identity, dimension, localCoordinates, retract,
|
||||
/// and additionally Logmap, Expmap, compose, between, and inverse
|
||||
template<class Class>
|
||||
struct LieGroupTraits {
|
||||
struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
|
||||
typedef lie_group_tag structure_category;
|
||||
|
||||
/// @name Group
|
||||
|
|
@ -186,8 +186,6 @@ struct LieGroupTraits {
|
|||
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
|
||||
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
|
||||
|
||||
static int GetDimension(const Class&) {return dimension;}
|
||||
|
||||
static TangentVector Local(const Class& origin, const Class& other,
|
||||
ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
|
||||
return origin.localCoordinates(other, Horigin, Hother);
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ struct HasManifoldPrereqs {
|
|||
|
||||
/// Extra manifold traits for fixed-dimension types
|
||||
template<class Class, int N>
|
||||
struct ManifoldImpl {
|
||||
struct GetDimensionImpl {
|
||||
// Compile-time dimensionality
|
||||
static int GetDimension(const Class&) {
|
||||
return N;
|
||||
|
|
@ -81,7 +81,7 @@ struct ManifoldImpl {
|
|||
|
||||
/// Extra manifold traits for variable-dimension types
|
||||
template<class Class>
|
||||
struct ManifoldImpl<Class, Eigen::Dynamic> {
|
||||
struct GetDimensionImpl<Class, Eigen::Dynamic> {
|
||||
// Run-time dimensionality
|
||||
static int GetDimension(const Class& m) {
|
||||
return m.dim();
|
||||
|
|
@ -92,7 +92,7 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
|
|||
/// To use this for your class type, define:
|
||||
/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
|
||||
template<class Class>
|
||||
struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
|
||||
struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
|
||||
|
||||
// Check that Class has the necessary machinery
|
||||
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
|
||||
|
|
|
|||
|
|
@ -261,13 +261,13 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
|
|||
|
||||
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
// we do something special
|
||||
if (std::abs(tr + 1.0) < 1e-10) {
|
||||
if (std::abs(R33 + 1.0) > 1e-10)
|
||||
if (tr + 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);
|
||||
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);
|
||||
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);
|
||||
} else {
|
||||
double magnitude;
|
||||
|
|
|
|||
|
|
@ -33,9 +33,10 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// TODO(frank): is this better than SOn::Random?
|
||||
// /* *************************************************************************
|
||||
// */ 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);
|
||||
// }
|
||||
|
||||
|
|
@ -58,9 +59,9 @@ Matrix4 SO4::Hat(const Vector6& xi) {
|
|||
Y(0, 1) = -xi(5);
|
||||
Y(0, 2) = +xi(4);
|
||||
Y(1, 2) = -xi(3);
|
||||
Y(0, 3) = -xi(2);
|
||||
Y(1, 3) = +xi(1);
|
||||
Y(2, 3) = -xi(0);
|
||||
Y(0, 3) = +xi(2);
|
||||
Y(1, 3) = -xi(1);
|
||||
Y(2, 3) = +xi(0);
|
||||
return Y - Y.transpose();
|
||||
}
|
||||
|
||||
|
|
@ -72,9 +73,9 @@ Vector6 SO4::Vee(const Matrix4& X) {
|
|||
xi(5) = -X(0, 1);
|
||||
xi(4) = +X(0, 2);
|
||||
xi(3) = -X(1, 2);
|
||||
xi(2) = -X(0, 3);
|
||||
xi(1) = +X(1, 3);
|
||||
xi(0) = -X(2, 3);
|
||||
xi(2) = +X(0, 3);
|
||||
xi(1) = -X(1, 3);
|
||||
xi(0) = +X(2, 3);
|
||||
return xi;
|
||||
}
|
||||
|
||||
|
|
@ -208,9 +209,9 @@ GTSAM_EXPORT Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
|
|||
if (H) {
|
||||
const Vector3 m1 = M.col(0), m2 = M.col(1), m3 = M.col(2),
|
||||
q = R.topRightCorner<3, 1>();
|
||||
*H << Z_3x1, Z_3x1, q, Z_3x1, -m3, m2, //
|
||||
Z_3x1, -q, Z_3x1, m3, Z_3x1, -m1, //
|
||||
q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
|
||||
*H << Z_3x1, Z_3x1, -q, Z_3x1, -m3, m2, //
|
||||
Z_3x1, q, Z_3x1, m3, Z_3x1, -m1, //
|
||||
-q, Z_3x1, Z_3x1, -m2, m1, Z_3x1;
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
|
@ -221,9 +222,9 @@ GTSAM_EXPORT Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
|
|||
const Matrix43 M = R.leftCols<3>();
|
||||
if (H) {
|
||||
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, //
|
||||
Z_4x1, -q, Z_4x1, m3, Z_4x1, -m1, //
|
||||
q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
|
||||
*H << Z_4x1, Z_4x1, -q, Z_4x1, -m3, m2, //
|
||||
Z_4x1, q, Z_4x1, m3, Z_4x1, -m1, //
|
||||
-q, Z_4x1, Z_4x1, -m2, m1, Z_4x1;
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,11 +38,12 @@ Matrix SOn::Hat(const Vector& xi) {
|
|||
const size_t dmin = (n - 1) * (n - 2) / 2;
|
||||
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
|
||||
double sign = 1.0;
|
||||
for (size_t i = 0; i < n - 1; 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);
|
||||
sign = -sign;
|
||||
}
|
||||
|
|
@ -67,10 +68,10 @@ Vector SOn::Vee(const Matrix& X) {
|
|||
Vector xi(d);
|
||||
|
||||
// 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++) {
|
||||
const size_t j = n - 2 - i;
|
||||
xi(i) = sign * X(n - 1, j);
|
||||
xi(i) = -sign * X(n - 1, j);
|
||||
sign = -sign;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -24,10 +24,11 @@
|
|||
|
||||
#include <Eigen/Core>
|
||||
|
||||
#include <random>
|
||||
#include <iostream> // TODO(frank): how to avoid?
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
#include <random>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -93,6 +94,16 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
|||
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>
|
||||
template <int M, int N_ = N, typename = IsDynamic<N_>>
|
||||
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:
|
||||
* a b c d | u v w | x y | z
|
||||
* where the latter elements correspond to "telescoping" sub-algebras:
|
||||
* 0 -z y -w d
|
||||
* z 0 -x v -c
|
||||
* -y x 0 -u b
|
||||
* w -v u 0 -a
|
||||
* -d c -b a 0
|
||||
* 0 -z y w -d
|
||||
* z 0 -x -v c
|
||||
* -y x 0 u -b
|
||||
* -w v -u 0 a
|
||||
* d -c b -a 0
|
||||
* This scheme behaves exactly as expected for SO(2) and SO(3).
|
||||
*/
|
||||
static MatrixNN Hat(const TangentVector& xi);
|
||||
|
|
|
|||
|
|
@ -181,63 +181,81 @@ TEST( Rot3, retract)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, log)
|
||||
{
|
||||
TEST(Rot3, log) {
|
||||
static const double PI = boost::math::constants::pi<double>();
|
||||
Vector w;
|
||||
Rot3 R;
|
||||
|
||||
#define CHECK_OMEGA(X,Y,Z) \
|
||||
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
|
||||
R = Rot3::Rodrigues(w); \
|
||||
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
|
||||
#define CHECK_OMEGA(X, Y, Z) \
|
||||
w = (Vector(3) << X, Y, Z).finished(); \
|
||||
R = Rot3::Rodrigues(w); \
|
||||
EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
|
||||
|
||||
// Check zero
|
||||
CHECK_OMEGA( 0, 0, 0)
|
||||
CHECK_OMEGA(0, 0, 0)
|
||||
|
||||
// create a random direction:
|
||||
double norm=sqrt(1.0+16.0+4.0);
|
||||
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
|
||||
double norm = sqrt(1.0 + 16.0 + 4.0);
|
||||
double x = 1.0 / norm, y = 4.0 / norm, z = 2.0 / norm;
|
||||
|
||||
// Check very small rotation for Taylor expansion
|
||||
// Note that tolerance above is 1e-12, so Taylor is pretty good !
|
||||
double d = 0.0001;
|
||||
CHECK_OMEGA( d, 0, 0)
|
||||
CHECK_OMEGA( 0, d, 0)
|
||||
CHECK_OMEGA( 0, 0, d)
|
||||
CHECK_OMEGA(x*d, y*d, z*d)
|
||||
CHECK_OMEGA(d, 0, 0)
|
||||
CHECK_OMEGA(0, d, 0)
|
||||
CHECK_OMEGA(0, 0, d)
|
||||
CHECK_OMEGA(x * d, y * d, z * d)
|
||||
|
||||
// check normal rotation
|
||||
d = 0.1;
|
||||
CHECK_OMEGA( d, 0, 0)
|
||||
CHECK_OMEGA( 0, d, 0)
|
||||
CHECK_OMEGA( 0, 0, d)
|
||||
CHECK_OMEGA(x*d, y*d, z*d)
|
||||
CHECK_OMEGA(d, 0, 0)
|
||||
CHECK_OMEGA(0, d, 0)
|
||||
CHECK_OMEGA(0, 0, d)
|
||||
CHECK_OMEGA(x * d, y * d, z * d)
|
||||
|
||||
// Check 180 degree rotations
|
||||
CHECK_OMEGA( PI, 0, 0)
|
||||
CHECK_OMEGA( 0, PI, 0)
|
||||
CHECK_OMEGA( 0, 0, PI)
|
||||
CHECK_OMEGA(PI, 0, 0)
|
||||
CHECK_OMEGA(0, PI, 0)
|
||||
CHECK_OMEGA(0, 0, PI)
|
||||
|
||||
// Windows and Linux have flipped sign in quaternion mode
|
||||
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
|
||||
w = (Vector(3) << x*PI, y*PI, z*PI).finished();
|
||||
#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
|
||||
w = (Vector(3) << x * PI, y * PI, z * PI).finished();
|
||||
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
|
||||
CHECK_OMEGA(x*PI,y*PI,z*PI)
|
||||
CHECK_OMEGA(x * PI, y * PI, z * PI)
|
||||
#endif
|
||||
|
||||
// Check 360 degree rotations
|
||||
#define CHECK_OMEGA_ZERO(X,Y,Z) \
|
||||
w = (Vector(3) << (double)X, (double)Y, double(Z)).finished(); \
|
||||
R = Rot3::Rodrigues(w); \
|
||||
EXPECT(assert_equal((Vector) Z_3x1, Rot3::Logmap(R)));
|
||||
#define CHECK_OMEGA_ZERO(X, Y, Z) \
|
||||
w = (Vector(3) << X, Y, Z).finished(); \
|
||||
R = Rot3::Rodrigues(w); \
|
||||
EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
|
||||
|
||||
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
|
||||
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0)
|
||||
CHECK_OMEGA_ZERO( 0, 0, 2.0*PI)
|
||||
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
|
||||
CHECK_OMEGA_ZERO(2.0 * PI, 0, 0)
|
||||
CHECK_OMEGA_ZERO(0, 2.0 * PI, 0)
|
||||
CHECK_OMEGA_ZERO(0, 0, 2.0 * 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);
|
||||
|
||||
// Rot3's Logmap returns different, but equivalent compacted
|
||||
// axis-angle vectors depending on whether Rot3 is implemented
|
||||
// by Quaternions or SO3.
|
||||
#if defined(GTSAM_USE_QUATERNIONS)
|
||||
// Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
|
||||
EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
|
||||
(Vector)Rot3::Logmap(Rlund), 1e-8));
|
||||
#else
|
||||
// SO3 does not bound angle resulting in ~180.1 degrees
|
||||
EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
|
||||
(Vector)Rot3::Logmap(Rlund), 1e-8));
|
||||
#endif
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -536,16 +554,15 @@ TEST( Rot3, logmapStability ) {
|
|||
TEST(Rot3, quaternion) {
|
||||
// NOTE: This is also verifying the ability to convert Vector to Quaternion
|
||||
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
|
||||
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
|
||||
0.271018623057411, 0.278786459830371, 0.921318086098018,
|
||||
0.578529366719085, 0.717799701969298, -0.387385285854279,
|
||||
-0.769319620053772, 0.637998195662053, 0.033250932803219).finished());
|
||||
Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
|
||||
0.578529366719085, 0.717799701969298, -0.387385285854279,
|
||||
-0.769319620053772, 0.637998195662053, 0.033250932803219);
|
||||
|
||||
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
|
||||
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
|
||||
-0.207341903877828, 0.250149415542075, 0.945745528564780,
|
||||
0.881304914479026, -0.371869043667957, 0.291573424846290,
|
||||
0.424630407073532, 0.893945571198514, -0.143353873763946).finished());
|
||||
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335,
|
||||
0.599136268678053);
|
||||
Rot3 R2(-0.207341903877828, 0.250149415542075, 0.945745528564780,
|
||||
0.881304914479026, -0.371869043667957, 0.291573424846290,
|
||||
0.424630407073532, 0.893945571198514, -0.143353873763946);
|
||||
|
||||
// Check creating Rot3 from quaternion
|
||||
EXPECT(assert_equal(R1, Rot3(q1)));
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ TEST(SOn, SO0) {
|
|||
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::dimension);
|
||||
EXPECT_LONGS_EQUAL(Eigen::Dynamic, SOn::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::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) {
|
||||
Vector6 v;
|
||||
v << 1, 2, 3, 4, 5, 6;
|
||||
Vector10 v;
|
||||
v << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
|
||||
|
||||
Matrix expected2(2, 2);
|
||||
expected2 << 0, -1, 1, 0;
|
||||
const auto actual2 = SOn::Hat(v.head<1>());
|
||||
CHECK(assert_equal(expected2, actual2));
|
||||
CHECK(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
|
||||
EXPECT(assert_equal(expected2, actual2));
|
||||
EXPECT(assert_equal((Vector)v.head<1>(), SOn::Vee(actual2)));
|
||||
|
||||
Matrix expected3(3, 3);
|
||||
expected3 << 0, -3, 2, //
|
||||
3, 0, -1, //
|
||||
-2, 1, 0;
|
||||
expected3 << 0, -3, 2, //
|
||||
3, 0, -1, //
|
||||
-2, 1, 0;
|
||||
const auto actual3 = SOn::Hat(v.head<3>());
|
||||
CHECK(assert_equal(expected3, actual3));
|
||||
CHECK(assert_equal(skewSymmetric(1, 2, 3), actual3));
|
||||
CHECK(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
|
||||
EXPECT(assert_equal(expected3, actual3));
|
||||
EXPECT(assert_equal(skewSymmetric(1, 2, 3), actual3));
|
||||
EXPECT(assert_equal((Vector)v.head<3>(), SOn::Vee(actual3)));
|
||||
|
||||
Matrix expected4(4, 4);
|
||||
expected4 << 0, -6, 5, -3, //
|
||||
6, 0, -4, 2, //
|
||||
-5, 4, 0, -1, //
|
||||
3, -2, 1, 0;
|
||||
const auto actual4 = SOn::Hat(v);
|
||||
CHECK(assert_equal(expected4, actual4));
|
||||
CHECK(assert_equal((Vector)v, SOn::Vee(actual4)));
|
||||
expected4 << 0, -6, 5, 3, //
|
||||
6, 0, -4, -2, //
|
||||
-5, 4, 0, 1, //
|
||||
-3, 2, -1, 0;
|
||||
const auto actual4 = SOn::Hat(v.head<6>());
|
||||
EXPECT(assert_equal(expected4, 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)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
|
|||
Loading…
Reference in New Issue