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

testDynamicValues passed

release/4.3a0
Duy-Nguyen Ta 2012-01-27 21:48:23 +00:00
parent 0eaf241340
commit 79a3855316
13 changed files with 117 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/base/DerivedValue.cpp
View File

@ -1,21 +0,0 @@
/*
* DerivedValue.cpp
*
* Created on: Jan 26, 2012
* Author: thduynguyen
*/
#include "DerivedValue.h"
namespace gtsam {
DerivedValue::DerivedValue() {
// TODO Auto-generated constructor stub
}
DerivedValue::~DerivedValue() {
// TODO Auto-generated destructor stub
}
} /* namespace gtsam */

3
gtsam/base/LieVector.h
View File

@ -19,13 +19,14 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
/**
* LieVector is a wrapper around vector to allow it to be a Lie type
*/
struct LieVector : public Vector {
struct LieVector : public Vector, public DerivedValue<LieVector> {
/** default constructor - should be unnecessary */
LieVector() {}

2
gtsam/geometry/Cal3Bundler.h
View File

@ -95,7 +95,7 @@ public:
Vector localCoordinates(const Cal3Bundler& T2) const ;
///TODO: comment
int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
///TODO: comment
static size_t Dim() { return 3; } //TODO: make a final dimension variable

2
gtsam/geometry/Cal3DS2.h
View File

@ -98,7 +98,7 @@ public:
Vector localCoordinates(const Cal3DS2& T2) const ;
///TODO: comment
int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
///TODO: comment
static size_t Dim() { return 9; } //TODO: make a final dimension variable

2
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3_S2Stereo: public DerivedValue<Cal3_S2Stereo>, public Cal3_S2 {
class Cal3_S2Stereo: public Cal3_S2 {
private:
double b_;

5
gtsam/geometry/CalibratedCamera.h
View File

@ -62,6 +62,11 @@ namespace gtsam {
/// @name Testable
/// @{
virtual void print(const std::string& s = "") const {
std::cout << s;
pose_.print("pose: ");
}
/// check equality to another camera
bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) ;

2
gtsam/geometry/Point2.h
View File

@ -32,7 +32,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Point2 : DerivedValue<Point2> {
class Point2 : public DerivedValue<Point2> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;

8
gtsam/geometry/SimpleCamera.h
View File

@ -30,7 +30,7 @@ namespace gtsam {
* to produce measurements in pixels.
* Not a sublass as a SimpleCamera *is not* a CalibratedCamera.
*/
class SimpleCamera : public DerivedValue<SimpleCamera> {
class SimpleCamera {
private:
CalibratedCamera calibrated_; // Calibrated camera
Cal3_S2 K_; // Calibration
@ -77,6 +77,12 @@ namespace gtsam {
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
virtual void print(const std::string& s = "") const {
std::cout << s;
calibrated_.print("calibrated: ");
K_.print("K: ");
}
bool equals(const SimpleCamera& X, double tol=1e-9) const {
return calibrated_.equals(X.calibrated_, tol) && K_.equals(X.K_, tol);
}

24
gtsam/nonlinear/DynamicValues-inl.h
View File

@ -93,28 +93,4 @@ namespace gtsam {
return exists<typename TypedKey::Value>(symbol);
}
/* ************************************************************************* */
template<class ValueType>
void DynamicValues::insert(const Symbol& j, const ValueType& val) {
std::pair<iterator,bool> insertResult = values_.insert(j, val);
if(!insertResult.second)
throw DynamicValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
template<class ValueType>
void DynamicValues::update(const Symbol& j, const ValueType& val) {
// Find the value to update
iterator item = values_.find(j);
if(item == values_.end())
throw DynamicValuesKeyDoesNotExist("update", j);
// Cast to the derived type
if(typeid(*item->second) != typeid(Value))
throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(Value));
ValueType& valueAsDerived = static_cast<ValueType&>(*item->second);
// Assign
valueAsDerived = val;
}
}

29
gtsam/nonlinear/DynamicValues.cpp
View File

@ -82,7 +82,6 @@ namespace gtsam {
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
Value* retractedValue(key_value->second->retract_(singleDelta)); // Retract
result.values_.insert(key, retractedValue); // Add retracted result directly to result values
retractedValue->deallocate_();
}
return result;
@ -107,14 +106,36 @@ namespace gtsam {
}
}
/* ************************************************************************* */
void DynamicValues::insert(const Symbol& j, const Value& val) {
Symbol key = j; // Non-const duplicate to deal with non-const insert argument
std::pair<iterator,bool> insertResult = values_.insert(key, val.clone_());
if(!insertResult.second)
throw DynamicValuesKeyAlreadyExists(j);
}
/* ************************************************************************* */
void DynamicValues::insert(const DynamicValues& values) {
for(KeyValueMap::const_iterator key_value = begin(); key_value != end(); ++key_value) {
for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
Symbol key = key_value->first; // Non-const duplicate to deal with non-const insert argument
values_.insert(key, key_value->second->clone_());
insert(key, *key_value->second);
}
}
/* ************************************************************************* */
void DynamicValues::update(const Symbol& j, const Value& val) {
// Find the value to update
iterator item = values_.find(j);
if(item == values_.end())
throw DynamicValuesKeyDoesNotExist("update", j);
// Cast to the derived type
if(typeid(*item->second) != typeid(val))
throw DynamicValuesIncorrectType(j, typeid(*item->second), typeid(val));
values_.replace(item, val.clone_());
}
/* ************************************************************************* */
void DynamicValues::update(const DynamicValues& values) {
for(KeyValueMap::const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {
@ -152,7 +173,7 @@ namespace gtsam {
result.assign(
boost::make_transform_iterator(values_.begin(),
boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))),
boost::make_transform_iterator(values_.begin(),
boost::make_transform_iterator(values_.end(),
boost::bind(&Value::dim, boost::bind(&KeyValuePair::second, _1))));
return result;
}

10
gtsam/nonlinear/DynamicValues.h
View File

@ -178,19 +178,17 @@ namespace gtsam {
// imperative methods:
/** Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present */
template<class ValueType>
void insert(const Symbol& j, const ValueType& val);
void insert(const Symbol& j, const Value& val);
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const DynamicValues& values);
/** single element change of existing element */
void update(const Symbol& j, const Value& val);
/** update the current available values without adding new ones */
void update(const DynamicValues& values);
/** single element change of existing element */
template<class ValueType>
void update(const Symbol& j, const ValueType& val);
/** Remove a variable from the config, throws KeyDoesNotExist<J> if j is not present */
void erase(const Symbol& j);

2
gtsam/nonlinear/Makefile.am
View File

@ -63,7 +63,7 @@ AM_CXXFLAGS =
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_LDFLAGS += $(boost_serialization)
LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../geometry/libgeometry.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp

157
gtsam/nonlinear/tests/testDynamicValues.cpp
View File

@ -22,6 +22,7 @@ using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/DynamicValues.h>
using namespace gtsam;
@ -36,7 +37,7 @@ VecKey key1(1), key2(2), key3(3), key4(4);
TEST( DynamicValues, equals1 )
{
DynamicValues expected;
Vector v = Vector_(3, 5.0, 6.0, 7.0);
LieVector v(3, 5.0, 6.0, 7.0);
expected.insert(key1,v);
DynamicValues actual;
actual.insert(key1,v);
@ -47,8 +48,8 @@ TEST( DynamicValues, equals1 )
TEST( DynamicValues, equals2 )
{
DynamicValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 5.0, 6.0, 8.0);
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 5.0, 6.0, 8.0);
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2));
@ -59,8 +60,8 @@ TEST( DynamicValues, equals2 )
TEST( DynamicValues, equals_nan )
{
DynamicValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, inf, inf, inf);
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, inf, inf, inf);
cfg1.insert(key1, v1);
cfg2.insert(key1, v2);
CHECK(!cfg1.equals(cfg2));
@ -71,10 +72,10 @@ TEST( DynamicValues, equals_nan )
TEST( DynamicValues, insert_good )
{
DynamicValues cfg1, cfg2, expected;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v3(3, 2.0, 4.0, 3.0);
LieVector v4(3, 8.0, 3.0, 7.0);
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
cfg2.insert(key3, v4);
@ -85,17 +86,17 @@ TEST( DynamicValues, insert_good )
expected.insert(key2, v2);
expected.insert(key3, v4);
CHECK(assert_equal(cfg1, expected));
CHECK(assert_equal(expected, cfg1));
}
/* ************************************************************************* */
TEST( DynamicValues, insert_bad )
{
DynamicValues cfg1, cfg2;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
Vector v3 = Vector_(3, 2.0, 4.0, 3.0);
Vector v4 = Vector_(3, 8.0, 3.0, 7.0);
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
LieVector v3(3, 2.0, 4.0, 3.0);
LieVector v4(3, 8.0, 3.0, 7.0);
cfg1.insert(key1, v1);
cfg1.insert(key2, v2);
cfg2.insert(key2, v3);
@ -108,8 +109,8 @@ TEST( DynamicValues, insert_bad )
TEST( DynamicValues, update_element )
{
DynamicValues cfg;
Vector v1 = Vector_(3, 5.0, 6.0, 7.0);
Vector v2 = Vector_(3, 8.0, 9.0, 1.0);
LieVector v1(3, 5.0, 6.0, 7.0);
LieVector v2(3, 8.0, 9.0, 1.0);
cfg.insert(key1, v1);
CHECK(cfg.size() == 1);
@ -124,9 +125,9 @@ TEST( DynamicValues, update_element )
//TEST(DynamicValues, dim_zero)
//{
// DynamicValues config0;
// config0.insert(key1, Vector_(2, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5,config0.dim());
// config0.insert(key1, LieVector(2, 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim());
//
// VectorValues expected;
// expected.insert(key1, zero(2));
@ -138,8 +139,8 @@ TEST( DynamicValues, update_element )
TEST(DynamicValues, expmap_a)
{
DynamicValues config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
Ordering ordering(*config0.orderingArbitrary());
VectorValues increment(config0.dims(ordering));
@ -147,94 +148,95 @@ TEST(DynamicValues, expmap_a)
increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
DynamicValues expected;
expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment, ordering)));
}
///* ************************************************************************* */
//TEST(DynamicValues, expmap_b)
//{
// DynamicValues config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
//
// Ordering ordering(*config0.orderingArbitrary());
// VectorValues increment(config0.dims(ordering));
// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5);
//
// DynamicValues expected;
// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
//
// CHECK(assert_equal(expected, config0.retract(increment, ordering)));
//}
/* ************************************************************************* */
TEST(DynamicValues, expmap_b)
{
DynamicValues config0;
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
///* ************************************************************************* */
Ordering ordering(*config0.orderingArbitrary());
VectorValues increment(config0.dims(ordering));
increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5);
DynamicValues expected;
expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment, ordering)));
}
/* ************************************************************************* */
//TEST(DynamicValues, expmap_c)
//{
// DynamicValues config0;
// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
//
// Vector increment = Vector_(6,
// Vector increment = LieVector(6,
// 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5);
//
// DynamicValues expected;
// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2));
// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5));
// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2));
// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5));
//
// CHECK(assert_equal(expected, config0.retract(increment)));
//}
/* ************************************************************************* */
/*TEST(DynamicValues, expmap_d)
TEST(DynamicValues, expmap_d)
{
DynamicValues config0;
config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0));
config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0));
config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0));
config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0));
//config0.print("config0");
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
DynamicValues<string,Pose2> poseconfig;
poseconfig.insert("p1", Pose2(1,2,3));
poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5));
//poseconfig.print("poseconfig");
typedef TypedSymbol<Pose2, 'p'> PoseKey;
DynamicValues poseconfig;
poseconfig.insert(PoseKey(1), Pose2(1,2,3));
poseconfig.insert(PoseKey(2), Pose2(0.3, 0.4, 0.5));
CHECK(equal(config0, config0));
CHECK(config0.equals(config0));
}*/
}
/* ************************************************************************* */
/*TEST(DynamicValues, extract_keys)
TEST(DynamicValues, extract_keys)
{
typedef TypedSymbol<Pose2, 'x'> PoseKey;
DynamicValues<PoseKey, Pose2> config;
DynamicValues config;
config.insert(PoseKey(1), Pose2());
config.insert(PoseKey(2), Pose2());
config.insert(PoseKey(4), Pose2());
config.insert(PoseKey(5), Pose2());
list<PoseKey> expected, actual;
FastList<Symbol> expected, actual;
expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5);
actual = config.keys();
CHECK(actual.size() == expected.size());
list<PoseKey>::const_iterator itAct = actual.begin(), itExp = expected.begin();
FastList<Symbol>::const_iterator itAct = actual.begin(), itExp = expected.begin();
for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) {
CHECK(assert_equal(*itExp, *itAct));
}
}*/
}
/* ************************************************************************* */
TEST(DynamicValues, exists_)
{
DynamicValues config0;
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
config0.insert(key1, LieVector(Vector_(1, 1.)));
config0.insert(key2, LieVector(Vector_(1, 2.)));
boost::optional<const LieVector&> v = config0.exists(key1);
CHECK(assert_equal(Vector_(1, 1.),*v));
@ -244,41 +246,20 @@ TEST(DynamicValues, exists_)
TEST(DynamicValues, update)
{
DynamicValues config0;
config0.insert(key1, Vector_(1, 1.));
config0.insert(key2, Vector_(1, 2.));
config0.insert(key1, LieVector(1, 1.));
config0.insert(key2, LieVector(1, 2.));
DynamicValues superset;
superset.insert(key1, Vector_(1, -1.));
superset.insert(key2, Vector_(1, -2.));
superset.insert(key3, Vector_(1, -3.));
superset.insert(key1, LieVector(1, -1.));
superset.insert(key2, LieVector(1, -2.));
config0.update(superset);
DynamicValues expected;
expected.insert(key1, Vector_(1, -1.));
expected.insert(key2, Vector_(1, -2.));
expected.insert(key1, LieVector(1, -1.));
expected.insert(key2, LieVector(1, -2.));
CHECK(assert_equal(expected,config0));
}
/* ************************************************************************* */
TEST(DynamicValues, dummy_initialization)
{
typedef TypedSymbol<LieVector, 'z'> Key;
DynamicValues init1;
init1.insert(Key(1), Vector_(2, 1.0, 2.0));
init1.insert(Key(2), Vector_(2, 4.0, 3.0));
DynamicValues init2;
init2.insert(key1, Vector_(2, 1.0, 2.0));
init2.insert(key2, Vector_(2, 4.0, 3.0));
DynamicValues actual1(init2);
DynamicValues actual2(init1);
EXPECT(actual1.empty());
EXPECT(actual2.empty());
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */