Merge pull request #325 from borglab/feature/alternate_hatvee_signs

Alternate signs on Hat/Vee and update tests
release/4.3a0
Akshay Krishnan 2020-06-12 08:27:15 -04:00 committed by GitHub
commit 8ce40a3584
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 139 additions and 97 deletions

View File

@ -2294,6 +2294,8 @@ virtual class NonlinearOptimizerParams {
bool checkConvergence(double relativeErrorTreshold,
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>

View File

@ -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);

View File

@ -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>));

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.
// 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;

View File

@ -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;
}

View File

@ -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;
}

View File

@ -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);

View File

@ -181,14 +181,13 @@ 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(); \
w = (Vector(3) << X, Y, Z).finished(); \
R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
@ -230,7 +229,7 @@ TEST(Rot3, log)
// Check 360 degree rotations
#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); \
EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
@ -238,6 +237,25 @@ TEST(Rot3, log)
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,
Rot3 R1(0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219).finished());
-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,
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).finished());
0.424630407073532, 0.893945571198514, -0.143353873763946);
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));

View File

@ -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;
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)));
}
//******************************************************************************