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

Now Pose3Config == LieConfig

release/4.3a0
Frank Dellaert 2010-01-10 14:59:39 +00:00
parent 92c58e50a1
commit 43b2facd10
3 changed files with 16 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cpp/Pose3.h
View File

@ -68,6 +68,9 @@ namespace gtsam {
}
}; // Pose3 class
// global print
inline void print(const Pose3& p, const std::string& s = "") { p.print(s);}
// Dimensionality of the tangent space
inline size_t dim(const Pose3&) { return 6; }

15
cpp/Pose3Factor.h
View File

@ -6,25 +6,16 @@
#pragma once
#include <map>
#include "BetweenFactor.h"
#include "Pose3.h"
#include "LieConfig.h"
#include "BetweenFactor.h"
namespace gtsam {
/**
* A config specifically for 3D poses
*/
class Pose3Config: public std::map<std::string, Pose3> {
public:
const Pose3& get(std::string key) const {
const_iterator it = find(key);
if (it == end()) throw std::invalid_argument("invalid key");
return it->second;
}
};
typedef LieConfig<Pose3> Pose3Config;
/**
* A Factor for 3D pose measurements

12
cpp/testPose3Factor.cpp
View File

@ -9,6 +9,7 @@
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include "Pose3Factor.h"
#include "LieConfig-inl.h"
using namespace std;
using namespace gtsam;
@ -27,8 +28,8 @@ TEST( Pose3Factor, error )
// Create config
Pose3Config x;
x.insert(make_pair("t1",t1));
x.insert(make_pair("t2",t2));
x.insert("t1",t1);
x.insert("t2",t2);
// Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z)
Vector actual = factor.error_vector(x);
@ -36,6 +37,13 @@ TEST( Pose3Factor, error )
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( Pose3Factor, simple_circle )
{
Pose3Config expected, actual;
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
int main() {
TestResult tr;