Created Pose3Config, Pose3Graph, and tested loop closure optimization.
Frank Dellaert
parent
2fe02dbaa7
commit
1a96ef41cf
44
.cproject
44
.cproject
|
|
@ -340,6 +340,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -355,6 +356,7 @@
|
|||
</target>
|
||||
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -378,6 +380,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -401,6 +404,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -416,6 +420,7 @@
|
|||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -423,6 +428,7 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -462,6 +468,7 @@
|
|||
</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>
|
||||
|
|
@ -469,7 +476,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>
|
||||
|
|
@ -477,6 +483,7 @@
|
|||
</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>
|
||||
|
|
@ -500,6 +507,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testVSLAMGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -507,6 +515,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testNonlinearEquality.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -522,6 +531,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testNonlinearConstraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -561,6 +571,7 @@
|
|||
</target>
|
||||
<target name="testControlGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testControlGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -576,6 +587,7 @@
|
|||
</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>
|
||||
|
|
@ -599,6 +611,7 @@
|
|||
</target>
|
||||
<target name="testPose3Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3Factor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -606,6 +619,7 @@
|
|||
</target>
|
||||
<target name="testUrbanGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testUrbanGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -613,6 +627,7 @@
|
|||
</target>
|
||||
<target name="testUrbanConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testUrbanConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -620,6 +635,7 @@
|
|||
</target>
|
||||
<target name="testUrbanMeasurement.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testUrbanMeasurement.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -635,6 +651,7 @@
|
|||
</target>
|
||||
<target name="testIterative.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testIterative.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -642,6 +659,7 @@
|
|||
</target>
|
||||
<target name="testGaussianISAM2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testGaussianISAM2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -665,6 +683,7 @@
|
|||
</target>
|
||||
<target name="testPose2Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose2Factor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -672,6 +691,7 @@
|
|||
</target>
|
||||
<target name="timeRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>timeRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -679,6 +699,7 @@
|
|||
</target>
|
||||
<target name="install" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -686,7 +707,6 @@
|
|||
</target>
|
||||
<target name="testPose2Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2Graph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -694,14 +714,30 @@
|
|||
</target>
|
||||
<target name="testPose2Config.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2Config.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3Config.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3Config.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3Graph.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>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -709,6 +745,7 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -716,6 +753,7 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -194,18 +194,27 @@ testSimulated3D_SOURCES = testSimulated3D.cpp
|
|||
testSimulated3D_LDADD = libgtsam.la
|
||||
check_PROGRAMS += testSimulated3D
|
||||
|
||||
# Pose constraints
|
||||
headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h Pose3Factor.h
|
||||
# 2D Pose constraints
|
||||
headers += BetweenFactor.h Pose2Factor.h Pose2Prior.h
|
||||
sources += Pose2Config.cpp Pose2Graph.cpp
|
||||
check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph testPose3Factor
|
||||
check_PROGRAMS += testPose2Factor testPose2Config testPose2Graph
|
||||
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
|
||||
|
||||
# 3D Pose constraints
|
||||
headers += Pose3Factor.h
|
||||
sources += Pose3Config.cpp Pose3Graph.cpp
|
||||
check_PROGRAMS += testPose3Factor testPose3Config testPose3Graph
|
||||
testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
|
||||
testPose3Factor_LDADD = libgtsam.la
|
||||
testPose3Config_SOURCES = $(example) testPose3Config.cpp
|
||||
testPose3Config_LDADD = libgtsam.la
|
||||
testPose3Graph_SOURCES = $(example) testPose3Graph.cpp
|
||||
testPose3Graph_LDADD = libgtsam.la
|
||||
|
||||
# Cameras
|
||||
sources += CalibratedCamera.cpp SimpleCamera.cpp
|
||||
|
|
|
|||
|
|
@ -0,0 +1,40 @@
|
|||
/**
|
||||
* @file Pose3Config.cpp
|
||||
* @brief Configuration of 3D poses
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#include "Pose3Config.h"
|
||||
#include "LieConfig-inl.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/** Explicit instantiation of templated methods and functions */
|
||||
template class LieConfig<Pose3>;
|
||||
template size_t dim(const LieConfig<Pose3>& c);
|
||||
template LieConfig<Pose3> expmap(const LieConfig<Pose3>& c, const Vector& delta);
|
||||
template LieConfig<Pose3> expmap(const LieConfig<Pose3>& c, const VectorConfig& delta);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// TODO: local version, should probably defined in LieConfig
|
||||
static string symbol(char c, int index) {
|
||||
stringstream ss;
|
||||
ss << c << index;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Pose3Config pose3Circle(size_t n, double R, char c) {
|
||||
Pose3Config x;
|
||||
double theta = 0, dtheta = 2*M_PI/n;
|
||||
// Vehicle at p0 is looking towards y axis
|
||||
Rot3 R0(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
|
||||
for (size_t i = 0; i < n; i++, theta += dtheta)
|
||||
x.insert(symbol(c,i), Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
/**
|
||||
* @file Pose3Config.cpp
|
||||
* @brief Configuration of 3D poses
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Pose3.h"
|
||||
#include "LieConfig.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Pose3Config is now simply a typedef
|
||||
*/
|
||||
typedef LieConfig<Pose3> Pose3Config;
|
||||
|
||||
/**
|
||||
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0,0)
|
||||
* The convention used is the Navlab/Aerospace convention: X-forward,Y-right,Z-down
|
||||
* @param n number of poses
|
||||
* @param R radius of circle
|
||||
* @param c character to use for keys
|
||||
* @return circle of n 2D poses
|
||||
*/
|
||||
Pose3Config pose3Circle(size_t n, double R, char c = 'p');
|
||||
|
||||
} // namespace
|
||||
|
|
@ -5,18 +5,11 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include "Pose3.h"
|
||||
#include "LieConfig.h"
|
||||
#include "Pose3Config.h"
|
||||
#include "BetweenFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A config specifically for 3D poses
|
||||
*/
|
||||
typedef LieConfig<Pose3> Pose3Config;
|
||||
|
||||
/**
|
||||
* A Factor for 3D pose measurements
|
||||
* This is just a typedef now
|
||||
|
|
|
|||
|
|
@ -0,0 +1,22 @@
|
|||
/**
|
||||
* @file Pose3Graph.cpp
|
||||
* @brief A factor graph for the 2D PoseSLAM problem
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
*/
|
||||
//#include "NonlinearOptimizer-inl.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "Pose3Graph.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// explicit instantiation so all the code is there and we can link with it
|
||||
template class FactorGraph<NonlinearFactor<gtsam::Pose3Config> > ;
|
||||
template class NonlinearFactorGraph<Pose3Config> ;
|
||||
//template class NonlinearOptimizer<Pose3Graph, Pose3Config> ;
|
||||
|
||||
bool Pose3Graph::equals(const Pose3Graph& p, double tol) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/**
|
||||
* @file Pose3Graph.h
|
||||
* @brief A factor graph for the 3D PoseSLAM problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "Pose3Factor.h"
|
||||
#include "Pose3Config.h"
|
||||
|
||||
namespace gtsam{
|
||||
|
||||
/**
|
||||
* Non-linear factor graph for visual SLAM
|
||||
*/
|
||||
class Pose3Graph : public gtsam::NonlinearFactorGraph<Pose3Config> {
|
||||
|
||||
public:
|
||||
|
||||
/** default constructor is empty graph */
|
||||
Pose3Graph() {}
|
||||
|
||||
/**
|
||||
* equals
|
||||
*/
|
||||
bool equals(const Pose3Graph& p, double tol=1e-9) const;
|
||||
|
||||
/**
|
||||
* Add a factor without having to do shared factor dance
|
||||
*/
|
||||
inline void add(const std::string& key1, const std::string& key2,
|
||||
const Pose3& measured, const Matrix& covariance) {
|
||||
push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -0,0 +1,61 @@
|
|||
/**
|
||||
* @file testPose3Config.cpp
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "Pose3Config.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// The world coordinate system has z pointing up, y north, x east
|
||||
// The vehicle has X forward, Y right, Z down
|
||||
Rot3 R1(Point3( 0, 1, 0), Point3( 1, 0, 0), Point3(0, 0, -1));
|
||||
Rot3 R2(Point3(-1, 0, 0), Point3( 0, 1, 0), Point3(0, 0, -1));
|
||||
Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1));
|
||||
Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1));
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3Config, pose3Circle )
|
||||
{
|
||||
// expected is 4 poses tangent to circle with radius 1m
|
||||
Pose3Config expected;
|
||||
expected.insert("p0", Pose3(R1, Point3( 1, 0, 0)));
|
||||
expected.insert("p1", Pose3(R2, Point3( 0, 1, 0)));
|
||||
expected.insert("p2", Pose3(R3, Point3(-1, 0, 0)));
|
||||
expected.insert("p3", Pose3(R4, Point3( 0,-1, 0)));
|
||||
|
||||
Pose3Config actual = pose3Circle(4,1.0,'p');
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3Config, expmap )
|
||||
{
|
||||
// expected is circle shifted to East
|
||||
Pose3Config expected;
|
||||
expected.insert("p0", Pose3(R1, Point3( 1.1, 0, 0)));
|
||||
expected.insert("p1", Pose3(R2, Point3( 0.1, 1, 0)));
|
||||
expected.insert("p2", Pose3(R3, Point3(-0.9, 0, 0)));
|
||||
expected.insert("p3", Pose3(R4, Point3( 0.1,-1, 0)));
|
||||
|
||||
// Note expmap coordinates are in global coordinates with non-compose expmap
|
||||
// so shifting to East requires little thought, different from with Pose2 !!!
|
||||
Vector delta = Vector_(24,
|
||||
0.0,0.0,0.0, 0.1, 0.0, 0.0,
|
||||
0.0,0.0,0.0, 0.1, 0.0, 0.0,
|
||||
0.0,0.0,0.0, 0.1, 0.0, 0.0,
|
||||
0.0,0.0,0.0, 0.1, 0.0, 0.0);
|
||||
Pose3Config actual = expmap(pose3Circle(4,1.0,'p'),delta);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -7,6 +7,7 @@
|
|||
#include <iostream>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "Pose3Factor.h"
|
||||
#include "LieConfig-inl.h"
|
||||
|
|
@ -37,13 +38,6 @@ TEST( Pose3Factor, error )
|
|||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose3Factor, simple_circle )
|
||||
{
|
||||
Pose3Config expected, actual;
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,87 @@
|
|||
/**
|
||||
* @file testPose3Graph.cpp
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "Ordering.h"
|
||||
#include "Pose3Graph.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// common measurement covariance
|
||||
static Matrix covariance = eye(6);
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool poseCompare(const std::string& key,
|
||||
const gtsam::Pose3Config& feasible,
|
||||
const gtsam::Pose3Config& input) {
|
||||
return feasible.get(key).equals(input.get(key));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// test optimization with 6 poses arranged in a hexagon and a loop closure
|
||||
TEST(Pose3Graph, optimizeCircle) {
|
||||
|
||||
// Create a hexagon of poses
|
||||
Pose3Config hexagon = pose3Circle(6,1.0,'p');
|
||||
Pose3 p0 = hexagon["p0"], p1 = hexagon["p1"];
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
Pose3Graph fg;
|
||||
Pose3Config feasible;
|
||||
feasible.insert("p0", p0);
|
||||
fg.push_back(Pose3Graph::sharedFactor(
|
||||
new NonlinearEquality<Pose3Config>("p0", feasible, dim(Pose3()), poseCompare)));
|
||||
Pose3 delta = between(p0,p1);
|
||||
fg.add("p0", "p1", delta, covariance);
|
||||
fg.add("p1", "p2", delta, covariance);
|
||||
fg.add("p2", "p3", delta, covariance);
|
||||
fg.add("p3", "p4", delta, covariance);
|
||||
fg.add("p4", "p5", delta, covariance);
|
||||
fg.add("p5", "p0", delta, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose3Config> initial(new Pose3Config());
|
||||
initial->insert("p0", p0);
|
||||
initial->insert("p1", expmap(hexagon["p1"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial->insert("p2", expmap(hexagon["p2"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial->insert("p3", expmap(hexagon["p3"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
initial->insert("p4", expmap(hexagon["p4"],Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1)));
|
||||
initial->insert("p5", expmap(hexagon["p5"],Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1)));
|
||||
|
||||
// Choose an ordering and optimize
|
||||
Ordering ordering;
|
||||
ordering += "p0","p1","p2","p3","p4","p5";
|
||||
typedef NonlinearOptimizer<Pose3Graph, Pose3Config> Optimizer;
|
||||
Optimizer optimizer(fg, ordering, initial);
|
||||
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
|
||||
// Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
|
||||
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
|
||||
|
||||
Pose3Config actual = *optimizer.config();
|
||||
|
||||
// Check with ground truth
|
||||
CHECK(assert_equal(hexagon, actual,1e-5));
|
||||
|
||||
// Check loop closure
|
||||
CHECK(assert_equal(delta,between(actual["p5"],actual["p0"]),1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue