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

Merged in feature/DirectProduct (pull request #143) 

Added machinery to create product types
release/4.3a0
Frank Dellaert 2015-06-04 14:45:25 -07:00
parent 7882d74263 6a67ea86af
commit 874c4796c9
32 changed files with 1041 additions and 399 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.cproject
View File

@ -1035,6 +1035,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCyclic.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCyclic.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -3190,6 +3198,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGroup.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testGroup.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -3414,6 +3430,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLie.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testLie.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

130
gtsam/base/Group.h
View File

@ -13,16 +13,20 @@
* @file Group.h
*
* @brief Concept check class for variable types with Group properties
* @date Nov 5, 2011
* @date November, 2011
* @author Alex Cunningham
* @author Frank Dellaert
*/
#pragma once
#include <gtsam/base/Testable.h>
#include <boost/concept_check.hpp>
#include <boost/concept/requires.hpp>
#include <boost/type_traits/is_base_of.hpp>
#include <boost/static_assert.hpp>
#include <utility>
namespace gtsam {
@ -83,21 +87,119 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
&& traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
}
/// Macro to add group traits, additive flavor
#define GTSAM_ADDITIVE_GROUP(GROUP) \
typedef additive_group_tag group_flavor; \
static GROUP Compose(const GROUP &g, const GROUP & h) { return g + h;} \
static GROUP Between(const GROUP &g, const GROUP & h) { return h - g;} \
static GROUP Inverse(const GROUP &g) { return -g;}
namespace internal {
/// Macro to add group traits, multiplicative flavor
#define GTSAM_MULTIPLICATIVE_GROUP(GROUP) \
typedef additive_group_tag group_flavor; \
static GROUP Compose(const GROUP &g, const GROUP & h) { return g * h;} \
static GROUP Between(const GROUP &g, const GROUP & h) { return g.inverse() * h;} \
static GROUP Inverse(const GROUP &g) { return g.inverse();}
/// A helper class that implements the traits interface for multiplicative groups.
/// Assumes existence of identity, operator* and inverse method
template<class Class>
struct MultiplicativeGroupTraits {
typedef group_tag structure_category;
typedef multiplicative_group_tag group_flavor;
static Class Identity() { return Class::identity(); }
static Class Compose(const Class &g, const Class & h) { return g * h;}
static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
static Class Inverse(const Class &g) { return g.inverse();}
};
} // \ namespace gtsam
/// Both multiplicative group traits and Testable
template<class Class>
struct MultiplicativeGroup : MultiplicativeGroupTraits<Class>, Testable<Class> {};
/// A helper class that implements the traits interface for additive groups.
/// Assumes existence of identity and three additive operators
template<class Class>
struct AdditiveGroupTraits {
typedef group_tag structure_category;
typedef additive_group_tag group_flavor;
static Class Identity() { return Class::identity(); }
static Class Compose(const Class &g, const Class & h) { return g + h;}
static Class Between(const Class &g, const Class & h) { return h - g;}
static Class Inverse(const Class &g) { return -g;}
};
/// Both additive group traits and Testable
template<class Class>
struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
} // namespace internal
/// compose multiple times
template<typename G>
BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
compose_pow(const G& g, size_t n) {
if (n == 0) return traits<G>::Identity();
else if (n == 1) return g;
else return traits<G>::Compose(compose_pow(g, n - 1), g);
}
/// Template to construct the direct product of two arbitrary groups
/// Assumes nothing except group structure and Testable from G and H
template<typename G, typename H>
class DirectProduct: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>));
BOOST_CONCEPT_ASSERT((IsGroup<H>));
public:
/// Default constructor yields identity
DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
// Construct from two subgroup elements
DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
// identity
static DirectProduct identity() { return DirectProduct(); }
DirectProduct operator*(const DirectProduct& other) const {
return DirectProduct(traits<G>::Compose(this->first, other.first),
traits<H>::Compose(this->second, other.second));
}
DirectProduct inverse() const {
return DirectProduct(this->first.inverse(), this->second.inverse());
}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename G, typename H>
struct traits<DirectProduct<G, H> > :
internal::MultiplicativeGroupTraits<DirectProduct<G, H> > {};
/// Template to construct the direct sum of two additive groups
/// Assumes existence of three additive operators for both groups
template<typename G, typename H>
class DirectSum: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive
BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive
const G& g() const { return this->first; }
const H& h() const { return this->second;}
public:
/// Default constructor yields identity
DirectSum():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
// Construct from two subgroup elements
DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
// identity
static DirectSum identity() { return DirectSum(); }
DirectSum operator+(const DirectSum& other) const {
return DirectSum(g()+other.g(), h()+other.h());
}
DirectSum operator-(const DirectSum& other) const {
return DirectSum(g()-other.g(), h()-other.h());
}
DirectSum operator-() const {
return DirectSum(- g(), - h());
}
};
// Define direct sums to be a model of the Additive Group concept
template<typename G, typename H>
struct traits<DirectSum<G, H> > :
internal::AdditiveGroupTraits<DirectSum<G, H> > {};
} // namespace gtsam
/**
* Macros for using the IsGroup

11
gtsam/base/Lie.h
View File

@ -136,8 +136,10 @@ namespace internal {
/// A helper class that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<Class> : public internal::LieGroupTraits<Class> {};
/// Assumes existence of: identity, dimension, localCoordinates, retract,
/// and additionally Logmap, Expmap, compose, between, and inverse
template<class Class>
struct LieGroupTraits : Testable<Class> {
struct LieGroupTraits {
typedef lie_group_tag structure_category;
/// @name Group
@ -167,12 +169,10 @@ struct LieGroupTraits : Testable<Class> {
ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
return origin.retract(v, Horigin, Hv);
}
/// @}
/// @name Lie Group
/// @{
static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
return Class::Logmap(m, Hm);
}
@ -195,11 +195,12 @@ struct LieGroupTraits : Testable<Class> {
ChartJacobian H = boost::none) {
return m.inverse(H);
}
/// @}
};
/// Both LieGroupTraits and Testable
template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
} // \ namepsace internal
/**

56
gtsam/base/Manifold.h
View File

@ -46,7 +46,7 @@ struct manifold_tag {};
* There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be
* inverse operations. The new notion of a Chart guarantees that.
* inverse operations.
*
*/
@ -90,9 +90,9 @@ struct ManifoldImpl<Class, Eigen::Dynamic> {
/// A helper that implements the traits interface for GTSAM manifolds.
/// To use this for your class type, define:
/// template<> struct traits<Class> : public Manifold<Class> { };
/// template<> struct traits<Class> : public internal::ManifoldTraits<Class> { };
template<class Class>
struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
struct ManifoldTraits: ManifoldImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
@ -116,6 +116,9 @@ struct Manifold: Testable<Class>, ManifoldImpl<Class, Class::dimension> {
}
};
/// Both ManifoldTraits and Testable
template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
} // \ namespace internal
/// Check invariants for Manifold type
@ -165,6 +168,53 @@ struct FixedDimension {
"FixedDimension instantiated for dymanically-sized type.");
};
/// Helper class to construct the product manifold of two other manifolds, M1 and M2
/// Assumes nothing except manifold structure from M1 and M2
template<typename M1, typename M2>
class ProductManifold: public std::pair<M1, M2> {
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
BOOST_CONCEPT_ASSERT((IsManifold<M2>));
protected:
enum { dimension1 = traits<M1>::dimension };
enum { dimension2 = traits<M2>::dimension };
public:
enum { dimension = dimension1 + dimension2 };
inline static size_t Dim() { return dimension;}
inline size_t dim() const { return dimension;}
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
/// Default constructor yields identity
ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
// Construct from two original manifold values
ProductManifold(const M1& m1, const M2& m2):std::pair<M1,M2>(m1,m2) {}
/// Retract delta to manifold
ProductManifold retract(const TangentVector& xi) const {
M1 m1 = traits<M1>::Retract(this->first, xi.template head<dimension1>());
M2 m2 = traits<M2>::Retract(this->second, xi.template tail<dimension2>());
return ProductManifold(m1,m2);
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const ProductManifold& other) const {
typename traits<M1>::TangentVector v1 = traits<M1>::Local(this->first, other.first);
typename traits<M2>::TangentVector v2 = traits<M2>::Local(this->second, other.second);
TangentVector v;
v << v1, v2;
return v;
}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename M1, typename M2>
struct traits<ProductManifold<M1, M2> > : internal::Manifold<ProductManifold<M1, M2> > {
};
} // \ namespace gtsam
///**

197
gtsam/base/ProductLieGroup.h Normal file
View File

@ -0,0 +1,197 @@
/* ----------------------------------------------------------------------------
* 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
* -------------------------------1------------------------------------------- */
/**
* @file ProductLieGroup.h
* @date May, 2015
* @author Frank Dellaert
* @brief Group product of two Lie Groups
*/
#pragma once
#include <gtsam/base/Lie.h>
#include <utility> // pair
namespace gtsam {
/// Template to construct the product Lie group of two other Lie groups
/// Assumes Lie group structure for G and H
template<typename G, typename H>
class ProductLieGroup: public std::pair<G, H> {
BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
typedef std::pair<G, H> Base;
protected:
enum {dimension1 = traits<G>::dimension};
enum {dimension2 = traits<H>::dimension};
public:
/// Default constructor yields identity
ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
// Construct from two subgroup elements
ProductLieGroup(const G& g, const H& h):Base(g,h) {}
// Construct from base
ProductLieGroup(const Base& base):Base(base) {}
/// @name Group
/// @{
typedef multiplicative_group_tag group_flavor;
static ProductLieGroup identity() {return ProductLieGroup();}
ProductLieGroup operator*(const ProductLieGroup& other) const {
return ProductLieGroup(traits<G>::Compose(this->first,other.first),
traits<H>::Compose(this->second,other.second));
}
ProductLieGroup inverse() const {
return ProductLieGroup(this->first.inverse(), this->second.inverse());
}
ProductLieGroup compose(const ProductLieGroup& g) const {
return (*this) * g;
}
ProductLieGroup between(const ProductLieGroup& g) const {
return this->inverse() * g;
}
/// @}
/// @name Manifold
/// @{
enum {dimension = dimension1 + dimension2};
inline static size_t Dim() {return dimension;}
inline size_t dim() const {return dimension;}
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static ProductLieGroup Retract(const TangentVector& v) {
return ProductLieGroup::ChartAtOrigin::Retract(v);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g) {
return ProductLieGroup::ChartAtOrigin::Local(g);
}
ProductLieGroup retract(const TangentVector& v) const {
return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
}
TangentVector localCoordinates(const ProductLieGroup& g) const {
return ProductLieGroup::ChartAtOrigin::Local(between(g));
}
/// @}
/// @name Lie Group
/// @{
protected:
typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
public:
ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
if (H1) {
H1->setZero();
H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
if (H2) *H2 = Jacobian::Identity();
return ProductLieGroup(g,h);
}
ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
ChartJacobian H2 = boost::none) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
if (H1) {
H1->setZero();
H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
if (H2) *H2 = Jacobian::Identity();
return ProductLieGroup(g,h);
}
ProductLieGroup inverse(ChartJacobian D) const {
Jacobian1 D_g_first; Jacobian2 D_h_second;
G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
if (D) {
D->setZero();
D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
}
return ProductLieGroup(g,h);
}
static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
if (Hv) throw std::runtime_error("ProductLieGroup::Expmap derivatives not implemented yet");
G g = traits<G>::Expmap(v.template head<dimension1>());
H h = traits<H>::Expmap(v.template tail<dimension2>());
return ProductLieGroup(g,h);
}
static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
if (Hp) throw std::runtime_error("ProductLieGroup::Logmap derivatives not implemented yet");
typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first);
typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second);
TangentVector v;
v << v1, v2;
return v;
}
struct ChartAtOrigin {
static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
return Logmap(m, Hm);
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
return Expmap(v, Hv);
}
};
ProductLieGroup expmap(const TangentVector& v) const {
return compose(ProductLieGroup::Expmap(v));
}
TangentVector logmap(const ProductLieGroup& g) const {
return ProductLieGroup::Logmap(between(g));
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Local(g,H1);
}
ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
ProductLieGroup h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
return h;
}
TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ProductLieGroup h = between(g,H1,H2);
Jacobian D_v_h;
TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
return v;
}
/// @}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename G, typename H>
struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
ProductLieGroup<G, H> > {
};
} // namespace gtsam

15
gtsam/base/VectorSpace.h
View File

@ -20,7 +20,7 @@ template<typename T> struct traits;
namespace internal {
/// VectorSpace Implementation for Fixed sizes
/// VectorSpaceTraits Implementation for Fixed sizes
template<class Class, int N>
struct VectorSpaceImpl {
@ -83,7 +83,7 @@ struct VectorSpaceImpl {
/// @}
};
/// VectorSpace implementation for dynamic types.
/// VectorSpaceTraits implementation for dynamic types.
template<class Class>
struct VectorSpaceImpl<Class,Eigen::Dynamic> {
@ -159,11 +159,11 @@ struct VectorSpaceImpl<Class,Eigen::Dynamic> {
/// @}
};
/// A helper that implements the traits interface for GTSAM lie groups.
/// A helper that implements the traits interface for GTSAM vector space types.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public VectorSpace<Type> { };
/// template<> struct traits<Type> : public VectorSpaceTraits<Type> { };
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
typedef vector_space_tag structure_category;
@ -178,9 +178,12 @@ struct VectorSpace: Testable<Class>, VectorSpaceImpl<Class, Class::dimension> {
enum { dimension = Class::dimension};
typedef Class ManifoldType;
/// @}
};
/// Both VectorSpaceTRaits and Testable
template<class Class>
struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
/// A helper that implements the traits interface for GTSAM lie groups.
/// To use this for your gtsam type, define:
/// template<> struct traits<Type> : public ScalarTraits<Type> { };

143
gtsam/base/tests/testGroup.cpp Normal file
View File

@ -0,0 +1,143 @@
/* ----------------------------------------------------------------------------
* 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 testGroup.cpp
* @brief Unit tests for groups
* @author Frank Dellaert
**/
#include <gtsam/base/Group.h>
#include <gtsam/base/Testable.h>
#include <Eigen/Core>
#include <iostream>
#include <boost/foreach.hpp>
namespace gtsam {
/// Symmetric group
template<int N>
class Symmetric: private Eigen::PermutationMatrix<N> {
Symmetric(const Eigen::PermutationMatrix<N>& P) :
Eigen::PermutationMatrix<N>(P) {
}
public:
static Symmetric identity() { return Symmetric(); }
Symmetric() {
Eigen::PermutationMatrix<N>::setIdentity();
}
static Symmetric Transposition(int i, int j) {
Symmetric g;
return g.applyTranspositionOnTheRight(i, j);
}
Symmetric operator*(const Symmetric& other) const {
return Eigen::PermutationMatrix<N>::operator*(other);
}
bool operator==(const Symmetric& other) const {
for (size_t i = 0; i < N; i++)
if (this->indices()[i] != other.indices()[i])
return false;
return true;
}
Symmetric inverse() const {
return Eigen::PermutationMatrix<N>(Eigen::PermutationMatrix<N>::inverse());
}
friend std::ostream &operator<<(std::ostream &os, const Symmetric& m) {
for (size_t i = 0; i < N; i++)
os << m.indices()[i] << " ";
return os;
}
void print(const std::string& s = "") const {
std::cout << s << *this << std::endl;
}
bool equals(const Symmetric<N>& other, double tol = 0) const {
return this->indices() == other.indices();
}
};
/// Define permutation group traits to be a model of the Multiplicative Group concept
template<int N>
struct traits<Symmetric<N> > : internal::MultiplicativeGroupTraits<Symmetric<N> >,
Testable<Symmetric<N> > {
};
} // namespace gtsam
#include <gtsam/base/Testable.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//******************************************************************************
typedef Symmetric<2> S2;
TEST(Group, S2) {
S2 e, s1 = S2::Transposition(0, 1);
BOOST_CONCEPT_ASSERT((IsGroup<S2>));
EXPECT(check_group_invariants(e, s1));
}
//******************************************************************************
typedef Symmetric<3> S3;
TEST(Group, S3) {
S3 e, s1 = S3::Transposition(0, 1), s2 = S3::Transposition(1, 2);
BOOST_CONCEPT_ASSERT((IsGroup<S3>));
EXPECT(check_group_invariants(e, s1));
EXPECT(assert_equal(s1, s1 * e));
EXPECT(assert_equal(s1, e * s1));
EXPECT(assert_equal(e, s1 * s1));
S3 g = s1 * s2; // 1 2 0
EXPECT(assert_equal(s1, g * s2));
EXPECT(assert_equal(e, compose_pow(g, 0)));
EXPECT(assert_equal(g, compose_pow(g, 1)));
EXPECT(assert_equal(e, compose_pow(g, 3))); // g is generator of Z3 subgroup
}
//******************************************************************************
// The direct product of S2=Z2 and S3 is the symmetry group of a hexagon,
// i.e., the dihedral group of order 12 (denoted Dih6 because 6-sided polygon)
typedef DirectProduct<S2, S3> Dih6;
std::ostream &operator<<(std::ostream &os, const Dih6& m) {
os << "( " << m.first << ", " << m.second << ")";
return os;
}
// Provide traits with Testable
namespace gtsam {
template<>
struct traits<Dih6> : internal::MultiplicativeGroupTraits<Dih6> {
static void Print(const Dih6& m, const string& s = "") {
cout << s << m << endl;
}
static bool Equals(const Dih6& m1, const Dih6& m2, double tol = 1e-8) {
return m1 == m2;
}
};
} // namespace gtsam
TEST(Group, Dih6) {
Dih6 e, g(S2::Transposition(0, 1),
S3::Transposition(0, 1) * S3::Transposition(1, 2));
BOOST_CONCEPT_ASSERT((IsGroup<Dih6>));
EXPECT(check_group_invariants(e, g));
EXPECT(assert_equal(e, compose_pow(g, 0)));
EXPECT(assert_equal(g, compose_pow(g, 1)));
EXPECT(assert_equal(e, compose_pow(g, 6))); // g is generator of Z6 subgroup
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

4
gtsam/base/tests/testLieScalar.cpp
View File

@ -37,8 +37,8 @@ TEST(LieScalar , Concept) {
//******************************************************************************
TEST(LieScalar , Invariants) {
LieScalar lie1(2), lie2(3);
check_group_invariants(lie1, lie2);
check_manifold_invariants(lie1, lie2);
CHECK(check_group_invariants(lie1, lie2));
CHECK(check_manifold_invariants(lie1, lie2));
}
/* ************************************************************************* */

29
gtsam/geometry/Cyclic.h
View File

@ -16,10 +16,8 @@
**/
#include <gtsam/base/Group.h>
#include <cstddef>
#include <string>
#include <iostream>
#include <assert.h>
#include <gtsam/base/Testable.h>
#include <iostream> // for cout :-(
namespace gtsam {
@ -33,10 +31,11 @@ public:
i_(i) {
assert(i < N);
}
/// Idenity element
static Cyclic Identity() {
return Cyclic(0);
/// Default constructor yields identity
Cyclic():i_(0) {
}
static Cyclic identity() { return Cyclic();}
/// Cast to size_t
operator size_t() const {
return i_;
@ -63,20 +62,10 @@ public:
}
};
/// Define cyclic group traits to be a model of the Group concept
/// Define cyclic group to be a model of the Additive Group concept
template<size_t N>
struct traits<Cyclic<N> > {
typedef group_tag structure_category;GTSAM_ADDITIVE_GROUP(Cyclic<N>)
static Cyclic<N> Identity() {
return Cyclic<N>::Identity();
}
static bool Equals(const Cyclic<N>& a, const Cyclic<N>& b,
double tol = 1e-9) {
return a.equals(b, tol);
}
static void Print(const Cyclic<N>& c, const std::string &s = "") {
c.print(s);
}
struct traits<Cyclic<N> > : internal::AdditiveGroupTraits<Cyclic<N> >, //
Testable<Cyclic<N> > {
};
} // \namespace gtsam

54
gtsam/geometry/EssentialMatrix.cpp
View File

@ -13,64 +13,46 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
OptionalJacobian<5, 6> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
const Rot3& aRb = aPb.rotation();
const Point3& aTb = aPb.translation();
if (!H) {
// just make a direction out of translation and create E
Unit3 direction(_1T2_);
return EssentialMatrix(_1R2_, direction);
Unit3 direction(aTb);
return EssentialMatrix(aRb, direction);
} else {
// Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Unit3::FromPoint3
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
*H << I_3x3, Z_3x3, //
Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction);
Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
return EssentialMatrix(aRb, direction);
}
}
/* ************************************************************************* */
void EssentialMatrix::print(const string& s) const {
cout << s;
aRb_.print("R:\n");
aTb_.print("d: ");
}
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
assert(xi.size() == 5);
Vector3 omega(sub(xi, 0, 3));
Vector2 z(sub(xi, 3, 5));
Rot3 R = aRb_.retract(omega);
Unit3 t = aTb_.retract(z);
return EssentialMatrix(R, t);
}
/* ************************************************************************* */
Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
Vector5 v;
v << aRb_.localCoordinates(other.aRb_),
aTb_.localCoordinates(other.aTb_);
return v;
rotation().print("R:\n");
direction().print("d: ");
}
/* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3());
Pose3 pose(rotation(), direction().point3());
Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix35 H;
H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
*DE = H;
}
return q;
@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
Rot3 c1Rc2 = rotation().conjugate(cRb);
if (!HE && !HR) {
// Rotate translation direction and return
Unit3 c1Tc2 = cRb * aTb_;
Unit3 c1Tc2 = cRb * direction();
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix23 D_c1Tc2_cRb; // 2*3
Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE)
*HE << cRb.matrix(), Matrix32::Zero(), //
Matrix23::Zero(), D_c1Tc2_aTb;
@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
if (H) {
// See math.lyx
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
* direction().basis();
*H << HR, HD;
}
return dot(vA, E_ * vB);

55
gtsam/geometry/EssentialMatrix.h
View File

@ -10,6 +10,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/Manifold.h>
#include <iosfwd>
namespace gtsam {
@ -20,17 +21,18 @@ namespace gtsam {
* but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
*/
class GTSAM_EXPORT EssentialMatrix {
class GTSAM_EXPORT EssentialMatrix : private ProductManifold<Rot3, Unit3> {
private:
Rot3 aRb_; ///< Rotation between a and b
Unit3 aTb_; ///< translation direction from a to b
typedef ProductManifold<Rot3, Unit3> Base;
Matrix3 E_; ///< Essential matrix
public:
/// Construct from Base
EssentialMatrix(const Base& base) :
Base(base), E_(direction().skew() * rotation().matrix()) {
}
enum { dimension = 5 };
public:
/// Static function to convert Point2 to homogeneous coordinates
static Vector3 Homogeneous(const Point2& p) {
@ -42,12 +44,12 @@ public:
/// Default constructor
EssentialMatrix() :
aTb_(1, 0, 0), E_(aTb_.skew()) {
Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
}
/// Construct from rotation and translation
EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
}
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
@ -72,7 +74,8 @@ public:
/// assert equality up to a tolerance
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
return rotation().equals(other.rotation(), tol)
&& direction().equals(other.direction(), tol);
}
/// @}
@ -80,22 +83,19 @@ public:
/// @name Manifold
/// @{
/// Dimensionality of tangent space = 5 DOF
inline static size_t Dim() {
return 5;
}
/// Return the dimensionality of the tangent space
size_t dim() const {
return 5;
}
using Base::dimension;
using Base::dim;
using Base::Dim;
/// Retract delta to manifold
EssentialMatrix retract(const Vector& xi) const;
EssentialMatrix retract(const TangentVector& v) const {
return Base::retract(v);
}
/// Compute the coordinates in the tangent space
Vector5 localCoordinates(const EssentialMatrix& other) const;
TangentVector localCoordinates(const EssentialMatrix& other) const {
return Base::localCoordinates(other);
}
/// @}
/// @name Essential matrix methods
@ -103,12 +103,12 @@ public:
/// Rotation
inline const Rot3& rotation() const {
return aRb_;
return this->first;
}
/// Direction
inline const Unit3& direction() const {
return aTb_;
return this->second;
}
/// Return 3*3 matrix representation
@ -118,12 +118,12 @@ public:
/// Return epipole in image_a , as Unit3 to allow for infinity
inline const Unit3& epipole_a() const {
return aTb_; // == direction()
return direction();
}
/// Return epipole in image_b, as Unit3 to allow for infinity
inline Unit3 epipole_b() const {
return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
return rotation().unrotate(direction());
}
/**
@ -180,8 +180,8 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(aRb_);
ar & BOOST_SERIALIZATION_NVP(aTb_);
ar & BOOST_SERIALIZATION_NVP(first);
ar & BOOST_SERIALIZATION_NVP(second);
ar & boost::serialization::make_nvp("E11", E_(0,0));
ar & boost::serialization::make_nvp("E12", E_(0,1));
@ -195,7 +195,6 @@ private:
}
/// @}
};
template<>

4
gtsam/geometry/Pose2.h
View File

@ -290,10 +290,10 @@ typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
template<>
struct traits<Pose2> : public internal::LieGroupTraits<Pose2> {};
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
template<>
struct traits<const Pose2> : public internal::LieGroupTraits<Pose2> {};
struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
} // namespace gtsam

4
gtsam/geometry/Pose3.h
View File

@ -324,10 +324,10 @@ GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
typedef std::vector<Pose3> Pose3Vector;
template<>
struct traits<Pose3> : public internal::LieGroupTraits<Pose3> {};
struct traits<Pose3> : public internal::LieGroup<Pose3> {};
template<>
struct traits<const Pose3> : public internal::LieGroupTraits<Pose3> {};
struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
} // namespace gtsam

4
gtsam/geometry/Rot2.h
View File

@ -208,9 +208,9 @@ namespace gtsam {
};
template<>
struct traits<Rot2> : public internal::LieGroupTraits<Rot2> {};
struct traits<Rot2> : public internal::LieGroup<Rot2> {};
template<>
struct traits<const Rot2> : public internal::LieGroupTraits<Rot2> {};
struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
} // gtsam

4
gtsam/geometry/Rot3.h
View File

@ -463,9 +463,9 @@ namespace gtsam {
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
template<>
struct traits<Rot3> : public internal::LieGroupTraits<Rot3> {};
struct traits<Rot3> : public internal::LieGroup<Rot3> {};
template<>
struct traits<const Rot3> : public internal::LieGroupTraits<Rot3> {};
struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
}

4
gtsam/geometry/SO3.h
View File

@ -129,11 +129,11 @@ public:
};
template<>
struct traits<SO3> : public internal::LieGroupTraits<SO3> {
struct traits<SO3> : public internal::LieGroup<SO3> {
};
template<>
struct traits<const SO3> : public internal::LieGroupTraits<SO3> {
struct traits<const SO3> : public internal::LieGroup<SO3> {
};
} // end namespace gtsam

106
gtsam/geometry/tests/testCyclic.cpp
View File

@ -22,52 +22,114 @@
using namespace std;
using namespace gtsam;
typedef Cyclic<3> G; // Let's use the cyclic group of order 3
typedef Cyclic<3> Z3; // Let's use the cyclic group of order 3
typedef Cyclic<2> Z2;
//******************************************************************************
TEST(Cyclic, Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<G>));
EXPECT_LONGS_EQUAL(0, traits<G>::Identity());
BOOST_CONCEPT_ASSERT((IsGroup<Z3>));
EXPECT_LONGS_EQUAL(0, traits<Z3>::Identity());
}
//******************************************************************************
TEST(Cyclic, Constructor) {
G g(0);
Z3 g(0);
}
//******************************************************************************
TEST(Cyclic, Compose) {
EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(0),G(2)));
EXPECT_LONGS_EQUAL(0, traits<Z3>::Compose(Z3(0),Z3(0)));
EXPECT_LONGS_EQUAL(1, traits<Z3>::Compose(Z3(0),Z3(1)));
EXPECT_LONGS_EQUAL(2, traits<Z3>::Compose(Z3(0),Z3(2)));
EXPECT_LONGS_EQUAL(2, traits<G>::Compose(G(2),G(0)));
EXPECT_LONGS_EQUAL(0, traits<G>::Compose(G(2),G(1)));
EXPECT_LONGS_EQUAL(1, traits<G>::Compose(G(2),G(2)));
EXPECT_LONGS_EQUAL(2, traits<Z3>::Compose(Z3(2),Z3(0)));
EXPECT_LONGS_EQUAL(0, traits<Z3>::Compose(Z3(2),Z3(1)));
EXPECT_LONGS_EQUAL(1, traits<Z3>::Compose(Z3(2),Z3(2)));
}
//******************************************************************************
TEST(Cyclic, Between) {
EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(0),G(0)));
EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(0),G(1)));
EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(0),G(2)));
EXPECT_LONGS_EQUAL(0, traits<Z3>::Between(Z3(0),Z3(0)));
EXPECT_LONGS_EQUAL(1, traits<Z3>::Between(Z3(0),Z3(1)));
EXPECT_LONGS_EQUAL(2, traits<Z3>::Between(Z3(0),Z3(2)));
EXPECT_LONGS_EQUAL(1, traits<G>::Between(G(2),G(0)));
EXPECT_LONGS_EQUAL(2, traits<G>::Between(G(2),G(1)));
EXPECT_LONGS_EQUAL(0, traits<G>::Between(G(2),G(2)));
EXPECT_LONGS_EQUAL(1, traits<Z3>::Between(Z3(2),Z3(0)));
EXPECT_LONGS_EQUAL(2, traits<Z3>::Between(Z3(2),Z3(1)));
EXPECT_LONGS_EQUAL(0, traits<Z3>::Between(Z3(2),Z3(2)));
}
//******************************************************************************
TEST(Cyclic, Ivnverse) {
EXPECT_LONGS_EQUAL(0, traits<G>::Inverse(G(0)));
EXPECT_LONGS_EQUAL(2, traits<G>::Inverse(G(1)));
EXPECT_LONGS_EQUAL(1, traits<G>::Inverse(G(2)));
TEST(Cyclic, Inverse) {
EXPECT_LONGS_EQUAL(0, traits<Z3>::Inverse(Z3(0)));
EXPECT_LONGS_EQUAL(2, traits<Z3>::Inverse(Z3(1)));
EXPECT_LONGS_EQUAL(1, traits<Z3>::Inverse(Z3(2)));
}
//******************************************************************************
TEST(Cyclic, Negation) {
EXPECT_LONGS_EQUAL(0, -Z3(0));
EXPECT_LONGS_EQUAL(2, -Z3(1));
EXPECT_LONGS_EQUAL(1, -Z3(2));
}
//******************************************************************************
TEST(Cyclic, Negation2) {
EXPECT_LONGS_EQUAL(0, -Z2(0));
EXPECT_LONGS_EQUAL(1, -Z2(1));
}
//******************************************************************************
TEST(Cyclic , Invariants) {
G g(2), h(1);
check_group_invariants(g,h);
Z3 g(2), h(1);
EXPECT(check_group_invariants(g,h));
}
//******************************************************************************
// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
// smallest non-cyclic group called the Klein four-group:
typedef DirectSum<Z2, Z2> K4;
namespace gtsam {
/// Define K4 to be a model of the Additive Group concept, and provide Testable
template<>
struct traits<K4> : internal::AdditiveGroupTraits<K4> {
static void Print(const K4& m, const string& s = "") {
cout << s << "(" << m.first << "," << m.second << ")" << endl;
}
static bool Equals(const K4& m1, const K4& m2, double tol = 1e-8) {
return m1 == m2;
}
};
} // namespace gtsam
TEST(Cyclic , DirectSum) {
// The Direct sum of Z2 and Z2 is *not* Cyclic<4>, but the
// smallest non-cyclic group called the Klein four-group:
BOOST_CONCEPT_ASSERT((IsGroup<K4>));
BOOST_CONCEPT_ASSERT((IsTestable<K4>));
// Refer to http://en.wikipedia.org/wiki/Klein_four-group
K4 e(0,0), a(0, 1), b(1, 0), c(1, 1);
EXPECT(assert_equal(a, - a));
EXPECT(assert_equal(b, - b));
EXPECT(assert_equal(c, - c));
EXPECT(assert_equal(a, a + e));
EXPECT(assert_equal(b, b + e));
EXPECT(assert_equal(c, c + e));
EXPECT(assert_equal(e, a + a));
EXPECT(assert_equal(e, b + b));
EXPECT(assert_equal(e, c + c));
EXPECT(assert_equal(c, a + b));
EXPECT(assert_equal(b, a + c));
EXPECT(assert_equal(a, b + c));
EXPECT(assert_equal(c, a - b));
EXPECT(assert_equal(a, b - c));
EXPECT(assert_equal(b, c - a));
EXPECT(check_group_invariants(a, b));
EXPECT(check_group_invariants(b, c));
EXPECT(check_group_invariants(c, a));
}
//******************************************************************************

23
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) {
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, FromPose3) {
EssentialMatrix expected(c1Rc2, Unit3(c1Tc2));
Pose3 pose(c1Rc2, c1Tc2);
EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
EXPECT(assert_equal(expected, actual));
}
//*******************************************************************************
TEST(EssentialMatrix, localCoordinates0) {
EssentialMatrix E;
@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) {
Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
EXPECT(assert_equal(zero(5), actual, 1e-8));
Vector d = zero(6);
d(4) += 1e-5;
Vector6 d;
d << 0.1, 0.2, 0.3, 0, 0, 0;
Vector actual2 = hx.localCoordinates(
EssentialMatrix::FromPose3(pose.retract(d)));
EXPECT(assert_equal(zero(5), actual2, 1e-8));
EXPECT(assert_equal(d.head(5), actual2, 1e-8));
}
//*************************************************************************
@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) {
EXPECT(assert_equal(expected, actual));
}
//*************************************************************************
TEST (EssentialMatrix, RoundTrip) {
Vector5 d;
d << 0.1, 0.2, 0.3, 0.4, 0.5;
EssentialMatrix e, hx = e.retract(d);
Vector actual = e.localCoordinates(hx);
EXPECT(assert_equal(d, actual, 1e-8));
}
//*************************************************************************
Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
return E.transform_to(point);

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -37,8 +37,8 @@ TEST(Double , Concept) {
//******************************************************************************
TEST(Double , Invariants) {
double p1(2), p2(5);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
EXPECT(check_group_invariants(p1, p2));
EXPECT(check_manifold_invariants(p1, p2));
}
//******************************************************************************
@ -51,8 +51,8 @@ TEST(Point2 , Concept) {
//******************************************************************************
TEST(Point2 , Invariants) {
Point2 p1(1, 2), p2(4, 5);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
EXPECT(check_group_invariants(p1, p2));
EXPECT(check_manifold_invariants(p1, p2));
}
/* ************************************************************************* */

4
gtsam/geometry/tests/testPoint3.cpp
View File

@ -36,8 +36,8 @@ TEST(Point3 , Concept) {
//******************************************************************************
TEST(Point3 , Invariants) {
Point3 p1(1, 2, 3), p2(4, 5, 6);
check_group_invariants(p1, p2);
check_manifold_invariants(p1, p2);
EXPECT(check_group_invariants(p1, p2));
EXPECT(check_manifold_invariants(p1, p2));
}
/* ************************************************************************* */

16
gtsam/geometry/tests/testPose2.cpp
View File

@ -775,15 +775,15 @@ Pose2 T2(M_PI / 2.0, Point2(0.0, 2.0));
TEST(Pose2 , Invariants) {
Pose2 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T1));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T1));
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T1));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T1));
}

16
gtsam/geometry/tests/testPose3.cpp
View File

@ -760,15 +760,15 @@ TEST( Pose3, stream)
TEST(Pose3 , Invariants) {
Pose3 id;
check_group_invariants(id,id);
check_group_invariants(id,T3);
check_group_invariants(T2,id);
check_group_invariants(T2,T3);
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T3));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T3));
check_manifold_invariants(id,id);
check_manifold_invariants(id,T3);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T3);
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T3));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T3));
}
//******************************************************************************

16
gtsam/geometry/tests/testQuaternion.cpp
View File

@ -107,15 +107,15 @@ TEST(Quaternion , Inverse) {
//******************************************************************************
TEST(Quaternion , Invariants) {
check_group_invariants(id, id);
check_group_invariants(id, R1);
check_group_invariants(R2, id);
check_group_invariants(R2, R1);
EXPECT(check_group_invariants(id, id));
EXPECT(check_group_invariants(id, R1));
EXPECT(check_group_invariants(R2, id));
EXPECT(check_group_invariants(R2, R1));
check_manifold_invariants(id, id);
check_manifold_invariants(id, R1);
check_manifold_invariants(R2, id);
check_manifold_invariants(R2, R1);
EXPECT(check_manifold_invariants(id, id));
EXPECT(check_manifold_invariants(id, R1));
EXPECT(check_manifold_invariants(R2, id));
EXPECT(check_manifold_invariants(R2, R1));
}
//******************************************************************************

16
gtsam/geometry/tests/testRot2.cpp
View File

@ -163,15 +163,15 @@ Rot2 T2(0.2);
TEST(Rot2 , Invariants) {
Rot2 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T1));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T1));
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T1));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T1));
}

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

@ -659,17 +659,17 @@ Rot3 T2(Rot3::rodriguez(Vector3(0, 1, 0), 2));
TEST(Rot3 , Invariants) {
Rot3 id;
check_group_invariants(id,id);
check_group_invariants(id,T1);
check_group_invariants(T2,id);
check_group_invariants(T2,T1);
check_group_invariants(T1,T2);
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,T1));
EXPECT(check_group_invariants(T2,id));
EXPECT(check_group_invariants(T2,T1));
EXPECT(check_group_invariants(T1,T2));
check_manifold_invariants(id,id);
check_manifold_invariants(id,T1);
check_manifold_invariants(T2,id);
check_manifold_invariants(T2,T1);
check_manifold_invariants(T1,T2);
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,T1));
EXPECT(check_manifold_invariants(T2,id));
EXPECT(check_manifold_invariants(T2,T1));
EXPECT(check_manifold_invariants(T1,T2));
}
//******************************************************************************

16
gtsam/geometry/tests/testSO3.cpp
View File

@ -57,15 +57,15 @@ TEST(SO3 , Retract) {
//******************************************************************************
TEST(SO3 , Invariants) {
check_group_invariants(id,id);
check_group_invariants(id,R1);
check_group_invariants(R2,id);
check_group_invariants(R2,R1);
EXPECT(check_group_invariants(id,id));
EXPECT(check_group_invariants(id,R1));
EXPECT(check_group_invariants(R2,id));
EXPECT(check_group_invariants(R2,R1));
check_manifold_invariants(id,id);
check_manifold_invariants(id,R1);
check_manifold_invariants(R2,id);
check_manifold_invariants(R2,R1);
EXPECT(check_manifold_invariants(id,id));
EXPECT(check_manifold_invariants(id,R1));
EXPECT(check_manifold_invariants(R2,id));
EXPECT(check_manifold_invariants(R2,R1));
}
//******************************************************************************

157
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -3,18 +3,15 @@
* @author Alex Cunningham
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/Vector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam_unstable/dynamics/PoseRTV.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Vector.h>
namespace gtsam {
using namespace std;
static const Vector g = delta(3, 2, 9.81);
static const Vector kGravity = delta(3, 2, 9.81);
/* ************************************************************************* */
double bound(double a, double min, double max) {
@ -24,28 +21,30 @@ double bound(double a, double min, double max) {
}
/* ************************************************************************* */
PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
double vx, double vy, double vz)
: Rt_(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)), v_(vx, vy, vz) {}
PoseRTV::PoseRTV(double roll, double pitch, double yaw, double x, double y,
double z, double vx, double vy, double vz) :
Base(Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z)),
Velocity3(vx, vy, vz)) {
}
/* ************************************************************************* */
PoseRTV::PoseRTV(const Vector& rtv)
: Rt_(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))), v_(rtv.tail(3))
{
PoseRTV::PoseRTV(const Vector& rtv) :
Base(Pose3(Rot3::RzRyRx(rtv.head(3)), Point3(rtv.segment(3, 3))),
Velocity3(rtv.tail(3))) {
}
/* ************************************************************************* */
Vector PoseRTV::vector() const {
Vector rtv(9);
rtv.head(3) = Rt_.rotation().xyz();
rtv.segment(3,3) = Rt_.translation().vector();
rtv.tail(3) = v_.vector();
rtv.head(3) = rotation().xyz();
rtv.segment(3,3) = translation().vector();
rtv.tail(3) = velocity().vector();
return rtv;
}
/* ************************************************************************* */
bool PoseRTV::equals(const PoseRTV& other, double tol) const {
return Rt_.equals(other.Rt_, tol) && v_.equals(other.v_, tol);
return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
}
/* ************************************************************************* */
@ -53,73 +52,7 @@ void PoseRTV::print(const string& s) const {
cout << s << ":" << endl;
gtsam::print((Vector)R().xyz(), " R:rpy");
t().print(" T");
v_.print(" V");
}
/* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector9& v, ChartJacobian H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
Pose3 newPose = Pose3::Expmap(v.head<6>());
Velocity3 newVel = Velocity3(v.tail<3>());
return PoseRTV(newPose, newVel);
}
/* ************************************************************************* */
Vector9 PoseRTV::Logmap(const PoseRTV& p, ChartJacobian H) {
if (H) CONCEPT_NOT_IMPLEMENTED;
Vector6 Lx = Pose3::Logmap(p.Rt_);
Vector3 Lv = p.v_.vector();
return (Vector9() << Lx, Lv).finished();
}
/* ************************************************************************* */
PoseRTV PoseRTV::retract(const Vector& v,
ChartJacobian Horigin,
ChartJacobian Hv) const {
if (Horigin || Hv) CONCEPT_NOT_IMPLEMENTED;
assert(v.size() == 9);
// First order approximation
Pose3 newPose = Rt_.retract(sub(v, 0, 6));
Velocity3 newVel = v_ + Rt_.rotation() * Point3(sub(v, 6, 9));
return PoseRTV(newPose, newVel);
}
/* ************************************************************************* */
Vector PoseRTV::localCoordinates(const PoseRTV& p1,
ChartJacobian Horigin,
ChartJacobian Hp) const {
if (Horigin || Hp) CONCEPT_NOT_IMPLEMENTED;
const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation
Vector6 poseLogmap = x0.localCoordinates(x1);
Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
return (Vector(9) << poseLogmap, lv).finished();
}
/* ************************************************************************* */
PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
PoseRTV PoseRTV::inverse(ChartJacobian H1) const {
if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
return PoseRTV(Rt_.inverse(), - v_);
}
/* ************************************************************************* */
PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
PoseRTV PoseRTV::compose(const PoseRTV& p, ChartJacobian H1,
ChartJacobian H2) const {
if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
return PoseRTV(Rt_.compose(p.Rt_), v_+p.v_);
}
/* ************************************************************************* */
PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
PoseRTV PoseRTV::between(const PoseRTV& p,
ChartJacobian H1,
ChartJacobian H2) const {
if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5);
return inverse().compose(p);
velocity().print(" V");
}
/* ************************************************************************* */
@ -187,7 +120,7 @@ PoseRTV PoseRTV::generalDynamics(
Rot3 r2 = rotation().retract(gyro * dt);
// Integrate Velocity Equations
Velocity3 v2 = v_ + (Velocity3(dt * (r2.matrix() * accel + g)));
Velocity3 v2 = velocity() + Velocity3(dt * (r2.matrix() * accel + kGravity));
// Integrate Position Equations
Point3 t2 = translationIntegration(r2, v2, dt);
@ -205,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// acceleration
Vector3 accel = (v2-v1).vector() / dt;
imu.head<3>() = r2.transpose() * (accel - g);
imu.head<3>() = r2.transpose() * (accel - kGravity);
// rotation rates
// just using euler angles based on matlab code
@ -232,54 +165,40 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub
}
/* ************************************************************************* */
double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
double PoseRTV::range(const PoseRTV& other,
OptionalJacobian<1,9> H1, OptionalJacobian<1,9> H2) const {
if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
return t().distance(other.t());
Matrix36 D_t1_pose, D_t2_other;
const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
Matrix13 D_d_t1, D_d_t2;
double d = t1.distance(t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
return d;
}
/* ************************************************************************* */
PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
return global.transformed_from(transform);
}
PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
OptionalJacobian<9, 6> Dtrans) const {
// Note that we rotate the velocity
Matrix DVr, DTt;
Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);
if (!Dglobal && !Dtrans)
return PoseRTV(trans.compose(pose()), newvel);
// Pose3 transform is just compose
Matrix DTc, DGc;
Pose3 newpose = trans.compose(pose(), DTc, DGc);
Matrix6 D_newpose_trans, D_newpose_pose;
Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose);
// Note that we rotate the velocity
Matrix3 D_newvel_R, D_newvel_v;
Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v);
if (Dglobal) {
*Dglobal = zeros(9,9);
insertSub(*Dglobal, DGc, 0, 0);
// Rotate velocity
insertSub(*Dglobal, eye(3,3), 6, 6); // FIXME: should this actually be an identity matrix?
Dglobal->setZero();
Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
}
if (Dtrans) {
*Dtrans = numericalDerivative22(transformed_from_, *this, trans, 1e-8);
//
// *Dtrans = zeros(9,6);
// // directly affecting the pose
// insertSub(*Dtrans, DTc, 0, 0); // correct in tests
//
// // rotating the velocity
// Matrix vRhat = skewSymmetric(-v_.x(), -v_.y(), -v_.z());
// trans.rotation().print("Transform rotation");
// gtsam::print(vRhat, "vRhat");
// gtsam::print(DVr, "DVr");
// // FIXME: find analytic derivative
//// insertSub(*Dtrans, vRhat, 6, 0); // works if PoseRTV.rotation() = I
//// insertSub(*Dtrans, trans.rotation().matrix() * vRhat, 6, 0); // FAIL: both tests fail
Dtrans->setZero();
Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
}
return PoseRTV(newpose, newvel);
}

112
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -7,8 +7,8 @@
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/ProductLieGroup.h>
namespace gtsam {
@ -19,29 +19,30 @@ typedef Point3 Velocity3;
* Robot state for use with IMU measurements
* - contains translation, translational velocity and rotation
*/
class GTSAM_UNSTABLE_EXPORT PoseRTV {
class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
protected:
Pose3 Rt_;
Velocity3 v_;
typedef ProductLieGroup<Pose3,Velocity3> Base;
typedef OptionalJacobian<9, 9> ChartJacobian;
public:
enum { dimension = 9 };
// constructors - with partial versions
PoseRTV() {}
PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel)
: Rt_(rot, pt), v_(vel) {}
PoseRTV(const Rot3& rot, const Point3& pt, const Velocity3& vel)
: Rt_(rot, pt), v_(vel) {}
explicit PoseRTV(const Point3& pt)
: Rt_(Rot3::identity(), pt) {}
PoseRTV(const Point3& t, const Rot3& rot, const Velocity3& vel)
: Base(Pose3(rot, t), vel) {}
PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
: Base(Pose3(rot, t), vel) {}
explicit PoseRTV(const Point3& t)
: Base(Pose3(Rot3(), t),Velocity3()) {}
PoseRTV(const Pose3& pose, const Velocity3& vel)
: Rt_(pose), v_(vel) {}
: Base(pose, vel) {}
explicit PoseRTV(const Pose3& pose)
: Rt_(pose) {}
: Base(pose,Velocity3()) {}
// Construct from Base
PoseRTV(const Base& base)
: Base(base) {}
/** build from components - useful for data files */
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z,
@ -51,72 +52,46 @@ public:
explicit PoseRTV(const Vector& v);
// access
const Point3& t() const { return Rt_.translation(); }
const Rot3& R() const { return Rt_.rotation(); }
const Velocity3& v() const { return v_; }
const Pose3& pose() const { return Rt_; }
const Pose3& pose() const { return first; }
const Velocity3& v() const { return second; }
const Point3& t() const { return pose().translation(); }
const Rot3& R() const { return pose().rotation(); }
// longer function names
const Point3& translation() const { return Rt_.translation(); }
const Rot3& rotation() const { return Rt_.rotation(); }
const Velocity3& velocity() const { return v_; }
const Point3& translation() const { return pose().translation(); }
const Rot3& rotation() const { return pose().rotation(); }
const Velocity3& velocity() const { return second; }
// Access to vector for ease of use with Matlab
// and avoidance of Point3
Vector vector() const;
Vector translationVec() const { return Rt_.translation().vector(); }
Vector velocityVec() const { return v_.vector(); }
Vector translationVec() const { return pose().translation().vector(); }
Vector velocityVec() const { return velocity().vector(); }
// testable
bool equals(const PoseRTV& other, double tol=1e-6) const;
void print(const std::string& s="") const;
// Manifold
static size_t Dim() { return 9; }
size_t dim() const { return Dim(); }
/// @name Manifold
/// @{
using Base::dimension;
using Base::dim;
using Base::Dim;
using Base::retract;
using Base::localCoordinates;
/// @}
/**
* retract/unretract assume independence of components
* Tangent space parameterization:
* - v(0-2): Rot3 (roll, pitch, yaw)
* - v(3-5): Point3
* - v(6-8): Translational velocity
*/
PoseRTV retract(const Vector& v, ChartJacobian Horigin=boost::none, ChartJacobian Hv=boost::none) const;
Vector localCoordinates(const PoseRTV& p, ChartJacobian Horigin=boost::none,ChartJacobian Hp=boost::none) const;
/// @name measurement functions
/// @{
// Lie TODO IS this a Lie group or just a Manifold????
/**
* expmap/logmap are poor approximations that assume independence of components
* Currently implemented using the poor retract/unretract approximations
*/
static PoseRTV Expmap(const Vector9& v, ChartJacobian H = boost::none);
static Vector9 Logmap(const PoseRTV& p, ChartJacobian H = boost::none);
static PoseRTV identity() { return PoseRTV(); }
/** Derivatives calculated numerically */
PoseRTV inverse(ChartJacobian H1=boost::none) const;
/** Derivatives calculated numerically */
PoseRTV compose(const PoseRTV& p,
ChartJacobian H1=boost::none,
ChartJacobian H2=boost::none) const;
PoseRTV operator*(const PoseRTV& p) const { return compose(p); }
/** Derivatives calculated numerically */
PoseRTV between(const PoseRTV& p,
ChartJacobian H1=boost::none,
ChartJacobian H2=boost::none) const;
// measurement functions
/** Derivatives calculated numerically */
/** range between translations */
double range(const PoseRTV& other,
OptionalJacobian<1,9> H1=boost::none,
OptionalJacobian<1,9> H2=boost::none) const;
/// @}
// IMU-specific
/// @name IMU-specific
/// @{
/// Dynamics integrator for ground robots
/// Always move from time 1 to time 2
@ -164,7 +139,9 @@ public:
ChartJacobian Dglobal = boost::none,
OptionalJacobian<9, 6> Dtrans = boost::none) const;
// Utility functions
/// @}
/// @name Utility functions
/// @{
/// RRTMbn - Function computes the rotation rate transformation matrix from
/// body axis rates to euler angle (global) rates
@ -177,19 +154,20 @@ public:
static Matrix RRTMnb(const Vector& euler);
static Matrix RRTMnb(const Rot3& att);
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(Rt_);
ar & BOOST_SERIALIZATION_NVP(v_);
ar & BOOST_SERIALIZATION_NVP(first);
ar & BOOST_SERIALIZATION_NVP(second);
}
};
template<>
struct traits<PoseRTV> : public internal::LieGroupTraits<PoseRTV> {};
struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
} // \namespace gtsam

48
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -76,11 +76,12 @@ TEST( testPoseRTV, equals ) {
/* ************************************************************************* */
TEST( testPoseRTV, Lie ) {
// origin and zero deltas
EXPECT(assert_equal(PoseRTV(), PoseRTV().retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), PoseRTV().localCoordinates(PoseRTV()), tol));
PoseRTV identity;
EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol));
PoseRTV state1(pt, rot, vel);
EXPECT(assert_equal(state1, state1.retract(zero(9)), tol));
EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished();
@ -88,9 +89,9 @@ TEST( testPoseRTV, Lie ) {
Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
PoseRTV state2(pt2, rot2, vel2);
EXPECT(assert_equal(state2, state1.retract(delta), 1e-1));
EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
EXPECT(assert_equal(-delta, state2.localCoordinates(state1), 1e-1)); // loose tolerance due to retract approximation
EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
}
/* ************************************************************************* */
@ -119,6 +120,43 @@ TEST( testPoseRTV, dynamics_identities ) {
}
/* ************************************************************************* */
PoseRTV compose_proxy(const PoseRTV& A, const PoseRTV& B) { return A.compose(B); }
TEST( testPoseRTV, compose ) {
PoseRTV state1(pt, rot, vel), state2 = state1;
Matrix actH1, actH2;
state1.compose(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
PoseRTV between_proxy(const PoseRTV& A, const PoseRTV& B) { return A.between(B); }
TEST( testPoseRTV, between ) {
PoseRTV state1(pt, rot, vel), state2 = state1;
Matrix actH1, actH2;
state1.between(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
PoseRTV inverse_proxy(const PoseRTV& A) { return A.inverse(); }
TEST( testPoseRTV, inverse ) {
PoseRTV state1(pt, rot, vel);
Matrix actH1;
state1.inverse(actH1);
Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
EXPECT(assert_equal(numericH1, actH1, tol));
}
/* ************************************************************************* */
double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
TEST( testPoseRTV, range ) {

2
gtsam_unstable/geometry/Similarity3.h
View File

@ -148,5 +148,5 @@ public:
};
template<>
struct traits<Similarity3> : public internal::LieGroupTraits<Similarity3> {};
struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
}

111
tests/testLie.cpp Normal file
View File

@ -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
* -------------------------------1------------------------------------------- */
/**
* @file testLie.cpp
* @date May, 2015
* @author Frank Dellaert
* @brief unit tests for Lie group type machinery
*/
#include <gtsam/base/ProductLieGroup.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/base/testLie.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
static const double tol = 1e-9;
//******************************************************************************
typedef ProductLieGroup<Point2, Pose2> Product;
// Define any direct product group to be a model of the multiplicative Group concept
namespace gtsam {
template<> struct traits<Product> : internal::LieGroupTraits<Product> {
static void Print(const Product& m, const string& s = "") {
cout << s << "(" << m.first << "," << m.second.translation() << "/"
<< m.second.theta() << ")" << endl;
}
static bool Equals(const Product& m1, const Product& m2, double tol = 1e-8) {
return m1.first.equals(m2.first, tol) && m1.second.equals(m2.second, tol);
}
};
}
//******************************************************************************
TEST(Lie, ProductLieGroup) {
BOOST_CONCEPT_ASSERT((IsGroup<Product>));
BOOST_CONCEPT_ASSERT((IsManifold<Product>));
BOOST_CONCEPT_ASSERT((IsLieGroup<Product>));
Product pair1;
Vector5 d;
d << 1, 2, 0.1, 0.2, 0.3;
Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
Product pair2 = pair1.retract(d);
EXPECT(assert_equal(expected, pair2, 1e-9));
EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
}
/* ************************************************************************* */
Product compose_proxy(const Product& A, const Product& B) {
return A.compose(B);
}
TEST( testProduct, compose ) {
Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
Matrix actH1, actH2;
state1.compose(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
Product between_proxy(const Product& A, const Product& B) {
return A.between(B);
}
TEST( testProduct, between ) {
Product state1(Point2(1, 2), Pose2(3, 4, 5)), state2 = state1;
Matrix actH1, actH2;
state1.between(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
}
/* ************************************************************************* */
Product inverse_proxy(const Product& A) {
return A.inverse();
}
TEST( testProduct, inverse ) {
Product state1(Point2(1, 2), Pose2(3, 4, 5));
Matrix actH1;
state1.inverse(actH1);
Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
EXPECT(assert_equal(numericH1, actH1, tol));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************

31
tests/testManifold.cpp
View File

@ -10,13 +10,14 @@
* -------------------------------1------------------------------------------- */
/**
* @file testExpression.cpp
* @file testManifold.cpp
* @date September 18, 2014
* @author Frank Dellaert
* @author Paul Furgale
* @brief unit tests for Block Automatic Differentiation
* @brief unit tests for Manifold type machinery
*/
#include <gtsam/base/Manifold.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
@ -148,6 +149,32 @@ TEST(Manifold, DefaultChart) {
EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
}
//******************************************************************************
typedef ProductManifold<Point2,Point2> MyPoint2Pair;
// Define any direct product group to be a model of the multiplicative Group concept
namespace gtsam {
template<> struct traits<MyPoint2Pair> : internal::ManifoldTraits<MyPoint2Pair> {
static void Print(const MyPoint2Pair& m, const string& s = "") {
cout << s << "(" << m.first << "," << m.second << ")" << endl;
}
static bool Equals(const MyPoint2Pair& m1, const MyPoint2Pair& m2, double tol = 1e-8) {
return m1 == m2;
}
};
}
TEST(Manifold, ProductManifold) {
BOOST_CONCEPT_ASSERT((IsManifold<MyPoint2Pair>));
MyPoint2Pair pair1;
Vector4 d;
d << 1,2,3,4;
MyPoint2Pair expected(Point2(1,2),Point2(3,4));
MyPoint2Pair pair2 = pair1.retract(d);
EXPECT(assert_equal(expected,pair2,1e-9));
EXPECT(assert_equal(d, pair1.localCoordinates(pair2),1e-9));
}
//******************************************************************************
int main() {
TestResult tr;