2D Pose SLAM: created a new templated factor to accommodate GPS measurements, and as part of the refactor I consolidated all Pose2 SLAM classes in pose2SLAM.h. For backwards compatibility it contains
typedef pose2SLAM::Prior Pose2Prior; typedef pose2SLAM::Odometry Pose2Factor; typedef pose2SLAM::Constraint Pose2Constraint; typedef pose2SLAM::Config Pose2Config; typedef pose2SLAM::Graph Pose2Graph;release/4.3a0
Frank Dellaert
parent
3c9c8bcfe5
commit
99db4c37d8
16
.cproject
16
.cproject
|
|
@ -468,7 +468,6 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -476,6 +475,7 @@
|
|||
</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,7 +483,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -585,14 +584,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2Constraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2Constraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -689,10 +680,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testPose2SLAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2Graph.run</buildTarget>
|
||||
<buildTarget>testPose2SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -723,7 +714,6 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -188,20 +188,21 @@ testSimulated3D_LDADD = libgtsam.la
|
|||
check_PROGRAMS += testSimulated3D
|
||||
|
||||
# Pose SLAM headers
|
||||
headers += BetweenFactor.h LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
|
||||
headers += BetweenFactor.h PriorFactor.h
|
||||
headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
|
||||
|
||||
# 2D Pose constraints
|
||||
headers += Pose2Prior.h
|
||||
sources += Pose2Graph.cpp
|
||||
check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose2Prior
|
||||
# 2D Pose SLAM
|
||||
headers +=
|
||||
sources += pose2SLAM.cpp
|
||||
check_PROGRAMS += testPose2Factor testPose2Config testPose2SLAM testPose2Prior
|
||||
testPose2Prior_SOURCES = $(example) testPose2Prior.cpp
|
||||
testPose2Prior_LDADD = libgtsam.la
|
||||
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
|
||||
testPose2Factor_LDADD = libgtsam.la
|
||||
testPose2Config_SOURCES = $(example) testPose2Config.cpp
|
||||
testPose2Config_LDADD = libgtsam.la
|
||||
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
|
||||
testPose2Graph_LDADD = libgtsam.la
|
||||
testPose2Prior_SOURCES = $(example) testPose2Prior.cpp
|
||||
testPose2Prior_LDADD = libgtsam.la
|
||||
testPose2SLAM_SOURCES = $(example) testPose2SLAM.cpp
|
||||
testPose2SLAM_LDADD = libgtsam.la
|
||||
|
||||
# 2D SLAM using Bearing and Range
|
||||
headers +=
|
||||
|
|
|
|||
|
|
@ -1,30 +0,0 @@
|
|||
/**
|
||||
* @file Pose2Config.cpp
|
||||
* @brief Configuration of 2D poses
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "Pose2Config.h"
|
||||
#include "LieConfig-inl.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Explicit instantiation of templated methods and functions */
|
||||
template class LieConfig<Symbol<Pose2,'x'>,Pose2>;
|
||||
template size_t dim(const Pose2Config& c);
|
||||
template Pose2Config expmap(const Pose2Config& c, const Vector& delta);
|
||||
template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2Config pose2Circle(size_t n, double R) {
|
||||
Pose2Config x;
|
||||
double theta = 0, dtheta = 2*M_PI/n;
|
||||
for(size_t i=0;i<n;i++, theta+=dtheta)
|
||||
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace
|
||||
|
|
@ -1,28 +0,0 @@
|
|||
/**
|
||||
* @file Pose2Config.cpp
|
||||
* @brief Configuration of 2D poses
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Pose2.h"
|
||||
#include "LieConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Pose2Config is now simply a typedef
|
||||
*/
|
||||
typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
|
||||
|
||||
/**
|
||||
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
|
||||
* @param n number of poses
|
||||
* @param R radius of circle
|
||||
* @param c character to use for keys
|
||||
* @return circle of n 2D poses
|
||||
*/
|
||||
Pose2Config pose2Circle(size_t n, double R);
|
||||
|
||||
} // namespace
|
||||
|
|
@ -1,17 +0,0 @@
|
|||
/**
|
||||
* @file Pose2Factor.H
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "BetweenFactor.h"
|
||||
#include "Pose2.h"
|
||||
#include "Pose2Config.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** This is just a typedef now */
|
||||
typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
/**
|
||||
* @file Pose2Graph.cpp
|
||||
* @brief A factor graph for the 2D PoseSLAM problem
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
*/
|
||||
|
||||
#include "Pose2Graph.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "graph-inl.h"
|
||||
#include "LieConfig-inl.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Explicit instantiation of templated methods and functions */
|
||||
template class LieConfig<Symbol<Pose2,'x'>,Pose2>;
|
||||
template size_t dim(const Pose2Config& c);
|
||||
template Pose2Config expmap(const Pose2Config& c, const Vector& delta);
|
||||
template Pose2Config expmap(const Pose2Config& c, const VectorConfig& delta);
|
||||
|
||||
// explicit instantiation so all the code is there and we can link with it
|
||||
template class FactorGraph<NonlinearFactor<Pose2Config> > ;
|
||||
template class NonlinearFactorGraph<Pose2Config> ;
|
||||
template class NonlinearEquality<Pose2Config,Pose2Config::Key,Pose2> ;
|
||||
template class NonlinearOptimizer<Pose2Graph, Pose2Config>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose2Config pose2Circle(size_t n, double R) {
|
||||
Pose2Config x;
|
||||
double theta = 0, dtheta = 2*M_PI/n;
|
||||
for(size_t i=0;i<n;i++, theta+=dtheta)
|
||||
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Pose2Graph::addConstraint(const Pose2Config::Key& key, const Pose2& pose) {
|
||||
push_back(sharedFactor(new NonlinearEquality<Pose2Config, Pose2Config::Key,
|
||||
Pose2> (key, pose)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Pose2Graph::equals(const Pose2Graph& p, double tol) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,74 +0,0 @@
|
|||
/**
|
||||
* @file Pose2Graph.h
|
||||
* @brief A factor graph for the 2D PoseSLAM problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "Pose2.h"
|
||||
#include "LieConfig.h"
|
||||
#include "BetweenFactor.h"
|
||||
#include "Key.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Pose2Config is now simply a typedef
|
||||
*/
|
||||
typedef LieConfig<Symbol<Pose2,'x'>,Pose2> Pose2Config;
|
||||
|
||||
typedef BetweenFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Factor;
|
||||
|
||||
/**
|
||||
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
|
||||
* @param n number of poses
|
||||
* @param R radius of circle
|
||||
* @param c character to use for keys
|
||||
* @return circle of n 2D poses
|
||||
*/
|
||||
Pose2Config pose2Circle(size_t n, double R);
|
||||
|
||||
|
||||
/**
|
||||
* Non-linear factor graph for visual SLAM
|
||||
*/
|
||||
class Pose2Graph: public gtsam::NonlinearFactorGraph<Pose2Config> {
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor is empty graph */
|
||||
Pose2Graph() {
|
||||
}
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const Pose2Graph& p, double tol = 1e-9) const;
|
||||
|
||||
/**
|
||||
* Add a factor without having to do shared factor dance
|
||||
*/
|
||||
inline void add(const Pose2Config::Key& key1, const Pose2Config::Key& key2,
|
||||
const Pose2& measured, const Matrix& covariance) {
|
||||
push_back(sharedFactor(new Pose2Factor(key1, key2, measured, covariance)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Add an equality constraint on a pose
|
||||
* @param key of pose
|
||||
* @param pose which pose to constrain it to
|
||||
*/
|
||||
void addConstraint(const Pose2Config::Key& key, const Pose2& pose = Pose2());
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,17 +1,12 @@
|
|||
/**
|
||||
* @file Pose2Prior.h
|
||||
* @authors Michael Kaess
|
||||
* @file PriorFactor.h
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <ostream>
|
||||
#include "NonlinearFactor.h"
|
||||
#include "GaussianFactor.h"
|
||||
#include "Pose2.h"
|
||||
#include "Matrix.h"
|
||||
#include "Key.h"
|
||||
#include "Pose2Graph.h"
|
||||
#include "ostream"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -68,7 +63,4 @@ namespace gtsam {
|
|||
}
|
||||
};
|
||||
|
||||
/** This is just a typedef now */
|
||||
typedef PriorFactor<Pose2Config, Pose2Config::Key, Pose2> Pose2Prior;
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
/**
|
||||
* @file pose2SLAM.cpp
|
||||
* @brief: bearing/range measurements in 2D plane
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include "pose2SLAM.h"
|
||||
#include "LieConfig-inl.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
|
||||
// Use pose2SLAM namespace for specific SLAM instance
|
||||
namespace gtsam {
|
||||
|
||||
using namespace pose2SLAM;
|
||||
INSTANTIATE_LIE_CONFIG(Key, Pose2)
|
||||
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
|
||||
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
|
||||
|
||||
namespace pose2SLAM {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Config circle(size_t n, double R) {
|
||||
Config x;
|
||||
double theta = 0, dtheta = 2 * M_PI / n;
|
||||
for (size_t i = 0; i < n; i++, theta += dtheta)
|
||||
x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta));
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Graph::addConstraint(const Key& i, const Pose2& p) {
|
||||
sharedFactor factor(new Constraint(i, p));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) {
|
||||
sharedFactor factor(new Prior(i, p, cov));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) {
|
||||
sharedFactor factor(new Odometry(i, j, z, cov));
|
||||
push_back(factor);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // pose2SLAM
|
||||
|
||||
} // gtsam
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
/**
|
||||
* @file pose2SLAM.h
|
||||
* @brief: bearing/range measurements in 2D plane
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Key.h"
|
||||
#include "Pose2.h"
|
||||
#include "LieConfig.h"
|
||||
#include "PriorFactor.h"
|
||||
#include "BetweenFactor.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "NonlinearOptimizer.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Use pose2SLAM namespace for specific SLAM instance
|
||||
namespace pose2SLAM {
|
||||
|
||||
// Keys and Config
|
||||
typedef Symbol<Pose2, 'x'> Key;
|
||||
typedef LieConfig<Key, Pose2> Config;
|
||||
|
||||
/**
|
||||
* Create a circle of n 2D poses tangent to circle of radius R, first pose at (R,0)
|
||||
* @param n number of poses
|
||||
* @param R radius of circle
|
||||
* @param c character to use for keys
|
||||
* @return circle of n 2D poses
|
||||
*/
|
||||
Config circle(size_t n, double R);
|
||||
|
||||
// Factors
|
||||
typedef PriorFactor<Config, Key, Pose2> Prior;
|
||||
typedef BetweenFactor<Config, Key, Pose2> Odometry;
|
||||
typedef NonlinearEquality<Config, Key, Pose2> Constraint;
|
||||
|
||||
// Graph
|
||||
struct Graph: public NonlinearFactorGraph<Config> {
|
||||
void addConstraint(const Key& i, const Pose2& p);
|
||||
void addPrior(const Key& i, const Pose2& p, const Matrix& cov);
|
||||
void add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov);
|
||||
};
|
||||
|
||||
// Optimizer
|
||||
typedef NonlinearOptimizer<Graph, Config> Optimizer;
|
||||
|
||||
} // pose2SLAM
|
||||
|
||||
/**
|
||||
* Backwards compatibility
|
||||
*/
|
||||
typedef pose2SLAM::Prior Pose2Prior;
|
||||
typedef pose2SLAM::Odometry Pose2Factor;
|
||||
typedef pose2SLAM::Constraint Pose2Constraint;
|
||||
typedef pose2SLAM::Config Pose2Config;
|
||||
typedef pose2SLAM::Graph Pose2Graph;
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -12,7 +12,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "Pose2Graph.h"
|
||||
#include "pose2SLAM.h"
|
||||
#include "LieConfig-inl.h"
|
||||
#include "graph-inl.h"
|
||||
|
||||
|
|
@ -43,8 +43,8 @@ TEST( Graph, composePoses )
|
|||
{
|
||||
Pose2Graph graph;
|
||||
Matrix cov = eye(3);
|
||||
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(1,2, Pose2(2.0, 0.0, 0.0), cov)));
|
||||
graph.push_back(boost::shared_ptr<Pose2Factor>(new Pose2Factor(2,3, Pose2(3.0, 0.0, 0.0), cov)));
|
||||
graph.add(1,2, Pose2(2.0, 0.0, 0.0), cov);
|
||||
graph.add(2,3, Pose2(3.0, 0.0, 0.0), cov);
|
||||
|
||||
PredecessorMap<Pose2Config::Key> tree;
|
||||
tree.insert(1,2);
|
||||
|
|
|
|||
|
|
@ -12,8 +12,7 @@ using namespace boost::assign;
|
|||
#include "Ordering.h"
|
||||
#include "iterative.h"
|
||||
#include "smallExample.h"
|
||||
#include "Pose2Graph.h"
|
||||
#include "Pose2Prior.h"
|
||||
#include "pose2SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -101,8 +100,6 @@ TEST( Iterative, conjugateGradientDescent_hard_constraint )
|
|||
/* ************************************************************************* */
|
||||
TEST( Iterative, conjugateGradientDescent_soft_constraint )
|
||||
{
|
||||
typedef Pose2Config::Key Key;
|
||||
|
||||
Pose2Config config;
|
||||
config.insert(1, Pose2(0.,0.,0.));
|
||||
config.insert(2, Pose2(1.5,0.,0.));
|
||||
|
|
@ -110,8 +107,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
|
|||
Pose2Graph graph;
|
||||
Matrix cov = eye(3);
|
||||
Matrix cov2 = eye(3) * 1e-10;
|
||||
graph.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(1.,0.,0.), cov)));
|
||||
graph.push_back(boost::shared_ptr<Pose2Prior>(new Pose2Prior(Key(1), Pose2(0.,0.,0.), cov2)));
|
||||
graph.add(1,2, Pose2(1.,0.,0.), cov);
|
||||
graph.addPrior(1, Pose2(0.,0.,0.), cov2);
|
||||
|
||||
VectorConfig zeros;
|
||||
zeros.insert("x1",zero(3));
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "Ordering-inl.h"
|
||||
#include "Pose2Graph.h"
|
||||
#include "pose2SLAM.h"
|
||||
|
||||
|
||||
using namespace std;
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "Pose2Graph.h"
|
||||
#include "pose2SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -21,7 +21,7 @@ TEST( Pose2Config, pose2Circle )
|
|||
expected.insert(2, Pose2(-1, 0, - M_PI_2));
|
||||
expected.insert(3, Pose2( 0, -1, 0 ));
|
||||
|
||||
Pose2Config actual = pose2Circle(4,1.0);
|
||||
Pose2Config actual = pose2SLAM::circle(4,1.0);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
|
|
@ -37,7 +37,7 @@ TEST( Pose2Config, expmap )
|
|||
|
||||
// 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),delta);
|
||||
Pose2Config actual = expmap(pose2SLAM::circle(4,1.0),delta);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "numericalDerivative.h"
|
||||
#include "Pose2Graph.h"
|
||||
#include "pose2SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "numericalDerivative.h"
|
||||
#include "Pose2Prior.h"
|
||||
#include "pose2SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ using namespace boost::assign;
|
|||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "Ordering.h"
|
||||
#include "Pose2Graph.h"
|
||||
#include "pose2SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -115,7 +115,7 @@ TEST(Pose2Graph, optimize) {
|
|||
TEST(Pose2Graph, optimizeCircle) {
|
||||
|
||||
// Create a hexagon of poses
|
||||
Pose2Config hexagon = pose2Circle(6,1.0);
|
||||
Pose2Config hexagon = pose2SLAM::circle(6,1.0);
|
||||
Pose2 p0 = hexagon[0], p1 = hexagon[1];
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/
|
||||
|
||||
# for maximum performance on Intel Core2 platform:
|
||||
#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS="-O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static
|
||||
#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -ftree-vectorize O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static
|
||||
Loading…
Reference in New Issue