Pose2Config is now simply a typedef (note that for linking specific template instantiations were needed).
Frank Dellaert
parent
4a21fb9387
commit
710bce5cc4
|
|
@ -4,64 +4,18 @@
|
|||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include "VectorConfig.h"
|
||||
#include "Pose2Config.h"
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
#include "LieConfig-inl.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Pose2& Pose2Config::get(std::string key) const {
|
||||
std::map<std::string, Pose2>::const_iterator it = values_.find(key);
|
||||
if (it == values_.end()) throw std::invalid_argument("invalid key");
|
||||
return it->second;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Pose2Config::insert(const std::string& name, const Pose2& val) {
|
||||
values_.insert(make_pair(name, val));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Pose2Config::equals(const Pose2Config& expected, double tol) const {
|
||||
if (values_.size() != expected.values_.size()) return false;
|
||||
|
||||
// iterate over all nodes
|
||||
string j;
|
||||
Pose2 pj;
|
||||
FOREACH_PAIR(j, pj, values_)
|
||||
if(!pj.equals(expected.get(j),tol))
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Pose2Config::print(const std::string &s) const {
|
||||
std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n";
|
||||
std::string j; Pose2 pj;
|
||||
FOREACH_PAIR(j, pj, values_)
|
||||
pj.print(j + ": ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta) {
|
||||
Pose2Config newConfig;
|
||||
std::string j; Pose2 pj;
|
||||
FOREACH_PAIR(j, pj, c.values_) {
|
||||
if (delta.contains(j)) {
|
||||
const Vector& dj = delta[j];
|
||||
//check_size(j,vj,dj);
|
||||
newConfig.insert(j, expmap(pj,dj));
|
||||
} else
|
||||
newConfig.insert(j, pj);
|
||||
}
|
||||
return newConfig;
|
||||
}
|
||||
/** Explicit instantiation of templated methods and functions */
|
||||
template class LieConfig<Pose2>;
|
||||
template size_t dim(const LieConfig<Pose2>& c);
|
||||
template LieConfig<Pose2> expmap(const LieConfig<Pose2>& c, const Vector& delta);
|
||||
template LieConfig<Pose2> expmap(const LieConfig<Pose2>& c, const VectorConfig& delta);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: local version, should probably defined in LieConfig
|
||||
|
|
|
|||
|
|
@ -1,65 +1,20 @@
|
|||
/**
|
||||
* @file VectorConfig.cpp
|
||||
* @brief Pose2Graph Configuration
|
||||
* @file PoseConfig.cpp
|
||||
* @brief Configuration of 2D poses
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include "Pose2.h"
|
||||
#include "Testable.h"
|
||||
#include "LieConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class VectorConfig;
|
||||
|
||||
class Pose2Config: public Testable<Pose2Config> {
|
||||
|
||||
private:
|
||||
std::map<std::string, Pose2> values_;
|
||||
|
||||
public:
|
||||
|
||||
typedef std::map<std::string, Pose2>::const_iterator iterator;
|
||||
typedef iterator const_iterator;
|
||||
|
||||
Pose2Config() {}
|
||||
|
||||
Pose2Config(const Pose2Config &config) :values_(config.values_) {}
|
||||
|
||||
virtual ~Pose2Config() {}
|
||||
|
||||
const Pose2& get(std::string key) const;
|
||||
|
||||
void insert(const std::string& name, const Pose2& val);
|
||||
|
||||
inline Pose2Config& operator=(const Pose2Config& rhs) {
|
||||
values_ = rhs.values_;
|
||||
return (*this);
|
||||
}
|
||||
|
||||
bool equals(const Pose2Config& expected, double tol) const;
|
||||
|
||||
void print(const std::string &s) const;
|
||||
|
||||
void clear() { values_.clear(); }
|
||||
|
||||
int size() { return values_.size(); };
|
||||
|
||||
inline const_iterator begin() const {return values_.begin();}
|
||||
inline const_iterator end() const {return values_.end();}
|
||||
inline iterator begin() {return values_.begin();}
|
||||
inline iterator end() {return values_.end();}
|
||||
|
||||
friend Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
|
||||
};
|
||||
|
||||
/**
|
||||
* Add a delta config, needed for use in NonlinearOptimizer
|
||||
* Pose2Config is now simply a typedef
|
||||
*/
|
||||
Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
|
||||
typedef LieConfig<Pose2> Pose2Config;
|
||||
|
||||
/**
|
||||
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
|
||||
|
|
|
|||
|
|
@ -25,6 +25,22 @@ TEST( Pose2Config, pose2Circle )
|
|||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2Config, expmap )
|
||||
{
|
||||
// expected is circle shifted to right
|
||||
Pose2Config expected;
|
||||
expected.insert("p0", Pose2( 1.1, 0, M_PI_2));
|
||||
expected.insert("p1", Pose2( 0.1, 1, - M_PI ));
|
||||
expected.insert("p2", Pose2(-0.9, 0, - M_PI_2));
|
||||
expected.insert("p3", Pose2( 0.1, -1, 0 ));
|
||||
|
||||
// Note expmap coordinates are in local coordinates, so shifting to right requires thought !!!
|
||||
Vector delta = Vector_(12, 0.0,-0.1,0.0, -0.1,0.0,0.0, 0.0,0.1,0.0, 0.1,0.0,0.0);
|
||||
Pose2Config actual = expmap(pose2Circle(4,1.0,'p'),delta);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue