Merge pull request #73 from borglab/feature/rotation_group
Merging the rotation groups branch into developrelease/4.3a0
Fan Jiang
GitHub
commit
662e5acbd5
|
|
@ -24,6 +24,7 @@ function configure()
|
|||
-DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
|
||||
-DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
|
||||
-DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
|
||||
-DGTSAM_USE_QUATERNIONS=${GTSAM_BUILD_UNSTABLE:-OFF} \
|
||||
-DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
|
||||
-DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \
|
||||
-DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF \
|
||||
|
|
|
|||
33
.travis.yml
33
.travis.yml
|
|
@ -28,6 +28,7 @@ install:
|
|||
stages:
|
||||
- compile
|
||||
- test
|
||||
- special
|
||||
|
||||
env:
|
||||
global:
|
||||
|
|
@ -82,8 +83,8 @@ jobs:
|
|||
compiler: clang
|
||||
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
|
||||
script: bash .travis.sh -b
|
||||
# on Linux, with deprecated ON to make sure that path still compiles
|
||||
- stage: compile
|
||||
# on Linux, with deprecated ON to make sure that path still compiles/tests
|
||||
- stage: special
|
||||
os: linux
|
||||
compiler: clang
|
||||
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
|
||||
|
|
@ -105,15 +106,19 @@ jobs:
|
|||
compiler: gcc
|
||||
env: CMAKE_BUILD_TYPE=Release
|
||||
script: bash .travis.sh -t
|
||||
# Exclude g++ debug on Linux as it consistently times out
|
||||
# - stage: test
|
||||
# os: linux
|
||||
# compiler: gcc
|
||||
# env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
|
||||
# script: bash .travis.sh -t
|
||||
# Exclude clang on Linux/clang in release until issue #57 is solved
|
||||
# - stage: test
|
||||
# os: linux
|
||||
# compiler: clang
|
||||
# env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
|
||||
# script: bash .travis.sh -t
|
||||
- stage: test
|
||||
os: linux
|
||||
compiler: gcc
|
||||
env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF
|
||||
script: bash .travis.sh -t
|
||||
- stage: test
|
||||
os: linux
|
||||
compiler: clang
|
||||
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release
|
||||
script: bash .travis.sh -t
|
||||
# on Linux, with quaternions ON to make sure that path still compiles/tests
|
||||
- stage: special
|
||||
os: linux
|
||||
compiler: clang
|
||||
env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Release GTSAM_BUILD_UNSTABLE=OFF GTSAM_USE_QUATERNIONS=ON
|
||||
script: bash .travis.sh -t
|
||||
|
|
|
|||
|
|
@ -0,0 +1,80 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
KarcherMeanFactor unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
|
||||
# pylint: disable=invalid-name, no-name-in-module, no-member
|
||||
|
||||
import unittest
|
||||
|
||||
import gtsam
|
||||
import numpy as np
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
KEY = 0
|
||||
MODEL = gtsam.noiseModel_Unit.Create(3)
|
||||
|
||||
|
||||
def find_Karcher_mean_Rot3(rotations):
|
||||
"""Find the Karcher mean of given values."""
|
||||
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
|
||||
# No closed form solution.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
for R in rotations:
|
||||
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
|
||||
initial = gtsam.Values()
|
||||
initial.insert(KEY, gtsam.Rot3())
|
||||
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
|
||||
return result.atRot3(KEY)
|
||||
|
||||
|
||||
# Rot3 version
|
||||
R = gtsam.Rot3.Expmap(np.array([0.1, 0, 0]))
|
||||
|
||||
|
||||
class TestKarcherMean(GtsamTestCase):
|
||||
|
||||
def test_find(self):
|
||||
# Check that optimizing for Karcher mean (which minimizes Between distance)
|
||||
# gets correct result.
|
||||
rotations = {R, R.inverse()}
|
||||
expected = gtsam.Rot3()
|
||||
actual = find_Karcher_mean_Rot3(rotations)
|
||||
self.gtsamAssertEquals(expected, actual)
|
||||
|
||||
def test_factor(self):
|
||||
"""Check that the InnerConstraint factor leaves the mean unchanged."""
|
||||
# Make a graph with two variables, one between, and one InnerConstraint
|
||||
# The optimal result should satisfy the between, while moving the other
|
||||
# variable to make the mean the same as before.
|
||||
# Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
|
||||
# R*R*R, i.e. geodesic length is 3 rather than 2.
|
||||
graph = gtsam.NonlinearFactorGraph()
|
||||
R12 = R.compose(R.compose(R))
|
||||
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
|
||||
keys = gtsam.KeyVector()
|
||||
keys.push_back(1)
|
||||
keys.push_back(2)
|
||||
graph.add(gtsam.KarcherMeanFactorRot3(keys))
|
||||
|
||||
initial = gtsam.Values()
|
||||
initial.insert(1, R.inverse())
|
||||
initial.insert(2, R)
|
||||
expected = find_Karcher_mean_Rot3([R, R.inverse()])
|
||||
|
||||
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
|
||||
actual = find_Karcher_mean_Rot3(
|
||||
[result.atRot3(1), result.atRot3(2)])
|
||||
self.gtsamAssertEquals(expected, actual)
|
||||
self.gtsamAssertEquals(
|
||||
R12, result.atRot3(1).between(result.atRot3(2)))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -6,8 +6,9 @@ All Rights Reserved
|
|||
See LICENSE for the license information
|
||||
|
||||
Pose3 unit tests.
|
||||
Author: Frank Dellaert & Duy Nguyen Ta (Python)
|
||||
Author: Frank Dellaert, Duy Nguyen Ta
|
||||
"""
|
||||
# pylint: disable=no-name-in-module
|
||||
import math
|
||||
import unittest
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,59 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
SO4 unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-name-in-module, import-error
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import SO4
|
||||
|
||||
I4 = SO4()
|
||||
v1 = np.array([0, 0, 0, .1, 0, 0])
|
||||
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
|
||||
Q1 = SO4.Expmap(v1)
|
||||
Q2 = SO4.Expmap(v2)
|
||||
|
||||
|
||||
class TestSO4(unittest.TestCase):
|
||||
"""Test selected SO4 methods."""
|
||||
|
||||
def test_constructor(self):
|
||||
"""Construct from matrix."""
|
||||
matrix = np.eye(4)
|
||||
so4 = SO4(matrix)
|
||||
self.assertIsInstance(so4, SO4)
|
||||
|
||||
def test_retract(self):
|
||||
"""Test retraction to manifold."""
|
||||
v = np.zeros((6,), np.float)
|
||||
actual = I4.retract(v)
|
||||
self.assertTrue(actual.equals(I4, 1e-9))
|
||||
|
||||
def test_local(self):
|
||||
"""Check localCoordinates for trivial case."""
|
||||
v0 = np.zeros((6,), np.float)
|
||||
actual = I4.localCoordinates(I4)
|
||||
np.testing.assert_array_almost_equal(actual, v0)
|
||||
|
||||
def test_compose(self):
|
||||
"""Check compose works in subgroup."""
|
||||
expected = SO4.Expmap(2*v1)
|
||||
actual = Q1.compose(Q1)
|
||||
self.assertTrue(actual.equals(expected, 1e-9))
|
||||
|
||||
def test_vec(self):
|
||||
"""Check wrapping of vec."""
|
||||
expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
|
||||
actual = I4.vec()
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,59 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Dynamic SO(n) unit tests.
|
||||
Author: Frank Dellaert
|
||||
"""
|
||||
# pylint: disable=no-name-in-module, import-error
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
from gtsam import SOn
|
||||
|
||||
I4 = SOn(4)
|
||||
v1 = np.array([0, 0, 0, .1, 0, 0])
|
||||
v2 = np.array([0, 0, 0, 0.01, 0.02, 0.03])
|
||||
Q1 = I4.retract(v1)
|
||||
Q2 = I4.retract(v2)
|
||||
|
||||
|
||||
class TestSO4(unittest.TestCase):
|
||||
"""Test selected SOn methods."""
|
||||
|
||||
def test_constructor(self):
|
||||
"""Construct from matrix."""
|
||||
matrix = np.eye(4)
|
||||
so4 = SOn.FromMatrix(matrix)
|
||||
self.assertIsInstance(so4, SOn)
|
||||
|
||||
def test_retract(self):
|
||||
"""Test retraction to manifold."""
|
||||
v = np.zeros((6,), np.float)
|
||||
actual = I4.retract(v)
|
||||
self.assertTrue(actual.equals(I4, 1e-9))
|
||||
|
||||
def test_local(self):
|
||||
"""Check localCoordinates for trivial case."""
|
||||
v0 = np.zeros((6,), np.float)
|
||||
actual = I4.localCoordinates(I4)
|
||||
np.testing.assert_array_almost_equal(actual, v0)
|
||||
|
||||
def test_compose(self):
|
||||
"""Check compose works in subgroup."""
|
||||
expected = I4.retract(2*v1)
|
||||
actual = Q1.compose(Q1)
|
||||
self.assertTrue(actual.equals(expected, 1e-3)) # not exmap so only approximate
|
||||
|
||||
def test_vec(self):
|
||||
"""Check wrapping of vec."""
|
||||
expected = np.array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1])
|
||||
actual = I4.vec()
|
||||
np.testing.assert_array_equal(actual, expected)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
103
gtsam.h
103
gtsam.h
|
|
@ -537,6 +537,88 @@ class Rot2 {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
class SO3 {
|
||||
// Standard Constructors
|
||||
SO3();
|
||||
SO3(Matrix R);
|
||||
static gtsam::SO3 FromMatrix(Matrix R);
|
||||
static gtsam::SO3 AxisAngle(const Vector axis, double theta);
|
||||
static gtsam::SO3 ClosestTo(const Matrix M);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::SO3& other, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::SO3 identity();
|
||||
gtsam::SO3 inverse() const;
|
||||
gtsam::SO3 between(const gtsam::SO3& R) const;
|
||||
gtsam::SO3 compose(const gtsam::SO3& R) const;
|
||||
|
||||
// Manifold
|
||||
gtsam::SO3 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::SO3& R) const;
|
||||
static gtsam::SO3 Expmap(Vector v);
|
||||
|
||||
// Other methods
|
||||
Vector vec() const;
|
||||
Matrix matrix() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
class SO4 {
|
||||
// Standard Constructors
|
||||
SO4();
|
||||
SO4(Matrix R);
|
||||
static gtsam::SO4 FromMatrix(Matrix R);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::SO4& other, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::SO4 identity();
|
||||
gtsam::SO4 inverse() const;
|
||||
gtsam::SO4 between(const gtsam::SO4& Q) const;
|
||||
gtsam::SO4 compose(const gtsam::SO4& Q) const;
|
||||
|
||||
// Manifold
|
||||
gtsam::SO4 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::SO4& Q) const;
|
||||
static gtsam::SO4 Expmap(Vector v);
|
||||
|
||||
// Other methods
|
||||
Vector vec() const;
|
||||
Matrix matrix() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/SOn.h>
|
||||
class SOn {
|
||||
// Standard Constructors
|
||||
SOn(size_t n);
|
||||
static gtsam::SOn FromMatrix(Matrix R);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::SOn& other, double tol) const;
|
||||
|
||||
// Group
|
||||
static gtsam::SOn identity();
|
||||
gtsam::SOn inverse() const;
|
||||
gtsam::SOn between(const gtsam::SOn& Q) const;
|
||||
gtsam::SOn compose(const gtsam::SOn& Q) const;
|
||||
|
||||
// Manifold
|
||||
gtsam::SOn retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::SOn& Q) const;
|
||||
static gtsam::SOn Expmap(Vector v);
|
||||
|
||||
// Other methods
|
||||
Vector vec() const;
|
||||
Matrix matrix() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
class Rot3 {
|
||||
// Standard Constructors and Named Constructors
|
||||
|
|
@ -559,6 +641,7 @@ class Rot3 {
|
|||
static gtsam::Rot3 Quaternion(double w, double x, double y, double z);
|
||||
static gtsam::Rot3 Rodrigues(Vector v);
|
||||
static gtsam::Rot3 Rodrigues(double wx, double wy, double wz);
|
||||
static gtsam::Rot3 ClosestTo(const Matrix M);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
|
|
@ -2032,6 +2115,9 @@ class Values {
|
|||
void insert(size_t j, const gtsam::Point3& point3);
|
||||
void insert(size_t j, const gtsam::Rot2& rot2);
|
||||
void insert(size_t j, const gtsam::Pose2& pose2);
|
||||
void insert(size_t j, const gtsam::SO3& R);
|
||||
void insert(size_t j, const gtsam::SO4& Q);
|
||||
void insert(size_t j, const gtsam::SOn& P);
|
||||
void insert(size_t j, const gtsam::Rot3& rot3);
|
||||
void insert(size_t j, const gtsam::Pose3& pose3);
|
||||
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
|
||||
|
|
@ -2047,6 +2133,9 @@ class Values {
|
|||
void update(size_t j, const gtsam::Point3& point3);
|
||||
void update(size_t j, const gtsam::Rot2& rot2);
|
||||
void update(size_t j, const gtsam::Pose2& pose2);
|
||||
void update(size_t j, const gtsam::SO3& R);
|
||||
void update(size_t j, const gtsam::SO4& Q);
|
||||
void update(size_t j, const gtsam::SOn& P);
|
||||
void update(size_t j, const gtsam::Rot3& rot3);
|
||||
void update(size_t j, const gtsam::Pose3& pose3);
|
||||
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
|
||||
|
|
@ -2057,7 +2146,7 @@ class Values {
|
|||
void update(size_t j, Vector vector);
|
||||
void update(size_t j, Matrix matrix);
|
||||
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
|
||||
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
|
||||
T at(size_t j);
|
||||
|
||||
/// version for double
|
||||
|
|
@ -2394,7 +2483,7 @@ class NonlinearISAM {
|
|||
#include <gtsam/geometry/StereoPoint2.h>
|
||||
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
|
||||
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::imuBias::ConstantBias}>
|
||||
virtual class PriorFactor : gtsam::NoiseModelFactor {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
T prior() const;
|
||||
|
|
@ -2405,7 +2494,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
|
|||
|
||||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
template<T = {Vector,gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
|
||||
virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
||||
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
|
||||
T measured() const;
|
||||
|
|
@ -2417,7 +2506,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
|||
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
|
||||
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::imuBias::ConstantBias}>
|
||||
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
|
||||
// Constructor - forces exact evaluation
|
||||
NonlinearEquality(size_t j, const T& feasible);
|
||||
|
|
@ -2676,6 +2765,12 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool
|
|||
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
|
||||
const gtsam::Values& estimate, string filename);
|
||||
|
||||
#include <gtsam/slam/KarcherMeanFactor-inl.h>
|
||||
template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
|
||||
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
|
||||
KarcherMeanFactor(const gtsam::KeyVector& keys);
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// Navigation
|
||||
//*************************************************************************
|
||||
|
|
|
|||
|
|
@ -35,9 +35,6 @@ namespace gtsam {
|
|||
template <class Class, int N>
|
||||
struct LieGroup {
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG(N != Eigen::Dynamic,
|
||||
"LieGroup not yet specialized for dynamically sized types.");
|
||||
|
||||
enum { dimension = N };
|
||||
typedef OptionalJacobian<N, N> ChartJacobian;
|
||||
typedef Eigen::Matrix<double, N, N> Jacobian;
|
||||
|
|
@ -189,9 +186,6 @@ struct LieGroupTraits {
|
|||
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
|
||||
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
|
||||
|
||||
BOOST_STATIC_ASSERT_MSG(dimension != Eigen::Dynamic,
|
||||
"LieGroupTraits not yet specialized for dynamically sized types.");
|
||||
|
||||
static int GetDimension(const Class&) {return dimension;}
|
||||
|
||||
static TangentVector Local(const Class& origin, const Class& other,
|
||||
|
|
|
|||
|
|
@ -176,10 +176,11 @@ public:
|
|||
pointer_(NULL) {
|
||||
}
|
||||
|
||||
/// Constructor that will resize a dynamic matrix (unless already correct)
|
||||
OptionalJacobian(Eigen::MatrixXd& dynamic) :
|
||||
pointer_(&dynamic) {
|
||||
}
|
||||
/// Construct from pointer to dynamic matrix
|
||||
OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
|
||||
|
||||
/// Construct from refrence to dynamic matrix
|
||||
OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
|
||||
|
||||
#ifndef OPTIONALJACOBIAN_NOBOOST
|
||||
|
||||
|
|
|
|||
|
|
@ -52,7 +52,6 @@ Pose3 Pose3::inverse() const {
|
|||
/* ************************************************************************* */
|
||||
// Calculate Adjoint map
|
||||
// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
// Experimental - unit tests of derivatives based on it do not check out yet
|
||||
Matrix6 Pose3::AdjointMap() const {
|
||||
const Matrix3 R = R_.matrix();
|
||||
Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
|
||||
|
|
@ -418,24 +417,11 @@ boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs)
|
|||
for(const Point3Pair& abPair: abPointPairs) {
|
||||
Point3 da = abPair.first - aCentroid;
|
||||
Point3 db = abPair.second - bCentroid;
|
||||
H += db * da.transpose();
|
||||
H += da * db.transpose();
|
||||
}
|
||||
|
||||
// Compute SVD
|
||||
Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
|
||||
Matrix U = svd.matrixU();
|
||||
Vector S = svd.singularValues();
|
||||
Matrix V = svd.matrixV();
|
||||
|
||||
// Check rank
|
||||
if (S[1] < 1e-10)
|
||||
return boost::none;
|
||||
|
||||
// Recover transform with correction from Eggert97machinevisionandapplications
|
||||
Matrix3 UVtranspose = U * V.transpose();
|
||||
Matrix3 detWeighting = I_3x3;
|
||||
detWeighting(2, 2) = UVtranspose.determinant();
|
||||
Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
|
||||
// ClosestTo finds rotation matrix closest to H in Frobenius sense
|
||||
Rot3 aRb = Rot3::ClosestTo(H);
|
||||
Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
|
||||
return Pose3(aRb, aTb);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ namespace gtsam {
|
|||
/** Internal Eigen Quaternion */
|
||||
gtsam::Quaternion quaternion_;
|
||||
#else
|
||||
Matrix3 rot_;
|
||||
SO3 rot_;
|
||||
#endif
|
||||
|
||||
public:
|
||||
|
|
@ -92,26 +92,34 @@ namespace gtsam {
|
|||
* allow assignment/construction from a generic matrix.
|
||||
* See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
|
||||
*/
|
||||
template<typename Derived>
|
||||
inline explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
quaternion_=Matrix3(R);
|
||||
#else
|
||||
rot_ = R;
|
||||
#endif
|
||||
template <typename Derived>
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
|
||||
quaternion_ = Matrix3(R);
|
||||
}
|
||||
#else
|
||||
explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
|
||||
}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Constructor from a rotation matrix
|
||||
* Overload version for Matrix3 to avoid casting in quaternion mode.
|
||||
*/
|
||||
inline explicit Rot3(const Matrix3& R) {
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
quaternion_=R;
|
||||
#else
|
||||
rot_ = R;
|
||||
#endif
|
||||
}
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
explicit Rot3(const Matrix3& R) : quaternion_(R) {}
|
||||
#else
|
||||
explicit Rot3(const Matrix3& R) : rot_(R) {}
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Constructor from an SO3 instance
|
||||
*/
|
||||
#ifdef GTSAM_USE_QUATERNIONS
|
||||
explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
|
||||
#else
|
||||
explicit Rot3(const SO3& R) : rot_(R) {}
|
||||
#endif
|
||||
|
||||
/** Constructor from a quaternion. This can also be called using a plain
|
||||
* Vector, due to implicit conversion from Vector to Quaternion
|
||||
|
|
@ -228,6 +236,9 @@ namespace gtsam {
|
|||
static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
|
||||
const Unit3& a_q, const Unit3& b_q);
|
||||
|
||||
/// Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
|
||||
static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
@ -483,28 +494,27 @@ namespace gtsam {
|
|||
/// @}
|
||||
#endif
|
||||
|
||||
private:
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
|
||||
{
|
||||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
#ifndef GTSAM_USE_QUATERNIONS
|
||||
ar & boost::serialization::make_nvp("rot11", rot_(0,0));
|
||||
ar & boost::serialization::make_nvp("rot12", rot_(0,1));
|
||||
ar & boost::serialization::make_nvp("rot13", rot_(0,2));
|
||||
ar & boost::serialization::make_nvp("rot21", rot_(1,0));
|
||||
ar & boost::serialization::make_nvp("rot22", rot_(1,1));
|
||||
ar & boost::serialization::make_nvp("rot23", rot_(1,2));
|
||||
ar & boost::serialization::make_nvp("rot31", rot_(2,0));
|
||||
ar & boost::serialization::make_nvp("rot32", rot_(2,1));
|
||||
ar & boost::serialization::make_nvp("rot33", rot_(2,2));
|
||||
Matrix3& M = rot_.matrix_;
|
||||
ar& boost::serialization::make_nvp("rot11", M(0, 0));
|
||||
ar& boost::serialization::make_nvp("rot12", M(0, 1));
|
||||
ar& boost::serialization::make_nvp("rot13", M(0, 2));
|
||||
ar& boost::serialization::make_nvp("rot21", M(1, 0));
|
||||
ar& boost::serialization::make_nvp("rot22", M(1, 1));
|
||||
ar& boost::serialization::make_nvp("rot23", M(1, 2));
|
||||
ar& boost::serialization::make_nvp("rot31", M(2, 0));
|
||||
ar& boost::serialization::make_nvp("rot32", M(2, 1));
|
||||
ar& boost::serialization::make_nvp("rot33", M(2, 2));
|
||||
#else
|
||||
ar & boost::serialization::make_nvp("w", quaternion_.w());
|
||||
ar & boost::serialization::make_nvp("x", quaternion_.x());
|
||||
ar & boost::serialization::make_nvp("y", quaternion_.y());
|
||||
ar & boost::serialization::make_nvp("z", quaternion_.z());
|
||||
ar& boost::serialization::make_nvp("w", quaternion_.w());
|
||||
ar& boost::serialization::make_nvp("x", quaternion_.x());
|
||||
ar& boost::serialization::make_nvp("y", quaternion_.y());
|
||||
ar& boost::serialization::make_nvp("z", quaternion_.z());
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -36,18 +36,17 @@ Rot3::Rot3() : rot_(I_3x3) {}
|
|||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
|
||||
rot_.col(0) = col1;
|
||||
rot_.col(1) = col2;
|
||||
rot_.col(2) = col3;
|
||||
Matrix3 R;
|
||||
R << col1, col2, col3;
|
||||
rot_ = SO3(R);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(double R11, double R12, double R13,
|
||||
double R21, double R22, double R23,
|
||||
double R31, double R32, double R33) {
|
||||
rot_ << R11, R12, R13,
|
||||
R21, R22, R23,
|
||||
R31, R32, R33;
|
||||
Rot3::Rot3(double R11, double R12, double R13, double R21, double R22,
|
||||
double R23, double R31, double R32, double R33) {
|
||||
Matrix3 R;
|
||||
R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
|
||||
rot_ = SO3(R);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -107,21 +106,21 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(Matrix3(rot_*R2.rot_));
|
||||
return Rot3(rot_*R2.rot_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
|
||||
Matrix3 Rot3::transpose() const {
|
||||
return rot_.transpose();
|
||||
return rot_.matrix().transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
|
||||
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = rot_;
|
||||
return rot_ * p;
|
||||
if (H1) *H1 = rot_.matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = rot_.matrix();
|
||||
return rot_.matrix() * p;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -178,21 +177,21 @@ Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Rot3::matrix() const {
|
||||
return rot_;
|
||||
return rot_.matrix();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
|
||||
Point3 Rot3::r1() const { return Point3(rot_.matrix().col(0)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
|
||||
Point3 Rot3::r2() const { return Point3(rot_.matrix().col(1)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
|
||||
Point3 Rot3::r3() const { return Point3(rot_.matrix().col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::Quaternion Rot3::toQuaternion() const {
|
||||
return gtsam::Quaternion(rot_);
|
||||
return gtsam::Quaternion(rot_.matrix());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -18,18 +18,38 @@
|
|||
* @date December 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
#include <gtsam/base/concepts.h>
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
|
||||
#include <Eigen/SVD>
|
||||
|
||||
#include <cmath>
|
||||
#include <limits>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//******************************************************************************
|
||||
namespace so3 {
|
||||
|
||||
Matrix99 Dcompose(const SO3& Q) {
|
||||
Matrix99 H;
|
||||
auto R = Q.matrix();
|
||||
H << I_3x3 * R(0, 0), I_3x3 * R(1, 0), I_3x3 * R(2, 0), //
|
||||
I_3x3 * R(0, 1), I_3x3 * R(1, 1), I_3x3 * R(2, 1), //
|
||||
I_3x3 * R(0, 2), I_3x3 * R(1, 2), I_3x3 * R(2, 2);
|
||||
return H;
|
||||
}
|
||||
|
||||
Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, 9> H) {
|
||||
Matrix3 MR = M * R.matrix();
|
||||
if (H) *H = Dcompose(R);
|
||||
return MR;
|
||||
}
|
||||
|
||||
void ExpmapFunctor::init(bool nearZeroApprox) {
|
||||
nearZero = nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
|
||||
nearZero =
|
||||
nearZeroApprox || (theta2 <= std::numeric_limits<double>::epsilon());
|
||||
if (!nearZero) {
|
||||
sin_theta = std::sin(theta);
|
||||
const double s2 = std::sin(theta / 2.0);
|
||||
|
|
@ -48,7 +68,8 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
|
|||
}
|
||||
}
|
||||
|
||||
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox)
|
||||
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
|
||||
bool nearZeroApprox)
|
||||
: theta2(angle * angle), theta(angle) {
|
||||
const double ax = axis.x(), ay = axis.y(), az = axis.z();
|
||||
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
|
||||
|
|
@ -61,16 +82,16 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApp
|
|||
|
||||
SO3 ExpmapFunctor::expmap() const {
|
||||
if (nearZero)
|
||||
return I_3x3 + W;
|
||||
return SO3(I_3x3 + W);
|
||||
else
|
||||
return I_3x3 + sin_theta * K + one_minus_cos * KK;
|
||||
return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK);
|
||||
}
|
||||
|
||||
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
|
||||
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
|
||||
if (nearZero)
|
||||
if (nearZero) {
|
||||
dexp_ = I_3x3 - 0.5 * W;
|
||||
else {
|
||||
} else {
|
||||
a = one_minus_cos / theta;
|
||||
b = 1.0 - sin_theta / theta;
|
||||
dexp_ = I_3x3 - a * K + b * KK;
|
||||
|
|
@ -103,7 +124,7 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
|||
if (H1) {
|
||||
Matrix3 D_dexpv_omega;
|
||||
applyDexp(c, D_dexpv_omega); // get derivative H of forward mapping
|
||||
*H1 = -invDexp* D_dexpv_omega;
|
||||
*H1 = -invDexp * D_dexpv_omega;
|
||||
}
|
||||
if (H2) *H2 = invDexp;
|
||||
return c;
|
||||
|
|
@ -111,39 +132,117 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1,
|
|||
|
||||
} // namespace so3
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
template <>
|
||||
SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
|
||||
return so3::ExpmapFunctor(axis, theta).expmap();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void SO3::print(const std::string& s) const {
|
||||
std::cout << s << *this << std::endl;
|
||||
}
|
||||
//******************************************************************************
|
||||
template <>
|
||||
SO3 SO3::ClosestTo(const Matrix3& M) {
|
||||
Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||||
const auto& U = svd.matrixU();
|
||||
const auto& V = svd.matrixV();
|
||||
const double det = (U * V.transpose()).determinant();
|
||||
return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
template <>
|
||||
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
|
||||
// See Hartley13ijcv:
|
||||
// Cost function C(R) = \sum sqr(|R-R_i|_F)
|
||||
// Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
|
||||
Matrix3 C_e{Z_3x3};
|
||||
for (const auto& R_i : rotations) {
|
||||
C_e += R_i.matrix();
|
||||
}
|
||||
return ClosestTo(C_e);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Matrix3 SO3::Hat(const Vector3& xi) {
|
||||
// skew symmetric matrix X = xi^
|
||||
Matrix3 Y = Z_3x3;
|
||||
Y(0, 1) = -xi(2);
|
||||
Y(0, 2) = +xi(1);
|
||||
Y(1, 2) = -xi(0);
|
||||
return Y - Y.transpose();
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector3 SO3::Vee(const Matrix3& X) {
|
||||
Vector3 xi;
|
||||
xi(0) = -X(1, 2);
|
||||
xi(1) = +X(0, 2);
|
||||
xi(2) = -X(0, 1);
|
||||
return xi;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Matrix3 SO3::AdjointMap() const {
|
||||
return matrix_;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) {
|
||||
if (H) {
|
||||
so3::DexpFunctor impl(omega);
|
||||
*H = impl.dexp();
|
||||
return impl.expmap();
|
||||
} else
|
||||
} else {
|
||||
return so3::ExpmapFunctor(omega).expmap();
|
||||
}
|
||||
}
|
||||
|
||||
template <>
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
return so3::DexpFunctor(omega).dexp();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
||||
using std::sqrt;
|
||||
//******************************************************************************
|
||||
/* Right Jacobian for Log map in SO(3) - equation (10.86) and following
|
||||
equations in G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie
|
||||
Groups", Volume 2, 2008.
|
||||
|
||||
logmap( Rhat * expmap(omega) ) \approx logmap(Rhat) + Jrinv * omega
|
||||
|
||||
where Jrinv = LogmapDerivative(omega). This maps a perturbation on the
|
||||
manifold (expmap(omega)) to a perturbation in the tangent space (Jrinv *
|
||||
omega)
|
||||
*/
|
||||
template <>
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
double theta2 = omega.dot(omega);
|
||||
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
|
||||
double theta = std::sqrt(theta2); // rotation angle
|
||||
|
||||
// element of Lie algebra so(3): W = omega^
|
||||
const Matrix3 W = Hat(omega);
|
||||
return I_3x3 + 0.5 * W +
|
||||
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
|
||||
W * W;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
|
||||
using std::sin;
|
||||
using std::sqrt;
|
||||
|
||||
// note switch to base 1
|
||||
const double& R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
|
||||
const double& R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
|
||||
const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
|
||||
const Matrix3& R = Q.matrix();
|
||||
const double &R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
|
||||
const double &R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
|
||||
const double &R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
|
||||
|
||||
// Get trace(R)
|
||||
const double tr = R.trace();
|
||||
|
|
@ -162,7 +261,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31);
|
||||
} else {
|
||||
double magnitude;
|
||||
const double tr_3 = tr - 3.0; // always negative
|
||||
const double tr_3 = tr - 3.0; // always negative
|
||||
if (tr_3 < -1e-7) {
|
||||
double theta = acos((tr - 1.0) / 2.0);
|
||||
magnitude = theta / (2.0 * sin(theta));
|
||||
|
|
@ -174,32 +273,49 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
|
||||
}
|
||||
|
||||
if(H) *H = LogmapDerivative(omega);
|
||||
if (H) *H = LogmapDerivative(omega);
|
||||
return omega;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
//******************************************************************************
|
||||
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
|
||||
|
||||
double theta2 = omega.dot(omega);
|
||||
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
|
||||
double theta = std::sqrt(theta2); // rotation angle
|
||||
/** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in
|
||||
* G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
|
||||
* logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega
|
||||
* where Jrinv = LogmapDerivative(omega);
|
||||
* This maps a perturbation on the manifold (expmap(omega))
|
||||
* to a perturbation in the tangent space (Jrinv * omega)
|
||||
*/
|
||||
const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^
|
||||
return I_3x3 + 0.5 * W +
|
||||
(1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) *
|
||||
W * W;
|
||||
template <>
|
||||
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) {
|
||||
return Expmap(omega, H);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <>
|
||||
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H) {
|
||||
return Logmap(R, H);
|
||||
}
|
||||
|
||||
} // end namespace gtsam
|
||||
//******************************************************************************
|
||||
// local vectorize
|
||||
static Vector9 vec3(const Matrix3& R) {
|
||||
return Eigen::Map<const Vector9>(R.data());
|
||||
}
|
||||
|
||||
// so<3> generators
|
||||
static std::vector<Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
|
||||
SO3::Hat(Vector3::Unit(1)),
|
||||
SO3::Hat(Vector3::Unit(2))});
|
||||
|
||||
// vectorized generators
|
||||
static const Matrix93 P3 =
|
||||
(Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished();
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const {
|
||||
const Matrix3& R = matrix_;
|
||||
if (H) {
|
||||
// As Luca calculated (for SO4), this is (I3 \oplus R) * P3
|
||||
*H << R * P3.block<3, 3>(0, 0), R * P3.block<3, 3>(3, 0),
|
||||
R * P3.block<3, 3>(6, 0);
|
||||
}
|
||||
return gtsam::vec3(R);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
||||
} // end namespace gtsam
|
||||
|
|
|
|||
|
|
@ -20,120 +20,103 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/SOn.h>
|
||||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <iosfwd>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using SO3 = SO<3>;
|
||||
|
||||
// Below are all declarations of SO<3> specializations.
|
||||
// They are *defined* in SO3.cpp.
|
||||
|
||||
template <>
|
||||
SO3 SO3::AxisAngle(const Vector3& axis, double theta);
|
||||
|
||||
template <>
|
||||
SO3 SO3::ClosestTo(const Matrix3& M);
|
||||
|
||||
template <>
|
||||
SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
|
||||
|
||||
template <>
|
||||
Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix
|
||||
|
||||
template <>
|
||||
Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat
|
||||
|
||||
/// Adjoint map
|
||||
template <>
|
||||
Matrix3 SO3::AdjointMap() const;
|
||||
|
||||
/**
|
||||
* True SO(3), i.e., 3*3 matrix subgroup
|
||||
* We guarantee (all but first) constructors only generate from sub-manifold.
|
||||
* However, round-off errors in repeated composition could move off it...
|
||||
* Exponential map at identity - create a rotation from canonical coordinates
|
||||
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
|
||||
*/
|
||||
class SO3: public Matrix3, public LieGroup<SO3, 3> {
|
||||
template <>
|
||||
SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
|
||||
|
||||
protected:
|
||||
/// Derivative of Expmap
|
||||
template <>
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
public:
|
||||
enum {
|
||||
dimension = 3
|
||||
};
|
||||
/**
|
||||
* Log map at identity - returns the canonical coordinates
|
||||
* \f$ [R_x,R_y,R_z] \f$ of this rotation
|
||||
*/
|
||||
template <>
|
||||
Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
|
||||
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
/// Derivative of Logmap
|
||||
template <>
|
||||
Matrix3 SO3::LogmapDerivative(const Vector3& omega);
|
||||
|
||||
/// Constructor from AngleAxisd
|
||||
SO3() :
|
||||
Matrix3(I_3x3) {
|
||||
}
|
||||
// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
|
||||
template <>
|
||||
SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
|
||||
|
||||
/// Constructor from Eigen Matrix
|
||||
template<typename Derived>
|
||||
SO3(const MatrixBase<Derived>& R) :
|
||||
Matrix3(R.eval()) {
|
||||
}
|
||||
template <>
|
||||
Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
|
||||
|
||||
/// Constructor from AngleAxisd
|
||||
SO3(const Eigen::AngleAxisd& angleAxis) :
|
||||
Matrix3(angleAxis) {
|
||||
}
|
||||
template <>
|
||||
Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
|
||||
|
||||
/// Static, named constructor TODO think about relation with above
|
||||
GTSAM_EXPORT static SO3 AxisAngle(const Vector3& axis, double theta);
|
||||
/** Serialization function */
|
||||
template <class Archive>
|
||||
void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
|
||||
Matrix3& M = R.matrix_;
|
||||
ar& boost::serialization::make_nvp("R11", M(0, 0));
|
||||
ar& boost::serialization::make_nvp("R12", M(0, 1));
|
||||
ar& boost::serialization::make_nvp("R13", M(0, 2));
|
||||
ar& boost::serialization::make_nvp("R21", M(1, 0));
|
||||
ar& boost::serialization::make_nvp("R22", M(1, 1));
|
||||
ar& boost::serialization::make_nvp("R23", M(1, 2));
|
||||
ar& boost::serialization::make_nvp("R31", M(2, 0));
|
||||
ar& boost::serialization::make_nvp("R32", M(2, 1));
|
||||
ar& boost::serialization::make_nvp("R33", M(2, 2));
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
GTSAM_EXPORT void print(const std::string& s) const;
|
||||
|
||||
bool equals(const SO3 & R, double tol) const {
|
||||
return equal_with_abs_tol(*this, R, tol);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// identity rotation for group operation
|
||||
static SO3 identity() {
|
||||
return I_3x3;
|
||||
}
|
||||
|
||||
/// inverse of a rotation = transpose
|
||||
SO3 inverse() const {
|
||||
return this->Matrix3::inverse();
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Exponential map at identity - create a rotation from canonical coordinates
|
||||
* \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula
|
||||
*/
|
||||
GTSAM_EXPORT static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Expmap
|
||||
GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& omega);
|
||||
|
||||
/**
|
||||
* Log map at identity - returns the canonical coordinates
|
||||
* \f$ [R_x,R_y,R_z] \f$ of this rotation
|
||||
*/
|
||||
GTSAM_EXPORT static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Logmap
|
||||
GTSAM_EXPORT static Matrix3 LogmapDerivative(const Vector3& omega);
|
||||
|
||||
Matrix3 AdjointMap() const {
|
||||
return *this;
|
||||
}
|
||||
|
||||
// Chart at origin
|
||||
struct ChartAtOrigin {
|
||||
static SO3 Retract(const Vector3& omega, ChartJacobian H = boost::none) {
|
||||
return Expmap(omega, H);
|
||||
}
|
||||
static Vector3 Local(const SO3& R, ChartJacobian H = boost::none) {
|
||||
return Logmap(R, H);
|
||||
}
|
||||
};
|
||||
|
||||
using LieGroup<SO3, 3>::inverse;
|
||||
|
||||
/// @}
|
||||
};
|
||||
|
||||
// This namespace exposes two functors that allow for saving computation when
|
||||
// exponential map and its derivatives are needed at the same location in so<3>
|
||||
// The second functor also implements dedicated methods to apply dexp and/or inv(dexp)
|
||||
namespace so3 {
|
||||
|
||||
/**
|
||||
* Compose general matrix with an SO(3) element.
|
||||
* We only provide the 9*9 derivative in the first argument M.
|
||||
*/
|
||||
Matrix3 compose(const Matrix3& M, const SO3& R,
|
||||
OptionalJacobian<9, 9> H = boost::none);
|
||||
|
||||
/// (constant) Jacobian of compose wrpt M
|
||||
Matrix99 Dcompose(const SO3& R);
|
||||
|
||||
// Below are two functors that allow for saving computation when exponential map
|
||||
// and its derivatives are needed at the same location in so<3>. The second
|
||||
// functor also implements dedicated methods to apply dexp and/or inv(dexp).
|
||||
|
||||
/// Functor implementing Exponential map
|
||||
class GTSAM_EXPORT ExpmapFunctor {
|
||||
protected:
|
||||
|
|
@ -146,7 +129,7 @@ class GTSAM_EXPORT ExpmapFunctor {
|
|||
|
||||
public:
|
||||
/// Constructor with element of Lie algebra so(3)
|
||||
ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
|
||||
/// Constructor with axis-angle
|
||||
ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
|
||||
|
|
@ -163,7 +146,7 @@ class DexpFunctor : public ExpmapFunctor {
|
|||
|
||||
public:
|
||||
/// Constructor with element of Lie algebra so(3)
|
||||
GTSAM_EXPORT DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
|
||||
|
||||
// NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
|
||||
// (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
|
||||
|
|
@ -184,12 +167,14 @@ class DexpFunctor : public ExpmapFunctor {
|
|||
};
|
||||
} // namespace so3
|
||||
|
||||
template<>
|
||||
struct traits<SO3> : public internal::LieGroup<SO3> {
|
||||
};
|
||||
/*
|
||||
* Define the traits. internal::LieGroup provides both Lie group and Testable
|
||||
*/
|
||||
|
||||
template<>
|
||||
struct traits<const SO3> : public internal::LieGroup<SO3> {
|
||||
};
|
||||
} // end namespace gtsam
|
||||
template <>
|
||||
struct traits<SO3> : public internal::LieGroup<SO3> {};
|
||||
|
||||
template <>
|
||||
struct traits<const SO3> : public internal::LieGroup<SO3> {};
|
||||
|
||||
} // end namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,226 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SO4.cpp
|
||||
* @brief 4*4 matrix representation of SO(4)
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
*/
|
||||
|
||||
#include <gtsam/base/concepts.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
#include <gtsam/geometry/Unit3.h>
|
||||
|
||||
#include <Eigen/Eigenvalues>
|
||||
#include <boost/random.hpp>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// /* *************************************************************************
|
||||
// */ static Vector3 randomOmega(boost::mt19937 &rng) {
|
||||
// static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
||||
// return Unit3::Random(rng).unitVector() * randomAngle(rng);
|
||||
// }
|
||||
|
||||
// /* *************************************************************************
|
||||
// */
|
||||
// // Create random SO(4) element using direct product of lie algebras.
|
||||
// SO4 SO4::Random(boost::mt19937 &rng) {
|
||||
// Vector6 delta;
|
||||
// delta << randomOmega(rng), randomOmega(rng);
|
||||
// return SO4::Expmap(delta);
|
||||
// }
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Matrix4 SO4::Hat(const Vector6& xi) {
|
||||
// skew symmetric matrix X = xi^
|
||||
// Unlike Luca, makes upper-left the SO(3) subgroup.
|
||||
Matrix4 Y = Z_4x4;
|
||||
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);
|
||||
return Y - Y.transpose();
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector6 SO4::Vee(const Matrix4& X) {
|
||||
Vector6 xi;
|
||||
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);
|
||||
return xi;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
/* Exponential map, porting MATLAB implementation by Luca, which follows
|
||||
* "SOME REMARKS ON THE EXPONENTIAL MAP ON THE GROUPS SO(n) AND SE(n)" by
|
||||
* Ramona-Andreaa Rohan */
|
||||
template <>
|
||||
SO4 SO4::Expmap(const Vector6& xi, ChartJacobian H) {
|
||||
using namespace std;
|
||||
if (H) throw std::runtime_error("SO4::Expmap Jacobian");
|
||||
|
||||
// skew symmetric matrix X = xi^
|
||||
const Matrix4 X = Hat(xi);
|
||||
|
||||
// do eigen-decomposition
|
||||
auto eig = Eigen::EigenSolver<Matrix4>(X);
|
||||
Eigen::Vector4cd e = eig.eigenvalues();
|
||||
using std::abs;
|
||||
sort(e.data(), e.data() + 4, [](complex<double> a, complex<double> b) {
|
||||
return abs(a.imag()) > abs(b.imag());
|
||||
});
|
||||
|
||||
// Get a and b from eigenvalues +/i ai and +/- bi
|
||||
double a = e[0].imag(), b = e[2].imag();
|
||||
if (!e.real().isZero() || e[1].imag() != -a || e[3].imag() != -b) {
|
||||
throw runtime_error("SO4::Expmap: wrong eigenvalues.");
|
||||
}
|
||||
|
||||
// Build expX = exp(xi^)
|
||||
Matrix4 expX;
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
const auto X2 = X * X;
|
||||
const auto X3 = X2 * X;
|
||||
double a2 = a * a, a3 = a2 * a, b2 = b * b, b3 = b2 * b;
|
||||
if (a != 0 && b == 0) {
|
||||
double c2 = (1 - cos(a)) / a2, c3 = (a - sin(a)) / a3;
|
||||
return SO4(I_4x4 + X + c2 * X2 + c3 * X3);
|
||||
} else if (a == b && b != 0) {
|
||||
double sin_a = sin(a), cos_a = cos(a);
|
||||
double c0 = (a * sin_a + 2 * cos_a) / 2,
|
||||
c1 = (3 * sin_a - a * cos_a) / (2 * a), c2 = sin_a / (2 * a),
|
||||
c3 = (sin_a - a * cos_a) / (2 * a3);
|
||||
return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
|
||||
} else if (a != b) {
|
||||
double sin_a = sin(a), cos_a = cos(a);
|
||||
double sin_b = sin(b), cos_b = cos(b);
|
||||
double c0 = (b2 * cos_a - a2 * cos_b) / (b2 - a2),
|
||||
c1 = (b3 * sin_a - a3 * sin_b) / (a * b * (b2 - a2)),
|
||||
c2 = (cos_a - cos_b) / (b2 - a2),
|
||||
c3 = (b * sin_a - a * sin_b) / (a * b * (b2 - a2));
|
||||
return SO4(c0 * I_4x4 + c1 * X + c2 * X2 + c3 * X3);
|
||||
} else {
|
||||
return SO4();
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// local vectorize
|
||||
static SO4::VectorN2 vec4(const Matrix4& Q) {
|
||||
return Eigen::Map<const SO4::VectorN2>(Q.data());
|
||||
}
|
||||
|
||||
// so<4> generators
|
||||
static std::vector<Matrix4, Eigen::aligned_allocator<Matrix4> > G4(
|
||||
{SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
|
||||
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
|
||||
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});
|
||||
|
||||
// vectorized generators
|
||||
static const Eigen::Matrix<double, 16, 6> P4 =
|
||||
(Eigen::Matrix<double, 16, 6>() << vec4(G4[0]), vec4(G4[1]), vec4(G4[2]),
|
||||
vec4(G4[3]), vec4(G4[4]), vec4(G4[5]))
|
||||
.finished();
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Matrix6 SO4::AdjointMap() const {
|
||||
// Elaborate way of calculating the AdjointMap
|
||||
// TODO(frank): find a closed form solution. In SO(3) is just R :-/
|
||||
const Matrix4& Q = matrix_;
|
||||
const Matrix4 Qt = Q.transpose();
|
||||
Matrix6 A;
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
// Calculate column i of linear map for coeffcient of Gi
|
||||
A.col(i) = SO4::Vee(Q * G4[i] * Qt);
|
||||
}
|
||||
return A;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const {
|
||||
const Matrix& Q = matrix_;
|
||||
if (H) {
|
||||
// As Luca calculated, this is (I4 \oplus Q) * P4
|
||||
*H << Q * P4.block<4, 6>(0, 0), Q * P4.block<4, 6>(4, 0),
|
||||
Q * P4.block<4, 6>(8, 0), Q * P4.block<4, 6>(12, 0);
|
||||
}
|
||||
return gtsam::vec4(Q);
|
||||
}
|
||||
|
||||
///******************************************************************************
|
||||
template <>
|
||||
SO4 SO4::ChartAtOrigin::Retract(const Vector6& xi, ChartJacobian H) {
|
||||
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||
gttic(SO4_Retract);
|
||||
const Matrix4 X = Hat(xi / 2);
|
||||
return SO4((I_4x4 + X) * (I_4x4 - X).inverse());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
template <>
|
||||
Vector6 SO4::ChartAtOrigin::Local(const SO4& Q, ChartJacobian H) {
|
||||
if (H) throw std::runtime_error("SO4::ChartAtOrigin::Retract Jacobian");
|
||||
const Matrix4& R = Q.matrix();
|
||||
const Matrix4 X = (I_4x4 - R) * (I_4x4 + R).inverse();
|
||||
return -2 * Vee(X);
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Matrix3 topLeft(const SO4& Q, OptionalJacobian<9, 6> H) {
|
||||
const Matrix4& R = Q.matrix();
|
||||
const Matrix3 M = R.topLeftCorner<3, 3>();
|
||||
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;
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H) {
|
||||
const Matrix4& R = Q.matrix();
|
||||
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;
|
||||
}
|
||||
return M;
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
||||
} // end namespace gtsam
|
||||
|
|
@ -0,0 +1,110 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SO4.h
|
||||
* @brief 4*4 matrix representation of SO(4)
|
||||
* @author Frank Dellaert
|
||||
* @author Luca Carlone
|
||||
* @date March 2019
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/SOn.h>
|
||||
|
||||
#include <gtsam/base/Group.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
using SO4 = SO<4>;
|
||||
|
||||
// /// Random SO(4) element (no big claims about uniformity)
|
||||
// static SO4 Random(boost::mt19937 &rng);
|
||||
|
||||
// Below are all declarations of SO<4> specializations.
|
||||
// They are *defined* in SO4.cpp.
|
||||
|
||||
template <>
|
||||
Matrix4 SO4::Hat(const TangentVector &xi);
|
||||
|
||||
template <>
|
||||
Vector6 SO4::Vee(const Matrix4 &X);
|
||||
|
||||
template <>
|
||||
SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
|
||||
|
||||
template <>
|
||||
Matrix6 SO4::AdjointMap() const;
|
||||
|
||||
template <>
|
||||
SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
|
||||
|
||||
template <>
|
||||
SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
|
||||
|
||||
template <>
|
||||
Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
|
||||
|
||||
/**
|
||||
* Project to top-left 3*3 matrix. Note this is *not* in general \in SO(3).
|
||||
*/
|
||||
Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
|
||||
|
||||
/**
|
||||
* Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q)
|
||||
* -> S \in St(3,4).
|
||||
*/
|
||||
Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
|
||||
|
||||
/** Serialization function */
|
||||
template <class Archive>
|
||||
void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
|
||||
Matrix4 &M = Q.matrix_;
|
||||
ar &boost::serialization::make_nvp("Q11", M(0, 0));
|
||||
ar &boost::serialization::make_nvp("Q12", M(0, 1));
|
||||
ar &boost::serialization::make_nvp("Q13", M(0, 2));
|
||||
ar &boost::serialization::make_nvp("Q14", M(0, 3));
|
||||
|
||||
ar &boost::serialization::make_nvp("Q21", M(1, 0));
|
||||
ar &boost::serialization::make_nvp("Q22", M(1, 1));
|
||||
ar &boost::serialization::make_nvp("Q23", M(1, 2));
|
||||
ar &boost::serialization::make_nvp("Q24", M(1, 3));
|
||||
|
||||
ar &boost::serialization::make_nvp("Q31", M(2, 0));
|
||||
ar &boost::serialization::make_nvp("Q32", M(2, 1));
|
||||
ar &boost::serialization::make_nvp("Q33", M(2, 2));
|
||||
ar &boost::serialization::make_nvp("Q34", M(2, 3));
|
||||
|
||||
ar &boost::serialization::make_nvp("Q41", M(3, 0));
|
||||
ar &boost::serialization::make_nvp("Q42", M(3, 1));
|
||||
ar &boost::serialization::make_nvp("Q43", M(3, 2));
|
||||
ar &boost::serialization::make_nvp("Q44", M(3, 3));
|
||||
}
|
||||
|
||||
/*
|
||||
* Define the traits. internal::LieGroup provides both Lie group and Testable
|
||||
*/
|
||||
|
||||
template <>
|
||||
struct traits<SO4> : public internal::LieGroup<SO4> {};
|
||||
|
||||
template <>
|
||||
struct traits<const SO4> : public internal::LieGroup<SO4> {};
|
||||
|
||||
} // end namespace gtsam
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* @file SOn-inl.h
|
||||
* @brief Template implementations for SO(n)
|
||||
* @author Frank Dellaert
|
||||
* @date March 2019
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Implementation for N>5 just uses dynamic version
|
||||
template <int N>
|
||||
typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
|
||||
return SOn::Hat(xi);
|
||||
}
|
||||
|
||||
// Implementation for N>5 just uses dynamic version
|
||||
template <int N>
|
||||
typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
|
||||
return SOn::Vee(X);
|
||||
}
|
||||
|
||||
template <int N>
|
||||
SO<N> SO<N>::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) {
|
||||
if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented.");
|
||||
const Matrix X = Hat(xi / 2.0);
|
||||
size_t n = AmbientDim(xi.size());
|
||||
const auto I = Eigen::MatrixXd::Identity(n, n);
|
||||
// https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
|
||||
return SO((I + X) * (I - X).inverse());
|
||||
}
|
||||
|
||||
template <int N>
|
||||
typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
|
||||
ChartJacobian H) {
|
||||
if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented.");
|
||||
const size_t n = R.rows();
|
||||
const auto I = Eigen::MatrixXd::Identity(n, n);
|
||||
const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
|
||||
return -2 * Vee(X);
|
||||
}
|
||||
|
||||
template <int N>
|
||||
typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
|
||||
throw std::runtime_error(
|
||||
"SO<N>::AdjointMap only implemented for SO3 and SO4.");
|
||||
}
|
||||
|
||||
template <int N>
|
||||
SO<N> SO<N>::Expmap(const TangentVector& omega, ChartJacobian H) {
|
||||
throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
|
||||
}
|
||||
|
||||
template <int N>
|
||||
typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
|
||||
throw std::runtime_error("SO<N>::ExpmapDerivative only implemented for SO3.");
|
||||
}
|
||||
|
||||
template <int N>
|
||||
typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
|
||||
throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
|
||||
}
|
||||
|
||||
template <int N>
|
||||
typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
|
||||
throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
|
||||
}
|
||||
|
||||
template <int N>
|
||||
typename SO<N>::VectorN2 SO<N>::vec(
|
||||
OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
|
||||
const size_t n = rows();
|
||||
const size_t n2 = n * n;
|
||||
|
||||
// Vectorize
|
||||
VectorN2 X(n2);
|
||||
X << Eigen::Map<const Matrix>(matrix_.data(), n2, 1);
|
||||
|
||||
// If requested, calculate H as (I \oplus Q) * P,
|
||||
// where Q is the N*N rotation matrix, and P is calculated below.
|
||||
if (H) {
|
||||
// Calculate P matrix of vectorized generators
|
||||
// TODO(duy): Should we refactor this as the jacobian of Hat?
|
||||
const size_t d = dim();
|
||||
Matrix P(n2, d);
|
||||
for (size_t j = 0; j < d; j++) {
|
||||
const auto X = Hat(Eigen::VectorXd::Unit(d, j));
|
||||
P.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
|
||||
}
|
||||
H->resize(n2, d);
|
||||
for (size_t i = 0; i < n; i++) {
|
||||
H->block(i * n, 0, n, d) = matrix_ * P.block(i * n, 0, n, d);
|
||||
}
|
||||
}
|
||||
return X;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,98 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SOn.cpp
|
||||
* @brief Definitions of dynamic specializations of SO(n)
|
||||
* @author Frank Dellaert
|
||||
* @date March 2019
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/SOn.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template <>
|
||||
Matrix SOn::Hat(const Vector& xi) {
|
||||
size_t n = AmbientDim(xi.size());
|
||||
if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
|
||||
|
||||
Matrix X(n, n); // allocate space for n*n skew-symmetric matrix
|
||||
X.setZero();
|
||||
if (n == 2) {
|
||||
// Handle SO(2) case as recursion bottom
|
||||
assert(xi.size() == 1);
|
||||
X << 0, -xi(0), xi(0), 0;
|
||||
} else {
|
||||
// Recursively call SO(n-1) call for top-left block
|
||||
const size_t dmin = (n - 1) * (n - 2) / 2;
|
||||
X.topLeftCorner(n - 1, n - 1) = Hat(xi.tail(dmin));
|
||||
|
||||
// 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(j, n - 1) = -X(n - 1, j);
|
||||
sign = -sign;
|
||||
}
|
||||
}
|
||||
return X;
|
||||
}
|
||||
|
||||
template <>
|
||||
Vector SOn::Vee(const Matrix& X) {
|
||||
const size_t n = X.rows();
|
||||
if (n < 2) throw std::invalid_argument("SO<N>::Hat: n<2 not supported");
|
||||
|
||||
if (n == 2) {
|
||||
// Handle SO(2) case as recursion bottom
|
||||
Vector xi(1);
|
||||
xi(0) = X(1, 0);
|
||||
return xi;
|
||||
} else {
|
||||
// Calculate dimension and allocate space
|
||||
const size_t d = n * (n - 1) / 2;
|
||||
Vector xi(d);
|
||||
|
||||
// Fill first n-1 spots from last row of X
|
||||
double sign = 1.0;
|
||||
for (size_t i = 0; i < n - 1; i++) {
|
||||
const size_t j = n - 2 - i;
|
||||
xi(i) = sign * X(n - 1, j);
|
||||
sign = -sign;
|
||||
}
|
||||
|
||||
// Recursively call Vee to fill remainder of x
|
||||
const size_t dmin = (n - 1) * (n - 2) / 2;
|
||||
xi.tail(dmin) = Vee(X.topLeftCorner(n - 1, n - 1));
|
||||
return xi;
|
||||
}
|
||||
}
|
||||
|
||||
template <>
|
||||
SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
|
||||
DynamicJacobian H2) const {
|
||||
if (H1) *H1 = g.inverse().AdjointMap();
|
||||
if (H2) *H2 = SOn::IdentityJacobian(g.rows());
|
||||
return derived() * g;
|
||||
}
|
||||
|
||||
template <>
|
||||
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
|
||||
DynamicJacobian H2) const {
|
||||
SOn result = derived().inverse() * g;
|
||||
if (H1) *H1 = -result.inverse().AdjointMap();
|
||||
if (H2) *H2 = SOn::IdentityJacobian(g.rows());
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,332 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file SOn.h
|
||||
* @brief N*N matrix representation of SO(N). N can be Eigen::Dynamic
|
||||
* @author Frank Dellaert
|
||||
* @date March 2019
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
|
||||
#include <Eigen/Core>
|
||||
#include <boost/random.hpp>
|
||||
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace internal {
|
||||
/// Calculate dimensionality of SO<N> manifold, or return Dynamic if so
|
||||
constexpr int DimensionSO(int N) {
|
||||
return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
|
||||
}
|
||||
|
||||
// Calculate N^2 at compile time, or return Dynamic if so
|
||||
constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
|
||||
} // namespace internal
|
||||
|
||||
/**
|
||||
* Manifold of special orthogonal rotation matrices SO<N>.
|
||||
* Template paramater N can be a fixed integer or can be Eigen::Dynamic
|
||||
*/
|
||||
template <int N>
|
||||
class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
|
||||
public:
|
||||
enum { dimension = internal::DimensionSO(N) };
|
||||
using MatrixNN = Eigen::Matrix<double, N, N>;
|
||||
using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
|
||||
using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
|
||||
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
protected:
|
||||
MatrixNN matrix_; ///< Rotation matrix
|
||||
|
||||
// enable_if_t aliases, used to specialize constructors/methods, see
|
||||
// https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
|
||||
template <int N_>
|
||||
using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
|
||||
template <int N_>
|
||||
using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
|
||||
template <int N_>
|
||||
using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
|
||||
|
||||
public:
|
||||
/// @name Constructors
|
||||
/// @{
|
||||
|
||||
/// Construct SO<N> identity for N >= 2
|
||||
template <int N_ = N, typename = IsFixed<N_>>
|
||||
SO() : matrix_(MatrixNN::Identity()) {}
|
||||
|
||||
/// Construct SO<N> identity for N == Eigen::Dynamic
|
||||
template <int N_ = N, typename = IsDynamic<N_>>
|
||||
explicit SO(size_t n = 0) {
|
||||
// We allow for n=0 as the default constructor, needed for serialization,
|
||||
// wrappers etc.
|
||||
matrix_ = Eigen::MatrixXd::Identity(n, n);
|
||||
}
|
||||
|
||||
/// Constructor from Eigen Matrix, dynamic version
|
||||
template <typename Derived>
|
||||
explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
|
||||
|
||||
/// Named constructor from Eigen Matrix
|
||||
template <typename Derived>
|
||||
static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
|
||||
return SO(R);
|
||||
}
|
||||
|
||||
/// 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()) {}
|
||||
|
||||
/// Constructor from AngleAxisd
|
||||
template <int N_ = N, typename = IsSO3<N_>>
|
||||
explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
|
||||
|
||||
/// Constructor from axis and angle. Only defined for SO3
|
||||
static SO AxisAngle(const Vector3& axis, double theta);
|
||||
|
||||
/// Named constructor that finds SO(n) matrix closest to M in Frobenius norm,
|
||||
/// currently only defined for SO3.
|
||||
static SO ClosestTo(const MatrixNN& M);
|
||||
|
||||
/// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F),
|
||||
/// currently only defined for SO3.
|
||||
static SO ChordalMean(const std::vector<SO>& rotations);
|
||||
|
||||
/// Random SO(n) element (no big claims about uniformity)
|
||||
template <int N_ = N, typename = IsDynamic<N_>>
|
||||
static SO Random(boost::mt19937& rng, size_t n = 0) {
|
||||
if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
|
||||
// TODO(frank): This needs to be re-thought!
|
||||
static boost::uniform_real<double> randomAngle(-M_PI, M_PI);
|
||||
const size_t d = SO::Dimension(n);
|
||||
Vector xi(d);
|
||||
for (size_t j = 0; j < d; j++) {
|
||||
xi(j) = randomAngle(rng);
|
||||
}
|
||||
return SO::Retract(xi);
|
||||
}
|
||||
|
||||
/// Random SO(N) element (no big claims about uniformity)
|
||||
template <int N_ = N, typename = IsFixed<N_>>
|
||||
static SO Random(boost::mt19937& rng) {
|
||||
// By default, use dynamic implementation above. Specialized for SO(3).
|
||||
return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard methods
|
||||
/// @{
|
||||
|
||||
/// Return matrix
|
||||
const MatrixNN& matrix() const { return matrix_; }
|
||||
|
||||
size_t rows() const { return matrix_.rows(); }
|
||||
size_t cols() const { return matrix_.cols(); }
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
||||
void print(const std::string& s) const {
|
||||
std::cout << s << matrix_ << std::endl;
|
||||
}
|
||||
|
||||
bool equals(const SO& other, double tol) const {
|
||||
return equal_with_abs_tol(matrix_, other.matrix_, tol);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Group
|
||||
/// @{
|
||||
|
||||
/// Multiplication
|
||||
SO operator*(const SO& other) const {
|
||||
assert(dim() == other.dim());
|
||||
return SO(matrix_ * other.matrix_);
|
||||
}
|
||||
|
||||
/// SO<N> identity for N >= 2
|
||||
template <int N_ = N, typename = IsFixed<N_>>
|
||||
static SO identity() {
|
||||
return SO();
|
||||
}
|
||||
|
||||
/// SO<N> identity for N == Eigen::Dynamic
|
||||
template <int N_ = N, typename = IsDynamic<N_>>
|
||||
static SO identity(size_t n = 0) {
|
||||
return SO(n);
|
||||
}
|
||||
|
||||
/// inverse of a rotation = transpose
|
||||
SO inverse() const { return SO(matrix_.transpose()); }
|
||||
|
||||
/// @}
|
||||
/// @name Manifold
|
||||
/// @{
|
||||
|
||||
using TangentVector = Eigen::Matrix<double, dimension, 1>;
|
||||
using ChartJacobian = OptionalJacobian<dimension, dimension>;
|
||||
|
||||
/// Return compile-time dimensionality: fixed size N or Eigen::Dynamic
|
||||
static int Dim() { return dimension; }
|
||||
|
||||
// Calculate manifold dimensionality for SO(n).
|
||||
// Available as dimension or Dim() for fixed N.
|
||||
static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
|
||||
|
||||
// Calculate ambient dimension n from manifold dimensionality d.
|
||||
static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
|
||||
|
||||
// Calculate run-time dimensionality of manifold.
|
||||
// Available as dimension or Dim() for fixed N.
|
||||
size_t dim() const { return Dimension(matrix_.rows()); }
|
||||
|
||||
/**
|
||||
* Hat operator creates Lie algebra element corresponding to d-vector, where d
|
||||
* is the dimensionality of the manifold. This function is implemented
|
||||
* recursively, and the d-vector is assumed to laid out such that the last
|
||||
* element corresponds to so(2), the last 3 to so(3), the last 6 to so(4)
|
||||
* 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
|
||||
* This scheme behaves exactly as expected for SO(2) and SO(3).
|
||||
*/
|
||||
static MatrixNN Hat(const TangentVector& xi);
|
||||
|
||||
/**
|
||||
* Inverse of Hat. See note about xi element order in Hat.
|
||||
*/
|
||||
static TangentVector Vee(const MatrixNN& X);
|
||||
|
||||
// Chart at origin
|
||||
struct ChartAtOrigin {
|
||||
/**
|
||||
* Retract uses Cayley map. See note about xi element order in Hat.
|
||||
* Deafault implementation has no Jacobian implemented
|
||||
*/
|
||||
static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
|
||||
|
||||
/**
|
||||
* Inverse of Retract. See note about xi element order in Hat.
|
||||
*/
|
||||
static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
|
||||
};
|
||||
|
||||
// Return dynamic identity DxD Jacobian for given SO(n)
|
||||
template <int N_ = N, typename = IsDynamic<N_>>
|
||||
static MatrixDD IdentityJacobian(size_t n) {
|
||||
const size_t d = Dimension(n);
|
||||
return MatrixDD::Identity(d, d);
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
||||
/// Adjoint map
|
||||
MatrixDD AdjointMap() const;
|
||||
|
||||
/**
|
||||
* Exponential map at identity - create a rotation from canonical coordinates
|
||||
*/
|
||||
static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Expmap, currently only defined for SO3
|
||||
static MatrixDD ExpmapDerivative(const TangentVector& omega);
|
||||
|
||||
/**
|
||||
* Log map at identity - returns the canonical coordinates of this rotation
|
||||
*/
|
||||
static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
|
||||
|
||||
/// Derivative of Logmap, currently only defined for SO3
|
||||
static MatrixDD LogmapDerivative(const TangentVector& omega);
|
||||
|
||||
// inverse with optional derivative
|
||||
using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
|
||||
|
||||
/// @}
|
||||
/// @name Other methods
|
||||
/// @{
|
||||
|
||||
/**
|
||||
* Return vectorized rotation matrix in column order.
|
||||
* Will use dynamic matrices as intermediate results, but returns a fixed size
|
||||
* X and fixed-size Jacobian if dimension is known at compile time.
|
||||
* */
|
||||
VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
|
||||
boost::none) const;
|
||||
/// @}
|
||||
|
||||
template <class Archive>
|
||||
friend void serialize(Archive&, SO&, const unsigned int);
|
||||
friend class boost::serialization::access;
|
||||
friend class Rot3; // for serialize
|
||||
};
|
||||
|
||||
using SOn = SO<Eigen::Dynamic>;
|
||||
|
||||
/*
|
||||
* Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
|
||||
* The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
|
||||
* and implementation for other fixed N is in SOn-inl.h.
|
||||
*/
|
||||
|
||||
template <>
|
||||
Matrix SOn::Hat(const Vector& xi);
|
||||
|
||||
template <>
|
||||
Vector SOn::Vee(const Matrix& X);
|
||||
|
||||
/*
|
||||
* Specialize dynamic compose and between, because the derivative is unknowable
|
||||
* by the LieGroup implementations, who return a fixed-size matrix for H2.
|
||||
*/
|
||||
|
||||
using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
|
||||
|
||||
template <>
|
||||
SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
|
||||
DynamicJacobian H2) const;
|
||||
|
||||
template <>
|
||||
SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
|
||||
DynamicJacobian H2) const;
|
||||
|
||||
/*
|
||||
* Define the traits. internal::LieGroup provides both Lie group and Testable
|
||||
*/
|
||||
|
||||
template <int N>
|
||||
struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
|
||||
|
||||
template <int N>
|
||||
struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#include "SOn-inl.h"
|
||||
|
|
@ -645,6 +645,22 @@ TEST(Rot3 , ChartDerivatives) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, ClosestTo) {
|
||||
Matrix3 M;
|
||||
M << 0.79067393, 0.6051136, -0.0930814, //
|
||||
0.4155925, -0.64214347, -0.64324489, //
|
||||
-0.44948549, 0.47046326, -0.75917576;
|
||||
|
||||
Matrix expected(3, 3);
|
||||
expected << 0.790687, 0.605096, -0.0931312, //
|
||||
0.415746, -0.642355, -0.643844, //
|
||||
-0.449411, 0.47036, -0.759468;
|
||||
|
||||
auto actual = Rot3::ClosestTo(3*M);
|
||||
EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -10,19 +10,31 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testQuaternion.cpp
|
||||
* @file testSO3.cpp
|
||||
* @brief Unit tests for SO3, as a GTSAM-adapted Lie Group
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Identity) {
|
||||
const SO3 R;
|
||||
EXPECT_LONGS_EQUAL(3, R.rows());
|
||||
EXPECT_LONGS_EQUAL(3, SO3::dimension);
|
||||
EXPECT_LONGS_EQUAL(3, SO3::Dim());
|
||||
EXPECT_LONGS_EQUAL(3, R.dim());
|
||||
EXPECT_LONGS_EQUAL(3, traits<SO3>::GetDimension(R));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO3>));
|
||||
|
|
@ -31,14 +43,59 @@ TEST(SO3, Concept) {
|
|||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); }
|
||||
TEST(SO3, Constructors) {
|
||||
const Vector3 axis(0, 0, 1);
|
||||
const double angle = 2.5;
|
||||
SO3 Q(Eigen::AngleAxisd(angle, axis));
|
||||
SO3 R = SO3::AxisAngle(axis, angle);
|
||||
EXPECT(assert_equal(Q, R));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ClosestTo) {
|
||||
Matrix3 M;
|
||||
M << 0.79067393, 0.6051136, -0.0930814, //
|
||||
0.4155925, -0.64214347, -0.64324489, //
|
||||
-0.44948549, 0.47046326, -0.75917576;
|
||||
|
||||
Matrix expected(3, 3);
|
||||
expected << 0.790687, 0.605096, -0.0931312, //
|
||||
0.415746, -0.642355, -0.643844, //
|
||||
-0.449411, 0.47036, -0.759468;
|
||||
|
||||
auto actual = SO3::ClosestTo(3 * M);
|
||||
EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
SO3 id;
|
||||
Vector3 z_axis(0, 0, 1);
|
||||
Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3);
|
||||
SO3 R1(Eigen::AngleAxisd(0.1, z_axis));
|
||||
SO3 R2(Eigen::AngleAxisd(0.2, z_axis));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, ChordalMean) {
|
||||
std::vector<SO3> rotations = {R1, R1.inverse()};
|
||||
EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Hat) {
|
||||
// Check that Hat specialization is equal to dynamic version
|
||||
EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis)));
|
||||
EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2)));
|
||||
EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Vee) {
|
||||
// Check that Hat specialization is equal to dynamic version
|
||||
auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
|
||||
EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1)));
|
||||
EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2)));
|
||||
EXPECT(assert_equal(SO3::Vee(X3), SOn::Vee(X3)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Local) {
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
|
|
@ -50,7 +107,21 @@ TEST(SO3, Local) {
|
|||
TEST(SO3, Retract) {
|
||||
Vector3 v(0, 0, 0.1);
|
||||
SO3 actual = traits<SO3>::Retract(R1, v);
|
||||
EXPECT(actual.isApprox(R2));
|
||||
EXPECT(assert_equal(R2, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Logmap) {
|
||||
Vector3 expected(0, 0, 0.1);
|
||||
Vector3 actual = SO3::Logmap(R1.between(R2));
|
||||
EXPECT(assert_equal((Vector)expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, Expmap) {
|
||||
Vector3 v(0, 0, 0.1);
|
||||
SO3 actual = R1 * SO3::Expmap(v);
|
||||
EXPECT(assert_equal(R2, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
|
|
@ -83,7 +154,7 @@ TEST(SO3, ChartDerivatives) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SO3, Expmap) {
|
||||
TEST(SO3, ExpmapFunctor) {
|
||||
Vector axis = Vector3(0., 1., 0.); // rotation around Y
|
||||
double angle = 3.14 / 4.0;
|
||||
Matrix expected(3,3);
|
||||
|
|
@ -134,7 +205,7 @@ TEST(SO3, ExpmapDerivative) {
|
|||
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative2) {
|
||||
const Vector3 theta(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
|
|
@ -145,7 +216,7 @@ TEST(SO3, ExpmapDerivative2) {
|
|||
SO3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative3) {
|
||||
const Vector3 theta(10, 20, 30);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
|
|
@ -156,7 +227,7 @@ TEST(SO3, ExpmapDerivative3) {
|
|||
SO3::ExpmapDerivative(-theta)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative4) {
|
||||
// Iserles05an (Lie-group Methods) says:
|
||||
// scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t)
|
||||
|
|
@ -184,7 +255,7 @@ TEST(SO3, ExpmapDerivative4) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative5) {
|
||||
auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); };
|
||||
auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); };
|
||||
|
|
@ -200,7 +271,7 @@ TEST(SO3, ExpmapDerivative5) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ExpmapDerivative6) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
|
||||
|
|
@ -210,7 +281,7 @@ TEST(SO3, ExpmapDerivative6) {
|
|||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, LogmapDerivative) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const SO3 R = SO3::Expmap(thetahat); // some rotation
|
||||
|
|
@ -220,7 +291,7 @@ TEST(SO3, LogmapDerivative) {
|
|||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, JacobianLogmap) {
|
||||
const Vector3 thetahat(0.1, 0, 0.1);
|
||||
const SO3 R = SO3::Expmap(thetahat); // some rotation
|
||||
|
|
@ -231,7 +302,7 @@ TEST(SO3, JacobianLogmap) {
|
|||
EXPECT(assert_equal(Jexpected, Jactual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ApplyDexp) {
|
||||
Matrix aH1, aH2;
|
||||
for (bool nearZeroApprox : {true, false}) {
|
||||
|
|
@ -254,7 +325,7 @@ TEST(SO3, ApplyDexp) {
|
|||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//******************************************************************************
|
||||
TEST(SO3, ApplyInvDexp) {
|
||||
Matrix aH1, aH2;
|
||||
for (bool nearZeroApprox : {true, false}) {
|
||||
|
|
@ -278,6 +349,33 @@ TEST(SO3, ApplyInvDexp) {
|
|||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO3, vec) {
|
||||
const Vector9 expected = Eigen::Map<const Vector9>(R2.matrix().data());
|
||||
Matrix actualH;
|
||||
const Vector9 actual = R2.vec(actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
boost::function<Vector9(const SO3&)> f = [](const SO3& Q) { return Q.vec(); };
|
||||
const Matrix numericalH = numericalDerivative11(f, R2, 1e-5);
|
||||
CHECK(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Matrix, compose) {
|
||||
Matrix3 M;
|
||||
M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
|
||||
SO3 R = SO3::Expmap(Vector3(1, 2, 3));
|
||||
const Matrix3 expected = M * R.matrix();
|
||||
Matrix actualH;
|
||||
const Matrix3 actual = so3::compose(M, R, actualH);
|
||||
CHECK(assert_equal(expected, actual));
|
||||
boost::function<Matrix3(const Matrix3&)> f = [R](const Matrix3& M) {
|
||||
return so3::compose(M, R);
|
||||
};
|
||||
Matrix numericalH = numericalDerivative11(f, M, 1e-2);
|
||||
CHECK(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,206 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testSO4.cpp
|
||||
* @brief Unit tests for SO4, as a GTSAM-adapted Lie Group
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Identity) {
|
||||
const SO4 R;
|
||||
EXPECT_LONGS_EQUAL(4, R.rows());
|
||||
EXPECT_LONGS_EQUAL(6, SO4::dimension);
|
||||
EXPECT_LONGS_EQUAL(6, SO4::Dim());
|
||||
EXPECT_LONGS_EQUAL(6, R.dim());
|
||||
EXPECT_LONGS_EQUAL(6, traits<SO4>::GetDimension(R));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SO4>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SO4>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SO4>));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
SO4 id;
|
||||
Vector6 v1 = (Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
|
||||
SO4 Q1 = SO4::Expmap(v1);
|
||||
Vector6 v2 = (Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
|
||||
SO4 Q2 = SO4::Expmap(v2);
|
||||
Vector6 v3 = (Vector(6) << 1, 2, 3, 4, 5, 6).finished();
|
||||
SO4 Q3 = SO4::Expmap(v3);
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Random) {
|
||||
boost::mt19937 rng(42);
|
||||
auto Q = SO4::Random(rng);
|
||||
EXPECT_LONGS_EQUAL(4, Q.matrix().rows());
|
||||
}
|
||||
//******************************************************************************
|
||||
TEST(SO4, Expmap) {
|
||||
// If we do exponential map in SO(3) subgroup, topleft should be equal to R1.
|
||||
auto R1 = SO3::Expmap(v1.tail<3>()).matrix();
|
||||
EXPECT((Q1.matrix().topLeftCorner<3, 3>().isApprox(R1)));
|
||||
|
||||
// Same here
|
||||
auto R2 = SO3::Expmap(v2.tail<3>()).matrix();
|
||||
EXPECT((Q2.matrix().topLeftCorner<3, 3>().isApprox(R2)));
|
||||
|
||||
// Check commutative subgroups
|
||||
for (size_t i = 0; i < 6; i++) {
|
||||
Vector6 xi = Vector6::Zero();
|
||||
xi[i] = 2;
|
||||
SO4 Q1 = SO4::Expmap(xi);
|
||||
xi[i] = 3;
|
||||
SO4 Q2 = SO4::Expmap(xi);
|
||||
EXPECT(assert_equal(Q1 * Q2, Q2 * Q1));
|
||||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Hat) {
|
||||
// Check that Hat specialization is equal to dynamic version
|
||||
EXPECT(assert_equal(SO4::Hat(v1), SOn::Hat(v1)));
|
||||
EXPECT(assert_equal(SO4::Hat(v2), SOn::Hat(v2)));
|
||||
EXPECT(assert_equal(SO4::Hat(v3), SOn::Hat(v3)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Vee) {
|
||||
// Check that Hat specialization is equal to dynamic version
|
||||
auto X1 = SOn::Hat(v1), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3);
|
||||
EXPECT(assert_equal(SO4::Vee(X1), SOn::Vee(X1)));
|
||||
EXPECT(assert_equal(SO4::Vee(X2), SOn::Vee(X2)));
|
||||
EXPECT(assert_equal(SO4::Vee(X3), SOn::Vee(X3)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Retract) {
|
||||
// Check that Cayley yields the same as Expmap for small values
|
||||
EXPECT(assert_equal(id.retract(v1 / 100), SO4::Expmap(v1 / 100)));
|
||||
EXPECT(assert_equal(id.retract(v2 / 100), SO4::Expmap(v2 / 100)));
|
||||
|
||||
// Check that Cayley is identical to dynamic version
|
||||
EXPECT(assert_equal(SOn(id.retract(v1 / 100)), SOn(4).retract(v1 / 100)));
|
||||
EXPECT(assert_equal(SOn(id.retract(v2 / 100)), SOn(4).retract(v2 / 100)));
|
||||
|
||||
auto v = Vector6::Zero();
|
||||
SO4 actual = traits<SO4>::Retract(id, v);
|
||||
EXPECT(assert_equal(id, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Local) {
|
||||
// Check that Cayley is identical to dynamic version
|
||||
EXPECT(
|
||||
assert_equal(id.localCoordinates(Q1), SOn(4).localCoordinates(SOn(Q1))));
|
||||
EXPECT(
|
||||
assert_equal(id.localCoordinates(Q2), SOn(4).localCoordinates(SOn(Q2))));
|
||||
|
||||
auto v0 = Vector6::Zero();
|
||||
Vector6 actual = traits<SO4>::Local(id, id);
|
||||
EXPECT(assert_equal((Vector)v0, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, Invariants) {
|
||||
EXPECT(check_group_invariants(id, id));
|
||||
EXPECT(check_group_invariants(id, Q1));
|
||||
EXPECT(check_group_invariants(Q2, id));
|
||||
EXPECT(check_group_invariants(Q2, Q1));
|
||||
EXPECT(check_group_invariants(Q1, Q2));
|
||||
|
||||
EXPECT(check_manifold_invariants(id, id));
|
||||
EXPECT(check_manifold_invariants(id, Q1));
|
||||
EXPECT(check_manifold_invariants(Q2, id));
|
||||
EXPECT(check_manifold_invariants(Q2, Q1));
|
||||
EXPECT(check_manifold_invariants(Q1, Q2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, compose) {
|
||||
SO4 expected = Q1 * Q2;
|
||||
Matrix actualH1, actualH2;
|
||||
SO4 actual = Q1.compose(Q2, actualH1, actualH2);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
Matrix numericalH1 =
|
||||
numericalDerivative21(testing::compose<SO4>, Q1, Q2, 1e-2);
|
||||
EXPECT(assert_equal(numericalH1, actualH1));
|
||||
|
||||
Matrix numericalH2 =
|
||||
numericalDerivative22(testing::compose<SO4>, Q1, Q2, 1e-2);
|
||||
EXPECT(assert_equal(numericalH2, actualH2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, vec) {
|
||||
using Vector16 = SO4::VectorN2;
|
||||
const Vector16 expected = Eigen::Map<const Vector16>(Q2.matrix().data());
|
||||
Matrix actualH;
|
||||
const Vector16 actual = Q2.vec(actualH);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
boost::function<Vector16(const SO4&)> f = [](const SO4& Q) {
|
||||
return Q.vec();
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q2, 1e-5);
|
||||
EXPECT(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, topLeft) {
|
||||
const Matrix3 expected = Q3.matrix().topLeftCorner<3, 3>();
|
||||
Matrix actualH;
|
||||
const Matrix3 actual = topLeft(Q3, actualH);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
boost::function<Matrix3(const SO4&)> f = [](const SO4& Q3) {
|
||||
return topLeft(Q3);
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
EXPECT(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SO4, stiefel) {
|
||||
const Matrix43 expected = Q3.matrix().leftCols<3>();
|
||||
Matrix actualH;
|
||||
const Matrix43 actual = stiefel(Q3, actualH);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
boost::function<Matrix43(const SO4&)> f = [](const SO4& Q3) {
|
||||
return stiefel(Q3);
|
||||
};
|
||||
const Matrix numericalH = numericalDerivative11(f, Q3, 1e-5);
|
||||
EXPECT(assert_equal(numericalH, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -0,0 +1,170 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file testSOn.cpp
|
||||
* @brief Unit tests for dynamic SO(n) classes.
|
||||
* @author Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <gtsam/geometry/SO3.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
#include <gtsam/geometry/SOn.h>
|
||||
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/lieProxies.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/testLie.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/random.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <type_traits>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//******************************************************************************
|
||||
// Test dhynamic with n=0
|
||||
TEST(SOn, SO0) {
|
||||
const auto R = SOn(0);
|
||||
EXPECT_LONGS_EQUAL(0, R.rows());
|
||||
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));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, SO5) {
|
||||
const auto R = SOn(5);
|
||||
EXPECT_LONGS_EQUAL(5, R.rows());
|
||||
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));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, Concept) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<SOn>));
|
||||
BOOST_CONCEPT_ASSERT((IsManifold<SOn>));
|
||||
BOOST_CONCEPT_ASSERT((IsLieGroup<SOn>));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, CopyConstructor) {
|
||||
const auto R = SOn(5);
|
||||
const auto B(R);
|
||||
EXPECT_LONGS_EQUAL(5, B.rows());
|
||||
EXPECT_LONGS_EQUAL(10, B.dim());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, Values) {
|
||||
const auto R = SOn(5);
|
||||
Values values;
|
||||
const Key key(0);
|
||||
values.insert(key, R);
|
||||
const auto B = values.at<SOn>(key);
|
||||
EXPECT_LONGS_EQUAL(5, B.rows());
|
||||
EXPECT_LONGS_EQUAL(10, B.dim());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, Random) {
|
||||
static boost::mt19937 rng(42);
|
||||
EXPECT_LONGS_EQUAL(3, SOn::Random(rng, 3).rows());
|
||||
EXPECT_LONGS_EQUAL(4, SOn::Random(rng, 4).rows());
|
||||
EXPECT_LONGS_EQUAL(5, SOn::Random(rng, 5).rows());
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, HatVee) {
|
||||
Vector6 v;
|
||||
v << 1, 2, 3, 4, 5, 6;
|
||||
|
||||
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)));
|
||||
|
||||
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)));
|
||||
|
||||
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)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, RetractLocal) {
|
||||
Vector6 v1 = (Vector(6) << 0, 0, 0, 1, 0, 0).finished() / 10000;
|
||||
Vector6 v2 = (Vector(6) << 0, 0, 0, 1, 2, 3).finished() / 10000;
|
||||
Vector6 v3 = (Vector(6) << 3, 2, 1, 1, 2, 3).finished() / 10000;
|
||||
|
||||
// Check that Cayley yields the same as Expmap for small values
|
||||
SOn id(4);
|
||||
EXPECT(assert_equal(id.retract(v1), SOn(SO4::Expmap(v1))));
|
||||
EXPECT(assert_equal(id.retract(v2), SOn(SO4::Expmap(v2))));
|
||||
EXPECT(assert_equal(id.retract(v3), SOn(SO4::Expmap(v3))));
|
||||
|
||||
// Same for SO3:
|
||||
SOn I3(3);
|
||||
EXPECT(
|
||||
assert_equal(I3.retract(v1.tail<3>()), SOn(SO3::Expmap(v1.tail<3>()))));
|
||||
EXPECT(
|
||||
assert_equal(I3.retract(v2.tail<3>()), SOn(SO3::Expmap(v2.tail<3>()))));
|
||||
|
||||
// If we do expmap in SO(3) subgroup, topleft should be equal to R1.
|
||||
Matrix R1 = SO3().retract(v1.tail<3>()).matrix();
|
||||
SOn Q1 = SOn::Retract(v1);
|
||||
CHECK(assert_equal(R1, Q1.matrix().block(0, 0, 3, 3), 1e-7));
|
||||
CHECK(assert_equal(v1, SOn::ChartAtOrigin::Local(Q1), 1e-7));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(SOn, vec) {
|
||||
Vector10 v;
|
||||
v << 0, 0, 0, 0, 1, 2, 3, 4, 5, 6;
|
||||
SOn Q = SOn::ChartAtOrigin::Retract(v);
|
||||
Matrix actualH;
|
||||
const Vector actual = Q.vec(actualH);
|
||||
boost::function<Vector(const SOn&)> h = [](const SOn& Q) { return Q.vec(); };
|
||||
const Matrix H = numericalDerivative11<Vector, SOn, 10>(h, Q, 1e-5);
|
||||
CHECK(assert_equal(H, actualH));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
//******************************************************************************
|
||||
|
|
@ -86,6 +86,8 @@ namespace gtsam {
|
|||
FastVector<derived_ptr> children;
|
||||
int problemSize_;
|
||||
|
||||
bool is_root = false;
|
||||
|
||||
/// Fill the elimination result produced during elimination. Here this just stores the
|
||||
/// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique
|
||||
/// to also cache the remaining factor.
|
||||
|
|
@ -165,8 +167,14 @@ namespace gtsam {
|
|||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
if(!parent_.lock()) {
|
||||
is_root = true;
|
||||
}
|
||||
ar & BOOST_SERIALIZATION_NVP(is_root);
|
||||
ar & BOOST_SERIALIZATION_NVP(conditional_);
|
||||
ar & BOOST_SERIALIZATION_NVP(parent_);
|
||||
if (!is_root) { // TODO(fan): Workaround for boost/serialization #119
|
||||
ar & BOOST_SERIALIZATION_NVP(parent_);
|
||||
}
|
||||
ar & BOOST_SERIALIZATION_NVP(children);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,8 @@
|
|||
#include <gtsam/inference/VariableSlots.h>
|
||||
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/serialization/version.hpp>
|
||||
#include <boost/serialization/split_member.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -406,13 +408,41 @@ namespace gtsam {
|
|||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
|
||||
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar & BOOST_SERIALIZATION_NVP(Ab_);
|
||||
ar & BOOST_SERIALIZATION_NVP(model_);
|
||||
void save(ARCHIVE & ar, const unsigned int version) const {
|
||||
// TODO(fan): This is a hack for Boost < 1.66
|
||||
// We really need to introduce proper versioning in the archives
|
||||
// As otherwise this will not read objects serialized by older
|
||||
// versions of GTSAM
|
||||
ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar << BOOST_SERIALIZATION_NVP(Ab_);
|
||||
bool model_null = false;
|
||||
if(model_.get() == nullptr) {
|
||||
model_null = true;
|
||||
ar << boost::serialization::make_nvp("model_null", model_null);
|
||||
} else {
|
||||
ar << boost::serialization::make_nvp("model_null", model_null);
|
||||
ar << BOOST_SERIALIZATION_NVP(model_);
|
||||
}
|
||||
}
|
||||
}; // JacobianFactor
|
||||
|
||||
template<class ARCHIVE>
|
||||
void load(ARCHIVE & ar, const unsigned int version) {
|
||||
// invoke serialization of the base class
|
||||
ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||
ar >> BOOST_SERIALIZATION_NVP(Ab_);
|
||||
if (version < 1) {
|
||||
ar >> BOOST_SERIALIZATION_NVP(model_);
|
||||
} else {
|
||||
bool model_null;
|
||||
ar >> BOOST_SERIALIZATION_NVP(model_null);
|
||||
if (!model_null) {
|
||||
ar >> BOOST_SERIALIZATION_NVP(model_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
BOOST_SERIALIZATION_SPLIT_MEMBER()
|
||||
}; // JacobianFactor
|
||||
/// traits
|
||||
template<>
|
||||
struct traits<JacobianFactor> : public Testable<JacobianFactor> {
|
||||
|
|
@ -420,6 +450,8 @@ struct traits<JacobianFactor> : public Testable<JacobianFactor> {
|
|||
|
||||
} // \ namespace gtsam
|
||||
|
||||
BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
|
||||
|
||||
#include <gtsam/linear/JacobianFactor-inl.h>
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -25,7 +25,6 @@ using namespace boost::assign;
|
|||
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
#include <gtsam/linear/GaussianJunctionTree.h>
|
||||
#include <gtsam/linear/GaussianBayesTree.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
|
|
|
|||
|
|
@ -68,30 +68,30 @@ Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
|
|||
Matrix3 w_tangent_H_theta, invH;
|
||||
const Vector3 w_tangent = // angular velocity mapped back to tangent space
|
||||
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
|
||||
const SO3 R = local.expmap();
|
||||
const Rot3 R(local.expmap());
|
||||
const Vector3 a_nav = R * a_body;
|
||||
const double dt22 = 0.5 * dt * dt;
|
||||
|
||||
Vector9 preintegratedPlus;
|
||||
preintegratedPlus << // new preintegrated vector:
|
||||
theta + w_tangent * dt, // theta
|
||||
position + velocity * dt + a_nav * dt22, // position
|
||||
velocity + a_nav * dt; // velocity
|
||||
preintegratedPlus << // new preintegrated vector:
|
||||
theta + w_tangent * dt, // theta
|
||||
position + velocity * dt + a_nav * dt22, // position
|
||||
velocity + a_nav * dt; // velocity
|
||||
|
||||
if (A) {
|
||||
// Exact derivative of R*a with respect to theta:
|
||||
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
|
||||
const Matrix3 a_nav_H_theta = R.matrix() * skewSymmetric(-a_body) * local.dexp();
|
||||
|
||||
A->setIdentity();
|
||||
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
|
||||
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
|
||||
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
|
||||
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
|
||||
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
|
||||
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
|
||||
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
|
||||
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
|
||||
}
|
||||
if (B) {
|
||||
B->block<3, 3>(0, 0) = Z_3x3;
|
||||
B->block<3, 3>(3, 0) = R * dt22;
|
||||
B->block<3, 3>(6, 0) = R * dt;
|
||||
B->block<3, 3>(3, 0) = R.matrix() * dt22;
|
||||
B->block<3, 3>(6, 0) = R.matrix() * dt;
|
||||
}
|
||||
if (C) {
|
||||
C->block<3, 3>(0, 0) = invH * dt;
|
||||
|
|
|
|||
|
|
@ -75,27 +75,15 @@ Values InitializePose3::normalizeRelaxedRotations(
|
|||
const VectorValues& relaxedRot3) {
|
||||
gttic(InitializePose3_computeOrientationsChordal);
|
||||
|
||||
Matrix ppm = Z_3x3; // plus plus minus
|
||||
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
|
||||
|
||||
Values validRot3;
|
||||
for(const auto& it: relaxedRot3) {
|
||||
Key key = it.first;
|
||||
if (key != kAnchorKey) {
|
||||
Matrix3 R;
|
||||
R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
|
||||
Matrix3 M;
|
||||
M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
|
||||
|
||||
// ClosestTo finds rotation matrix closest to H in Frobenius sense
|
||||
// Rot3 initRot = Rot3::ClosestTo(M.transpose());
|
||||
|
||||
Matrix U, V; Vector s;
|
||||
svd(R.transpose(), U, s, V);
|
||||
Matrix3 normalizedRotMat = U * V.transpose();
|
||||
|
||||
if(normalizedRotMat.determinant() < 0)
|
||||
normalizedRotMat = U * ppm * V.transpose();
|
||||
|
||||
Rot3 initRot = Rot3(normalizedRotMat);
|
||||
Rot3 initRot = Rot3::ClosestTo(M.transpose());
|
||||
validRot3.insert(key, initRot);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,78 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file KarcherMeanFactor.cpp
|
||||
* @author Frank Dellaert
|
||||
* @date March 2019
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template <class T, class ALLOC>
|
||||
T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
|
||||
// Cost function C(R) = \sum PriorFactor(R_i)::error(R)
|
||||
// No closed form solution.
|
||||
NonlinearFactorGraph graph;
|
||||
static const Key kKey(0);
|
||||
for (const auto& R : rotations) {
|
||||
graph.emplace_shared<PriorFactor<T> >(kKey, R);
|
||||
}
|
||||
Values initial;
|
||||
initial.insert<T>(kKey, T());
|
||||
auto result = GaussNewtonOptimizer(graph, initial).optimize();
|
||||
return result.at<T>(kKey);
|
||||
}
|
||||
|
||||
template <class T,
|
||||
typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
|
||||
T FindKarcherMean(const std::vector<T>& rotations) {
|
||||
return FindKarcherMeanImpl(rotations);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
|
||||
return FindKarcherMeanImpl(rotations);
|
||||
}
|
||||
|
||||
template <class T>
|
||||
T FindKarcherMean(std::initializer_list<T>&& rotations) {
|
||||
return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
|
||||
}
|
||||
|
||||
template <class T>
|
||||
template <typename CONTAINER>
|
||||
KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER& keys, int d)
|
||||
: NonlinearFactor(keys) {
|
||||
if (d <= 0) {
|
||||
throw std::invalid_argument(
|
||||
"KarcherMeanFactor needs dimension for dynamic types.");
|
||||
}
|
||||
// Create the constant Jacobian made of D*D identity matrices,
|
||||
// where D is the dimensionality of the manifold.
|
||||
const auto I = Eigen::MatrixXd::Identity(d, d);
|
||||
std::map<Key, Matrix> terms;
|
||||
for (Key j : keys) {
|
||||
terms[j] = I;
|
||||
}
|
||||
jacobian_ =
|
||||
boost::make_shared<JacobianFactor>(terms, Eigen::VectorXd::Zero(d));
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,73 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file KarcherMeanFactor.h
|
||||
* @author Frank Dellaert
|
||||
* @date March 2019
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
/**
|
||||
* Optimize for the Karcher mean, minimizing the geodesic distance to each of
|
||||
* the given rotations, by constructing a factor graph out of simple
|
||||
* PriorFactors.
|
||||
*/
|
||||
template <class T>
|
||||
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations);
|
||||
|
||||
template <class T>
|
||||
T FindKarcherMean(std::initializer_list<T>&& rotations);
|
||||
|
||||
/**
|
||||
* The KarcherMeanFactor creates a constraint on all SO(n) variables with
|
||||
* given keys that the Karcher mean (see above) will stay the same. Note the
|
||||
* mean itself is irrelevant to the constraint and is not a parameter: the
|
||||
* constraint is implemented as enforcing that the sum of local updates is
|
||||
* equal to zero, hence creating a rank-dim constraint. Note it is implemented as
|
||||
* a soft constraint, as typically it is used to fix a gauge freedom.
|
||||
* */
|
||||
template <class T>
|
||||
class KarcherMeanFactor : public NonlinearFactor {
|
||||
/// Constant Jacobian made of d*d identity matrices
|
||||
boost::shared_ptr<JacobianFactor> jacobian_;
|
||||
|
||||
enum {D = traits<T>::dimension};
|
||||
|
||||
public:
|
||||
/// Construct from given keys.
|
||||
template <typename CONTAINER>
|
||||
KarcherMeanFactor(const CONTAINER& keys, int d=D);
|
||||
|
||||
/// Destructor
|
||||
virtual ~KarcherMeanFactor() {}
|
||||
|
||||
/// Calculate the error of the factor: always zero
|
||||
double error(const Values& c) const override { return 0; }
|
||||
|
||||
/// get the dimension of the factor (number of rows on linearization)
|
||||
size_t dim() const override { return D; }
|
||||
|
||||
/// linearize to a GaussianFactor
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
|
||||
return jacobian_;
|
||||
}
|
||||
};
|
||||
// \KarcherMeanFactor
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,111 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/*
|
||||
* @file testKarcherMeanFactor.cpp
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/SO4.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor-inl.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Rot3 version
|
||||
/* ************************************************************************* */
|
||||
static const Rot3 R = Rot3::Expmap(Vector3(0.1, 0, 0));
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check that optimizing for Karcher mean (which minimizes Between distance)
|
||||
// gets correct result.
|
||||
TEST(KarcherMean, FindRot3) {
|
||||
std::vector<Rot3> rotations = {R, R.inverse()};
|
||||
Rot3 expected;
|
||||
auto actual = FindKarcherMean(rotations);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Check that the InnerConstraint factor leaves the mean unchanged.
|
||||
TEST(KarcherMean, FactorRot3) {
|
||||
// Make a graph with two variables, one between, and one InnerConstraint
|
||||
// The optimal result should satisfy the between, while moving the other
|
||||
// variable to make the mean the same as before.
|
||||
// Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
|
||||
// R*R*R, i.e. geodesic length is 3 rather than 2.
|
||||
NonlinearFactorGraph graph;
|
||||
graph.emplace_shared<BetweenFactor<Rot3>>(1, 2, R * R * R);
|
||||
std::vector<Key> keys{1, 2};
|
||||
graph.emplace_shared<KarcherMeanFactor<Rot3>>(keys);
|
||||
|
||||
Values initial;
|
||||
initial.insert<Rot3>(1, R.inverse());
|
||||
initial.insert<Rot3>(2, R);
|
||||
const auto expected = FindKarcherMean<Rot3>({R, R.inverse()});
|
||||
|
||||
auto result = GaussNewtonOptimizer(graph, initial).optimize();
|
||||
const auto actual =
|
||||
FindKarcherMean<Rot3>({result.at<Rot3>(1), result.at<Rot3>(2)});
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
EXPECT(
|
||||
assert_equal(R * R * R, result.at<Rot3>(1).between(result.at<Rot3>(2))));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// SO(4) version
|
||||
/* ************************************************************************* */
|
||||
static const SO4 Q = SO4::Expmap((Vector6() << 1, 2, 3, 4, 5, 6).finished());
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(KarcherMean, FindSO4) {
|
||||
std::vector<SO4, Eigen::aligned_allocator<SO4>> rotations = {Q, Q.inverse()};
|
||||
auto expected = SO4(); //::ChordalMean(rotations);
|
||||
auto actual = FindKarcherMean(rotations);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(KarcherMean, FactorSO4) {
|
||||
NonlinearFactorGraph graph;
|
||||
graph.emplace_shared<BetweenFactor<SO4>>(1, 2, Q * Q * Q);
|
||||
std::vector<Key> keys{1, 2};
|
||||
graph.emplace_shared<KarcherMeanFactor<SO4>>(keys);
|
||||
|
||||
Values initial;
|
||||
initial.insert<SO4>(1, Q.inverse());
|
||||
initial.insert<SO4>(2, Q);
|
||||
|
||||
std::vector<SO4, Eigen::aligned_allocator<SO4> > rotations = {Q, Q.inverse()};
|
||||
const auto expected = FindKarcherMean<SO4>(rotations);
|
||||
|
||||
auto result = GaussNewtonOptimizer(graph, initial).optimize();
|
||||
const auto actual =
|
||||
FindKarcherMean<SO4>({result.at<SO4>(1), result.at<SO4>(2)});
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
EXPECT(assert_equal((Matrix)(Q * Q * Q).matrix(),
|
||||
result.at<SO4>(1).between(result.at<SO4>(2)).matrix()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -23,33 +23,33 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#define TEST(TITLE,STATEMENT) \
|
||||
cout << endl << TITLE << endl;\
|
||||
timeLog = clock();\
|
||||
for(int i = 0; i < n; i++)\
|
||||
STATEMENT;\
|
||||
timeLog2 = clock();\
|
||||
seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;\
|
||||
cout << seconds << " seconds" << endl;\
|
||||
cout << ((double)n/seconds) << " calls/second" << endl;
|
||||
#define TEST(TITLE, STATEMENT) \
|
||||
cout << endl << TITLE << endl; \
|
||||
timeLog = clock(); \
|
||||
for (int i = 0; i < n; i++) STATEMENT; \
|
||||
timeLog2 = clock(); \
|
||||
seconds = static_cast<double>(timeLog2 - timeLog) / CLOCKS_PER_SEC; \
|
||||
cout << 1000 * seconds << " milliseconds" << endl; \
|
||||
cout << (1e9 * seconds / static_cast<double>(n)) << " nanosecs/call" << endl;
|
||||
|
||||
int main()
|
||||
{
|
||||
int n = 100000; long timeLog, timeLog2; double seconds;
|
||||
int main() {
|
||||
int n = 100000;
|
||||
clock_t timeLog, timeLog2;
|
||||
double seconds;
|
||||
// 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;
|
||||
Vector v = (Vector(3) << x, y, z).finished();
|
||||
Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v);
|
||||
|
||||
TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001))
|
||||
TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v, 0.001))
|
||||
TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v))
|
||||
TEST("Expmap", R*Rot3::Expmap(v))
|
||||
TEST("Expmap", R * Rot3::Expmap(v))
|
||||
TEST("Retract", R.retract(v))
|
||||
TEST("Logmap", Rot3::Logmap(R.between(R2)))
|
||||
TEST("localCoordinates", R.localCoordinates(R2))
|
||||
TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x))
|
||||
TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z))
|
||||
TEST("Slow rotation matrix", Rot3::Rz(z) * Rot3::Ry(y) * Rot3::Rx(x))
|
||||
TEST("Fast Rotation matrix", Rot3::RzRyRx(x, y, z))
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue