LieScalar loses its mojo (superfluous Lie/Manifold stuff)
parent
e9fa599a78
commit
128cc741ac
|
@ -2586,6 +2586,14 @@
|
||||||
<useDefaultCommand>true</useDefaultCommand>
|
<useDefaultCommand>true</useDefaultCommand>
|
||||||
<runAllBuilders>true</runAllBuilders>
|
<runAllBuilders>true</runAllBuilders>
|
||||||
</target>
|
</target>
|
||||||
|
<target name="testLieScalar.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
|
<buildCommand>make</buildCommand>
|
||||||
|
<buildArguments>-j4</buildArguments>
|
||||||
|
<buildTarget>testLieScalar.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">
|
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||||
<buildCommand>make</buildCommand>
|
<buildCommand>make</buildCommand>
|
||||||
<buildArguments>-j5</buildArguments>
|
<buildArguments>-j5</buildArguments>
|
||||||
|
|
|
@ -49,6 +49,9 @@ namespace gtsam {
|
||||||
/** Automatic conversion to underlying value */
|
/** Automatic conversion to underlying value */
|
||||||
operator double() const { return d_; }
|
operator double() const { return d_; }
|
||||||
|
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
/** print @param name optional string naming the object */
|
/** print @param name optional string naming the object */
|
||||||
void print(const std::string& name="") const;
|
void print(const std::string& name="") const;
|
||||||
|
|
||||||
|
@ -57,65 +60,7 @@ namespace gtsam {
|
||||||
return fabs(expected.d_ - d_) <= tol;
|
return fabs(expected.d_ - d_) <= tol;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Manifold requirements
|
/// @}
|
||||||
|
|
||||||
/** Returns dimensionality of the tangent space */
|
|
||||||
size_t dim() const { return 1; }
|
|
||||||
static size_t Dim() { return 1; }
|
|
||||||
|
|
||||||
/** Update the LieScalar with a tangent space update */
|
|
||||||
LieScalar retract(const Vector& v) const { return LieScalar(value() + v(0)); }
|
|
||||||
|
|
||||||
/** @return the local coordinates of another object */
|
|
||||||
Vector localCoordinates(const LieScalar& t2) const { return (Vector(1) << (t2.value() - value())).finished(); }
|
|
||||||
|
|
||||||
// Group requirements
|
|
||||||
|
|
||||||
/** identity */
|
|
||||||
static LieScalar identity() {
|
|
||||||
return LieScalar();
|
|
||||||
}
|
|
||||||
|
|
||||||
/** compose with another object */
|
|
||||||
LieScalar compose(const LieScalar& p,
|
|
||||||
boost::optional<Matrix&> H1=boost::none,
|
|
||||||
boost::optional<Matrix&> H2=boost::none) const {
|
|
||||||
if(H1) *H1 = eye(1);
|
|
||||||
if(H2) *H2 = eye(1);
|
|
||||||
return LieScalar(d_ + p.d_);
|
|
||||||
}
|
|
||||||
|
|
||||||
/** between operation */
|
|
||||||
LieScalar between(const LieScalar& l2,
|
|
||||||
boost::optional<Matrix&> H1=boost::none,
|
|
||||||
boost::optional<Matrix&> H2=boost::none) const {
|
|
||||||
if(H1) *H1 = -eye(1);
|
|
||||||
if(H2) *H2 = eye(1);
|
|
||||||
return LieScalar(l2.value() - value());
|
|
||||||
}
|
|
||||||
|
|
||||||
/** invert the object and yield a new one */
|
|
||||||
LieScalar inverse() const {
|
|
||||||
return LieScalar(-1.0 * value());
|
|
||||||
}
|
|
||||||
|
|
||||||
// Lie functions
|
|
||||||
|
|
||||||
/** Expmap around identity */
|
|
||||||
static LieScalar Expmap(const Vector& v) { return LieScalar(v(0)); }
|
|
||||||
|
|
||||||
/** Logmap around identity - just returns with default cast back */
|
|
||||||
static Vector Logmap(const LieScalar& p) { return (Vector(1) << p.value()).finished(); }
|
|
||||||
|
|
||||||
/// Left-trivialized derivative of the exponential map
|
|
||||||
static Matrix dexpL(const Vector& v) {
|
|
||||||
return eye(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Left-trivialized derivative inverse of the exponential map
|
|
||||||
static Matrix dexpInvL(const Vector& v) {
|
|
||||||
return eye(1);
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
double d_;
|
double d_;
|
||||||
|
|
|
@ -27,6 +27,20 @@ GTSAM_CONCEPT_LIE_INST(LieScalar)
|
||||||
|
|
||||||
const double tol=1e-9;
|
const double tol=1e-9;
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(LieScalar , Concept) {
|
||||||
|
BOOST_CONCEPT_ASSERT((IsGroup<LieScalar>));
|
||||||
|
BOOST_CONCEPT_ASSERT((IsManifold<LieScalar>));
|
||||||
|
BOOST_CONCEPT_ASSERT((IsLieGroup<LieScalar>));
|
||||||
|
}
|
||||||
|
|
||||||
|
//******************************************************************************
|
||||||
|
TEST(LieScalar , Invariants) {
|
||||||
|
LieScalar lie1(2), lie2(3);
|
||||||
|
check_group_invariants(lie1, lie2);
|
||||||
|
check_manifold_invariants(lie1, lie2);
|
||||||
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
TEST( testLieScalar, construction ) {
|
TEST( testLieScalar, construction ) {
|
||||||
double d = 2.;
|
double d = 2.;
|
||||||
|
@ -34,7 +48,7 @@ TEST( testLieScalar, construction ) {
|
||||||
|
|
||||||
EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol);
|
EXPECT_DOUBLES_EQUAL(2., lie1.value(),tol);
|
||||||
EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol);
|
EXPECT_DOUBLES_EQUAL(2., lie2.value(),tol);
|
||||||
EXPECT(lie1.dim() == 1);
|
EXPECT(traits_x<LieScalar>::dimension == 1);
|
||||||
EXPECT(assert_equal(lie1, lie2));
|
EXPECT(assert_equal(lie1, lie2));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -42,7 +56,8 @@ TEST( testLieScalar, construction ) {
|
||||||
TEST( testLieScalar, localCoordinates ) {
|
TEST( testLieScalar, localCoordinates ) {
|
||||||
LieScalar lie1(1.), lie2(3.);
|
LieScalar lie1(1.), lie2(3.);
|
||||||
|
|
||||||
EXPECT(assert_equal((Vector)(Vector(1) << 2).finished(), lie1.localCoordinates(lie2)));
|
Vector1 actual = traits_x<LieScalar>::Local(lie1, lie2);
|
||||||
|
EXPECT( assert_equal((Vector)(Vector(1) << 2).finished(), actual));
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
Loading…
Reference in New Issue