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

PairConfig is implemented, VSLAMConfig is now a typedef!

release/4.3a0
Richard Roberts 2010-01-14 02:58:29 +00:00
parent 6b3e8cf49c
commit ac10c440e1
14 changed files with 434 additions and 221 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
.cproject
View File

@ -1,4 +1,4 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?xml version="1.0" encoding="UTF-8"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
@ -298,22 +298,6 @@
</storageModule>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-k</buildArguments>
@ -468,6 +452,7 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -475,7 +460,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -483,6 +467,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -738,6 +723,7 @@
</target>
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments></buildArguments>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -751,6 +737,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTupleConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testTupleConfig.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -775,6 +769,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
</buildTargets>
</storageModule>
</cconfiguration>

36
cpp/LieConfig-inl.h
View File

@ -38,22 +38,30 @@ namespace gtsam {
}
template<class J, class T>
const T& LieConfig<J,T>::at(const Key& key) const {
const_iterator it = values_.find(key);
if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
const T& LieConfig<J,T>::at(const J& j) const {
const_iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
else return it->second;
}
template<class J, class T>
void LieConfig<J,T>::insert(const Key& name, const T& val) {
void LieConfig<J,T>::insert(const J& name, const T& val) {
values_.insert(make_pair(name, val));
dim_ += dim(val);
}
template<class J, class T>
void LieConfig<J,T>::erase(const Key& key) {
iterator it = values_.find(key);
if (it == values_.end()) throw std::invalid_argument("invalid key: " + (string)key);
void LieConfig<J,T>::erase(const J& j) {
size_t dim; // unused
erase(j, dim);
}
template<class J, class T>
void LieConfig<J,T>::erase(const J& j, size_t& dim) {
iterator it = values_.find(j);
if (it == values_.end()) throw std::invalid_argument("invalid j: " + (string)j);
dim = gtsam::dim(it->second);
dim_ -= dim;
values_.erase(it);
}
@ -61,13 +69,13 @@ namespace gtsam {
template<class J, class T>
LieConfig<J,T> expmap(const LieConfig<J,T>& c, const VectorConfig& delta) {
LieConfig<J,T> newConfig;
typedef pair<typename LieConfig<J,T>::Key,T> Value;
typedef pair<J,T> Value;
BOOST_FOREACH(const Value& value, c) {
const typename LieConfig<J,T>::Key& j = value.first;
const J& j = value.first;
const T& pj = value.second;
string key = (string)j;
if (delta.contains(key)) {
const Vector& dj = delta[key];
string jstr = (string)j;
if (delta.contains(jstr)) {
const Vector& dj = delta[jstr];
newConfig.insert(j, expmap(pj,dj));
} else
newConfig.insert(j, pj);
@ -82,9 +90,9 @@ namespace gtsam {
throw invalid_argument("Delta vector length does not match config dimensionality.");
LieConfig<J,T> newConfig;
int delta_offset = 0;
typedef pair<typename LieConfig<J,T>::Key,T> Value;
typedef pair<J,T> Value;
BOOST_FOREACH(const Value& value, c) {
const typename LieConfig<J,T>::Key& j = value.first;
const J& j = value.first;
const T& pj = value.second;
int cur_dim = dim(pj);
newConfig.insert(j,expmap(pj,sub(delta, delta_offset, delta_offset+cur_dim)));

26
cpp/LieConfig.h
View File

@ -48,8 +48,9 @@ namespace gtsam {
/**
* Typedefs
*/
typedef J Key;
typedef std::map<Key, T> Values;
typedef J Key;
typedef T Value;
typedef std::map<J, T> Values;
typedef typename Values::iterator iterator;
typedef typename Values::const_iterator const_iterator;
@ -71,17 +72,17 @@ namespace gtsam {
/** Test whether configs are identical in keys and values */
bool equals(const LieConfig& expected, double tol=1e-9) const;
/** Retrieve a variable by key, throws std::invalid_argument if not found */
const T& at(const Key& key) const;
/** Retrieve a variable by j, throws std::invalid_argument if not found */
const T& at(const J& j) const;
/** operator[] syntax for get */
inline const T& operator[](const Key& key) const { return at(key);}
const T& operator[](const J& j) const { return at(j); }
/** Check if a variable exists */
bool exists(const Key& i) const {return values_.find(i)!=values_.end();}
bool exists(const J& i) const { return values_.find(i)!=values_.end(); }
/** The number of variables in this config */
int size() const { return values_.size(); }
size_t size() const { return values_.size(); }
const_iterator begin() const { return values_.begin(); }
const_iterator end() const { return values_.end(); }
@ -90,11 +91,16 @@ namespace gtsam {
// imperative methods:
/** Add a variable with the given key */
void insert(const Key& key, const T& val);
/** Add a variable with the given j */
void insert(const J& j, const T& val);
/** Remove a variable from the config */
void erase(const Key& key) ;
void erase(const J& j);
/** Remove a variable from the config while returning the dimensionality of
* the removed element (normally not needed by user code).
*/
void erase(const J& j, size_t& dim);
/** Replace all keys and variables */
LieConfig& operator=(const LieConfig& rhs) {

6
cpp/Makefile.am
View File

@ -162,7 +162,7 @@ testSQPOptimizer_LDADD = libgtsam.la
# geometry
headers += Lie.h Lie-inl.h
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig
check_PROGRAMS += testPoint2 testRot2 testPose2 testPoint3 testRot3 testPose3 testCal3_S2 testLieConfig testTupleConfig
testPoint2_SOURCES = testPoint2.cpp
testRot2_SOURCES = testRot2.cpp
testPose2_SOURCES = testPose2.cpp
@ -171,6 +171,7 @@ testRot3_SOURCES = testRot3.cpp
testPose3_SOURCES = testPose3.cpp
testCal3_S2_SOURCES = testCal3_S2.cpp
testLieConfig_SOURCES = testLieConfig.cpp
testTupleConfig_SOURCES = testTupleConfig.cpp
testPoint2_LDADD = libgtsam.la
testRot2_LDADD = libgtsam.la
@ -180,6 +181,7 @@ testRot3_LDADD = libgtsam.la
testPose3_LDADD = libgtsam.la
testCal3_S2_LDADD = libgtsam.la
testLieConfig_LDADD = libgtsam.la
testTupleConfig_LDADD = libgtsam.la
noinst_PROGRAMS = timeRot3
timeRot3_SOURCES = timeRot3.cpp
@ -238,7 +240,7 @@ testSimpleCamera_SOURCES = testSimpleCamera.cpp
testSimpleCamera_LDADD = libgtsam.la
# Visual SLAM
sources += VSLAMConfig.cpp VSLAMGraph.cpp VSLAMFactor.cpp
sources += VSLAMGraph.cpp VSLAMFactor.cpp
check_PROGRAMS += testVSLAMFactor testVSLAMGraph testVSLAMConfig
testVSLAMFactor_SOURCES = testVSLAMFactor.cpp
testVSLAMFactor_LDADD = libgtsam.la

134
cpp/TupleConfig.h Normal file
View File

@ -0,0 +1,134 @@
/*
* TupleConfig.h
*
* Created on: Jan 13, 2010
* Author: Richard Roberts and Manohar Paluri
*/
#include <iostream>
#include "LieConfig-inl.h"
#pragma once
namespace gtsam {
/**
* PairConfig: a config that holds two data types.
*/
template<class J1, class X1, class J2, class X2>
class PairConfig : public Testable<PairConfig<J1, X1, J2, X2> > {
public:
// typedef J1 Key1;
// typedef J2 Key2;
// typedef X1 Value1;
// typedef X2 Value2;
typedef LieConfig<J1, X1> Config1;
typedef LieConfig<J2, X2> Config2;
private:
LieConfig<J1, X1> config1_;
LieConfig<J2, X2> config2_;
size_t size_;
size_t dim_;
PairConfig(const LieConfig<J1,X1>& config1, const LieConfig<J2,X2>& config2) :
config1_(config1), config2_(config2),
size_(config1.size()+config2.size()), dim_(gtsam::dim(config1)+gtsam::dim(config2)) {}
public:
/**
* Default constructor creates an empty config.
*/
PairConfig(): size_(0), dim_(0) {}
/**
* Copy constructor
*/
PairConfig(const PairConfig<J1, X1, J2, X2>& c):
config1_(c.config1_), config2_(c.config2_), size_(c.size_), dim_(c.dim_) {}
/**
* Print
*/
void print(const std::string& s = "") const {
std::cout << "TupleConfig " << s << ", size " << size_ << "\n";
config1_.print(s + "Config1: ");
config2_.print(s + "Config2: ");
}
/**
* Test for equality in keys and values
*/
bool equals(const PairConfig<J1, X1, J2, X2>& c, double tol=1e-9) const {
return config1_.equals(c.config1_, tol) && config2_.equals(c.config2_, tol); }
/**
* operator[] syntax to get a value by j, throws invalid_argument if
* value with specified j is not present. Will generate compile-time
* errors if j type does not match that on which the Config is templated.
*/
const X1& operator[](const J1& j) const { return config1_[j]; }
const X2& operator[](const J2& j) const { return config2_[j]; }
/**
* size is the total number of variables in this config.
*/
size_t size() const { return size_; }
/**
* dim is the dimensionality of the tangent space
*/
size_t dim() const { return dim_; }
private:
template<class Config, class Key, class Value>
void insert_helper(Config& config, const Key& j, const Value& value) {
config.insert(j, value);
size_ ++;
dim_ += gtsam::dim(value);
}
template<class Config, class Key>
void erase_helper(Config& config, const Key& j) {
size_t dim;
config.erase(j, dim);
dim_ -= dim;
size_ --;
}
public:
/**
* expmap each element
*/
PairConfig<J1,X1,J2,X2> expmap(const VectorConfig& delta) {
return PairConfig(gtsam::expmap(config1_, delta), gtsam::expmap(config2_, delta)); }
/**
* Insert a variable with the given j
*/
void insert(const J1& j, const X1& value) { insert_helper(config1_, j, value); }
void insert(const J2& j, const X2& value) { insert_helper(config2_, j, value); }
/**
* Remove the variable with the given j. Throws invalid_argument if the
* j is not present in the config.
*/
void erase(const J1& j) { erase_helper(config1_, j); }
void erase(const J2& j) { erase_helper(config2_, j); }
/**
* Check if a variable exists
*/
bool exists(const J1& j) const { return config1_.exists(j); }
bool exists(const J2& j) const { return config2_.exists(j); }
};
template<class J1, class X1, class J2, class X2>
inline PairConfig<J1,X1,J2,X2> expmap(PairConfig<J1,X1,J2,X2> c, const VectorConfig& delta) { return c.expmap(delta); }
}

44
cpp/VSLAMConfig.cpp
View File

@ -1,44 +0,0 @@
/**
* @file VSLAMConfig.cpp
* @brief The Config
* @author Alireza Fathi
* @author Carlos Nieto
*/
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include "VSLAMConfig.h"
#include "LieConfig-inl.h"
using namespace std;
namespace gtsam {
/* ************************************************************************* */
// Exponential map
VSLAMConfig expmap(const VSLAMConfig& x0, const VectorConfig & delta) {
VSLAMConfig x;
x.poses_ = expmap(x0.poses_, delta);
x.points_ = expmap(x0.points_, delta);
return x;
}
/* ************************************************************************* */
void VSLAMConfig::print(const std::string& s) const {
printf("%s:\n", s.c_str());
poses_.print("Poses");
points_.print("Points");
}
/* ************************************************************************* */
bool VSLAMConfig::equals(const VSLAMConfig& c, double tol) const {
return poses_.equals(c.poses_, tol) && points_.equals(c.points_, tol);
}
/* ************************************************************************* */
} // namespace gtsam

83
cpp/VSLAMConfig.h
View File

@ -5,85 +5,16 @@
* @author Carlos Nieto
*/
#include <string>
#include <map>
#include <vector>
#include <fstream>
#include "Pose3Config.h"
#include "Pose3.h"
#include "Point3.h"
#include "TupleConfig.h"
#pragma once
namespace gtsam{
/**
* Config that knows about points and poses
*/
class VSLAMConfig : Testable<VSLAMConfig> {
public:
typedef Symbol<Pose3,'x'> PoseKey;
typedef Symbol<Point3,'l'> PointKey;
private:
LieConfig<PoseKey, Pose3> poses_;
LieConfig<PointKey, Point3> points_;
public:
/**
* default constructor
*/
VSLAMConfig() {}
/**
* print
*/
void print(const std::string& s = "") const;
/**
* check whether two configs are equal
*/
bool equals(const VSLAMConfig& c, double tol=1e-6) const;
/**
* Get Poses or Points
*/
inline const Pose3& operator[](const PoseKey& key) const {return poses_[key];}
inline const Point3& operator[](const PointKey& key) const {return points_[key];}
// (Awkwardly named) backwards compatibility:
inline bool cameraPoseExists (const PoseKey& key) const {return poses_.exists(key);}
inline bool landmarkPointExists(const PointKey& key) const {return points_.exists(key);}
inline Pose3 cameraPose (const PoseKey& key) const {return poses_[key];}
inline Point3 landmarkPoint(const PointKey& key) const {return points_[key];}
inline size_t size() const {return points_.size() + poses_.size();}
inline size_t dim() const {return gtsam::dim(points_) + gtsam::dim(poses_);}
// Imperative functions:
inline void addCameraPose(const PoseKey& key, Pose3 cp) {poses_.insert(key,cp);}
inline void addLandmarkPoint(const PointKey& key, Point3 lp) {points_.insert(key,lp);}
inline void removeCameraPose(const PoseKey& key) { poses_.erase(key);}
inline void removeLandmarkPose(const PointKey& key) { points_.erase(key);}
inline void clear() {points_.clear(); poses_.clear();}
friend VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
};
/**
* Exponential map: takes 6D vectors in VectorConfig
* and applies them to the poses in the VSLAMConfig.
* Needed for use in nonlinear optimization
*/
VSLAMConfig expmap(const VSLAMConfig& c, const VectorConfig & delta);
} // namespace gtsam
typedef Symbol<Pose3,'x'> VSLAMPoseKey;
typedef Symbol<Point3,'l'> VSLAMPointKey;
typedef PairConfig<VSLAMPoseKey, Pose3, VSLAMPointKey, Point3> VSLAMConfig;
}

8
cpp/VSLAMFactor.h
View File

@ -15,8 +15,8 @@
namespace gtsam {
typedef NonlinearFactor2<VSLAMConfig, VSLAMConfig::PoseKey,
Pose3, VSLAMConfig::PointKey, Point3> VSLAMFactorBase;
typedef NonlinearFactor2<VSLAMConfig,
VSLAMPoseKey, Pose3, VSLAMPointKey, Point3> VSLAMFactorBase;
/**
* Non-linear factor for a constraint derived from a 2D measurement,
@ -81,8 +81,8 @@ namespace gtsam {
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
//ar & BOOST_SERIALIZATION_NVP(key1_);
//ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(z_);
ar & BOOST_SERIALIZATION_NVP(K_);
}

8
cpp/VSLAMGraph.cpp
View File

@ -28,12 +28,12 @@ bool compareLandmark(const std::string& key,
const VSLAMConfig& feasible,
const VSLAMConfig& input) {
int j = atoi(key.substr(1, key.size() - 1).c_str());
return feasible.landmarkPoint(j).equals(input.landmarkPoint(j));
return feasible[VSLAMPointKey(j)].equals(input[VSLAMPointKey(j)]);
}
/* ************************************************************************* */
void VSLAMGraph::addLandmarkConstraint(int j, const gtsam::Point3& p) {
typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PointKey,Point3> NLE;
typedef NonlinearEquality<VSLAMConfig,VSLAMPointKey,Point3> NLE;
boost::shared_ptr<NLE> factor(new NLE(j, p));
push_back(factor);
}
@ -43,12 +43,12 @@ bool compareCamera(const std::string& key,
const VSLAMConfig& feasible,
const VSLAMConfig& input) {
int j = atoi(key.substr(1, key.size() - 1).c_str());
return feasible.cameraPose(j).equals(input.cameraPose(j));
return feasible[VSLAMPoseKey(j)].equals(input[VSLAMPoseKey(j)]);
}
/* ************************************************************************* */
void VSLAMGraph::addCameraConstraint(int j, const gtsam::Pose3& p) {
typedef NonlinearEquality<VSLAMConfig,VSLAMConfig::PoseKey,Pose3> NLE;
typedef NonlinearEquality<VSLAMConfig,VSLAMPoseKey,Pose3> NLE;
boost::shared_ptr<NLE> factor(new NLE(j,p));
push_back(factor);
}

46
cpp/testSQP.cpp
View File

@ -493,9 +493,9 @@ TEST (SQP, stereo_truth ) {
// create truth config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
truthConfig->addCameraPose(1, camera1.pose());
truthConfig->addCameraPose(2, camera2.pose());
truthConfig->addLandmarkPoint(1, landmark);
truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark);
// create graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
@ -560,15 +560,15 @@ TEST (SQP, stereo_truth_noisy ) {
// create truth config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
truthConfig->addCameraPose(1, camera1.pose());
truthConfig->addCameraPose(2, camera2.pose());
truthConfig->addLandmarkPoint(1, landmark);
truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark);
// create config
boost::shared_ptr<VSLAMConfig> noisyConfig(new VSLAMConfig);
noisyConfig->addCameraPose(1, camera1.pose());
noisyConfig->addCameraPose(2, camera2.pose());
noisyConfig->addLandmarkPoint(1, landmarkNoisy);
noisyConfig->insert(1, camera1.pose());
noisyConfig->insert(2, camera2.pose());
noisyConfig->insert(1, landmarkNoisy);
// create graph
shared_ptr<VSLAMGraph> graph(new VSLAMGraph());
@ -629,8 +629,8 @@ namespace sqp_stereo {
// binary constraint between landmarks
/** g(x) = x-y = 0 */
Vector g(const VSLAMConfig& config, const list<string>& keys) {
return config.landmarkPoint(getNum(keys.front())).vector()
- config.landmarkPoint(getNum(keys.back())).vector();
return config[VSLAMPointKey(getNum(keys.front()))].vector()
- config[VSLAMPointKey(getNum(keys.back()))].vector();
}
/** jacobian at l1 */
@ -705,10 +705,10 @@ boost::shared_ptr<VSLAMConfig> stereoExampleTruthConfig() {
// create config
boost::shared_ptr<VSLAMConfig> truthConfig(new VSLAMConfig);
truthConfig->addCameraPose(1, camera1.pose());
truthConfig->addCameraPose(2, camera2.pose());
truthConfig->addLandmarkPoint(1, landmark1);
truthConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
truthConfig->insert(1, camera1.pose());
truthConfig->insert(2, camera2.pose());
truthConfig->insert(1, landmark1);
truthConfig->insert(2, landmark2); // create two landmarks in same place
return truthConfig;
}
@ -763,10 +763,10 @@ TEST (SQP, stereo_sqp_noisy ) {
// noisy config
boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig);
initConfig->addCameraPose(1, pose1);
initConfig->addCameraPose(2, pose2);
initConfig->addLandmarkPoint(1, landmark1);
initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
initConfig->insert(1, pose1);
initConfig->insert(2, pose2);
initConfig->insert(1, landmark1);
initConfig->insert(2, landmark2); // create two landmarks in same place
// create ordering
Ordering ord;
@ -828,10 +828,10 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) {
// noisy config
boost::shared_ptr<VSLAMConfig> initConfig(new VSLAMConfig);
initConfig->addCameraPose(1, pose1);
initConfig->addCameraPose(2, pose2);
initConfig->addLandmarkPoint(1, landmark1);
initConfig->addLandmarkPoint(2, landmark2); // create two landmarks in same place
initConfig->insert(1, pose1);
initConfig->insert(2, pose2);
initConfig->insert(1, landmark1);
initConfig->insert(2, landmark2); // create two landmarks in same place
// create ordering with lagrange multiplier included
Ordering ord;

166
cpp/testTupleConfig.cpp Normal file
View File

@ -0,0 +1,166 @@
/*
* testTupleConfig.cpp
*
* Created on: Jan 13, 2010
* Author: richard
*/
#include <CppUnitLite/TestHarness.h>
#include <stdexcept>
#include <Pose2.h>
#include <Point2.h>
#include "TupleConfig.h"
#include "Vector.h"
using namespace gtsam;
using namespace std;
typedef Symbol<Pose2, 'x'> PoseKey;
typedef Symbol<Point2, 'l'> PointKey;
typedef PairConfig<PoseKey, Pose2, PointKey, Point2> Config;
/* ************************************************************************* */
TEST( PairConfig, insert_equals1 )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Config expected;
expected.insert(1, x1);
expected.insert(2, x2);
expected.insert(1, l1);
expected.insert(2, l2);
Config actual;
actual.insert(1, x1);
actual.insert(2, x2);
actual.insert(1, l1);
actual.insert(2, l2);
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( PairConfig, insert_equals2 )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Config cfg1;
cfg1.insert(1, x1);
cfg1.insert(2, x2);
cfg1.insert(1, l1);
cfg1.insert(2, l2);
Config cfg2;
cfg2.insert(1, x1);
cfg2.insert(2, x2);
cfg2.insert(1, l1);
CHECK(!cfg1.equals(cfg2));
cfg2.insert(2, Point2(9,11));
CHECK(!cfg1.equals(cfg2));
}
///* ************************************************************************* */
//TEST( PairConfig, insert_duplicate )
//{
// Pose2 x1(1,2,3), x2(6,7,8);
// Point2 l1(4,5), l2(9,10);
//
// Config cfg1;
// cfg1.insert(1, x1);
// cfg1.insert(2, x2);
// cfg1.insert(1, l1);
// cfg1.insert(2, l2);
// cfg1.insert(2, l1);
//
// CHECK(assert_equal(l2, cfg1[PointKey(2)]));
// CHECK(cfg1.size() == 4);
// CHECK(cfg1.dim() == 10);
//}
/* ************************************************************************* */
TEST( PairConfig, size_dim )
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Config cfg1;
cfg1.insert(1, x1);
cfg1.insert(2, x2);
cfg1.insert(1, l1);
cfg1.insert(2, l2);
CHECK(cfg1.size() == 4);
CHECK(cfg1.dim() == 10);
}
/* ************************************************************************* */
TEST(PairConfig, at)
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Config cfg1;
cfg1.insert(1, x1);
cfg1.insert(2, x2);
cfg1.insert(1, l1);
cfg1.insert(2, l2);
CHECK(assert_equal(x1, cfg1[PoseKey(1)]));
CHECK(assert_equal(x2, cfg1[PoseKey(2)]));
CHECK(assert_equal(l1, cfg1[PointKey(1)]));
CHECK(assert_equal(l2, cfg1[PointKey(2)]));
bool caught = false;
try {
cfg1[PoseKey(3)];
} catch(invalid_argument e) {
caught = true;
}
CHECK(caught);
caught = false;
try {
cfg1[PointKey(3)];
} catch(invalid_argument e) {
caught = true;
}
CHECK(caught);
}
/* ************************************************************************* */
TEST(PairConfig, expmap)
{
Pose2 x1(1,2,3), x2(6,7,8);
Point2 l1(4,5), l2(9,10);
Config cfg1;
cfg1.insert(1, x1);
cfg1.insert(2, x2);
cfg1.insert(1, l1);
cfg1.insert(2, l2);
VectorConfig increment;
increment.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
increment.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
increment.insert("l1", Vector_(2, 1.0, 1.1));
increment.insert("l2", Vector_(2, 1.3, 1.4));
Config expected;
expected.insert(1, expmap(x1, Vector_(3, 1.0, 1.1, 1.2)));
expected.insert(2, expmap(x2, Vector_(3, 1.3, 1.4, 1.5)));
expected.insert(1, Point2(5.0, 6.1));
expected.insert(2, Point2(10.3, 11.4));
CHECK(assert_equal(expected, expmap(cfg1, increment)));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

8
cpp/testVSLAMConfig.cpp
View File

@ -16,12 +16,12 @@ TEST( VSLAMConfig, update_with_large_delta) {
// this test ensures that if the update for delta is larger than
// the size of the config, it only updates existing variables
VSLAMConfig init;
init.addCameraPose(1, Pose3());
init.addLandmarkPoint(1, Point3(1.0, 2.0, 3.0));
init.insert(1, Pose3());
init.insert(1, Point3(1.0, 2.0, 3.0));
VSLAMConfig expected;
expected.addCameraPose(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.addLandmarkPoint(1, Point3(1.1, 2.1, 3.1));
expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1)));
expected.insert(1, Point3(1.1, 2.1, 3.1));
VectorConfig delta;
delta.insert("x1", Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1));

8
cpp/testVSLAMFactor.cpp
View File

@ -36,8 +36,8 @@ TEST( VSLAMFactor, error )
// For the following configuration, the factor predicts 320,240
VSLAMConfig config;
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.addCameraPose(1, x1);
Point3 l1; config.addLandmarkPoint(1, l1);
Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1);
Point3 l1; config.insert(1, l1);
CHECK(assert_equal(Point2(320.,240.),factor->predict(x1,l1)));
// Which yields an error of 3^2/2 = 4.5
@ -65,8 +65,8 @@ TEST( VSLAMFactor, error )
delta.insert("l1",Vector_(3, 1.,2.,3.));
VSLAMConfig actual_config = expmap(config, delta);
VSLAMConfig expected_config;
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.addCameraPose(1, x2);
Point3 l2(1,2,3); expected_config.addLandmarkPoint(1, l2);
Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2);
Point3 l2(1,2,3); expected_config.insert(1, l2);
CHECK(assert_equal(expected_config,actual_config,1e-9));
}

36
cpp/testVSLAMGraph.cpp
View File

@ -76,12 +76,12 @@ TEST( VSLAMGraph, optimizeLM)
// Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
initialEstimate->addCameraPose(1, camera1);
initialEstimate->addCameraPose(2, camera2);
initialEstimate->addLandmarkPoint(1, landmark1);
initialEstimate->addLandmarkPoint(2, landmark2);
initialEstimate->addLandmarkPoint(3, landmark3);
initialEstimate->addLandmarkPoint(4, landmark4);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
// Create an ordering of the variables
list<string> keys;
@ -119,12 +119,12 @@ TEST( VSLAMGraph, optimizeLM2)
// Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
initialEstimate->addCameraPose(1, camera1);
initialEstimate->addCameraPose(2, camera2);
initialEstimate->addLandmarkPoint(1, landmark1);
initialEstimate->addLandmarkPoint(2, landmark2);
initialEstimate->addLandmarkPoint(3, landmark3);
initialEstimate->addLandmarkPoint(4, landmark4);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
// Create an ordering of the variables
list<string> keys;
@ -163,12 +163,12 @@ TEST( VSLAMGraph, CHECK_ORDERING)
// Create an initial configuration corresponding to the ground truth
boost::shared_ptr<VSLAMConfig> initialEstimate(new VSLAMConfig);
initialEstimate->addCameraPose(1, camera1);
initialEstimate->addCameraPose(2, camera2);
initialEstimate->addLandmarkPoint(1, landmark1);
initialEstimate->addLandmarkPoint(2, landmark2);
initialEstimate->addLandmarkPoint(3, landmark3);
initialEstimate->addLandmarkPoint(4, landmark4);
initialEstimate->insert(1, camera1);
initialEstimate->insert(2, camera2);
initialEstimate->insert(1, landmark1);
initialEstimate->insert(2, landmark2);
initialEstimate->insert(3, landmark3);
initialEstimate->insert(4, landmark4);
shared_ptr<Ordering> ordering(new Ordering(graph->getOrdering()));