Updated Pose2Config to expected coding standard
Frank Dellaert
parent
ff06cd757f
commit
a1c55b9f62
57
.cproject
57
.cproject
|
|
@ -300,7 +300,6 @@
|
|||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -308,7 +307,6 @@
|
|||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -324,7 +322,6 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -340,6 +337,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -347,7 +345,6 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -355,6 +352,7 @@
|
|||
</target>
|
||||
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -362,7 +360,6 @@
|
|||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -370,7 +367,6 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -378,6 +374,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -385,7 +382,6 @@
|
|||
</target>
|
||||
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -393,7 +389,6 @@
|
|||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -401,6 +396,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -408,7 +404,6 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -416,6 +411,7 @@
|
|||
</target>
|
||||
<target name="testVectorConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVectorConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -423,6 +419,7 @@
|
|||
</target>
|
||||
<target name="testPoint2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPoint2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -430,7 +427,6 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -438,7 +434,6 @@
|
|||
</target>
|
||||
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -446,7 +441,6 @@
|
|||
</target>
|
||||
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -454,7 +448,6 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -462,6 +455,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 +463,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 +470,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>
|
||||
|
|
@ -484,7 +478,6 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -492,7 +485,6 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -500,6 +492,7 @@
|
|||
</target>
|
||||
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -507,6 +500,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearEquality.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearEquality.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -514,7 +508,6 @@
|
|||
</target>
|
||||
<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSQP.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -522,6 +515,7 @@
|
|||
</target>
|
||||
<target name="testNonlinearConstraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testNonlinearConstraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -529,7 +523,6 @@
|
|||
</target>
|
||||
<target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSQPOptimizer.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -537,7 +530,6 @@
|
|||
</target>
|
||||
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testVSLAMConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -545,7 +537,6 @@
|
|||
</target>
|
||||
<target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testControlConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -553,7 +544,6 @@
|
|||
</target>
|
||||
<target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testControlPoint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -561,6 +551,7 @@
|
|||
</target>
|
||||
<target name="testControlGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testControlGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -568,7 +559,6 @@
|
|||
</target>
|
||||
<target name="testOrdering.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -576,6 +566,7 @@
|
|||
</target>
|
||||
<target name="testPose2Constraint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2Constraint.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -583,7 +574,6 @@
|
|||
</target>
|
||||
<target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testRot2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -591,7 +581,6 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -599,6 +588,7 @@
|
|||
</target>
|
||||
<target name="testPose3Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose3Factor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -606,6 +596,7 @@
|
|||
</target>
|
||||
<target name="testUrbanGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -613,6 +604,7 @@
|
|||
</target>
|
||||
<target name="testUrbanConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanConfig.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -620,6 +612,7 @@
|
|||
</target>
|
||||
<target name="testUrbanMeasurement.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanMeasurement.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -627,7 +620,6 @@
|
|||
</target>
|
||||
<target name="testUrbanOdometry.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testUrbanOdometry.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -635,6 +627,7 @@
|
|||
</target>
|
||||
<target name="testIterative.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testIterative.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -642,6 +635,7 @@
|
|||
</target>
|
||||
<target name="testGaussianISAM2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianISAM2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -649,7 +643,6 @@
|
|||
</target>
|
||||
<target name="testSubgraphPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSubgraphPreconditioner.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -657,14 +650,22 @@
|
|||
</target>
|
||||
<target name="testBayesNetPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesNetPreconditioner.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testPose2Factor.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/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -672,6 +673,7 @@
|
|||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -679,6 +681,7 @@
|
|||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
|
|||
|
|
@ -189,8 +189,8 @@ testSimulated3D_LDADD = libgtsam.la
|
|||
check_PROGRAMS += testSimulated3D
|
||||
|
||||
# Pose constraints
|
||||
headers += Pose2Config.h Pose2Factor.h Pose2Prior.h
|
||||
sources += Pose3Factor.cpp Pose2Graph.cpp
|
||||
headers += Pose2Factor.h Pose2Prior.h
|
||||
sources += Pose2Config.cpp Pose2Graph.cpp Pose3Factor.cpp
|
||||
check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor
|
||||
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
|
||||
testPose2Factor_LDADD = libgtsam.la
|
||||
|
|
|
|||
|
|
@ -0,0 +1,67 @@
|
|||
/**
|
||||
* @file VectorConfig.cpp
|
||||
* @brief Pose2Graph Configuration
|
||||
* @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)
|
||||
|
||||
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 Pose2Config::exmap(const VectorConfig& delta) const {
|
||||
Pose2Config newConfig;
|
||||
std::string j; Pose2 pj;
|
||||
FOREACH_PAIR(j, pj, values_) {
|
||||
if (delta.contains(j)) {
|
||||
const Vector& dj = delta[j];
|
||||
//check_size(j,vj,dj);
|
||||
newConfig.insert(j, pj.exmap(dj));
|
||||
} else
|
||||
newConfig.insert(j, pj);
|
||||
}
|
||||
return newConfig;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace
|
||||
|
|
@ -1,84 +1,57 @@
|
|||
/**
|
||||
* @file VectorConfig.cpp
|
||||
* @brief Pose2Graph Configuration
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <map>
|
||||
#include <boost/exception.hpp>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include "Pose2.h"
|
||||
#include "Testable.h"
|
||||
#include "VectorConfig.h"
|
||||
#include "Vector.h"
|
||||
|
||||
// trick from some reading group
|
||||
#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Pose2Config: public Testable<Pose2Config> {
|
||||
class VectorConfig;
|
||||
|
||||
private:
|
||||
std::map<std::string, Pose2> values_;
|
||||
class Pose2Config: public Testable<Pose2Config> {
|
||||
|
||||
public:
|
||||
typedef std::map<std::string, Pose2>::const_iterator iterator;
|
||||
typedef iterator const_iterator;
|
||||
private:
|
||||
std::map<std::string, Pose2> values_;
|
||||
|
||||
Pose2Config() {}
|
||||
Pose2Config(const Pose2Config &config) : values_(config.values_) { }
|
||||
virtual ~Pose2Config() { }
|
||||
public:
|
||||
|
||||
Pose2 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 insert(const std::string& name, const Pose2& val){
|
||||
values_.insert(make_pair(name, val));
|
||||
}
|
||||
typedef std::map<std::string, Pose2>::const_iterator iterator;
|
||||
typedef iterator const_iterator;
|
||||
|
||||
Pose2Config& operator=(Pose2Config& rhs) {
|
||||
values_ = rhs.values_;
|
||||
return (*this);
|
||||
}
|
||||
Pose2Config() {}
|
||||
|
||||
bool equals(const Pose2Config& expected, double tol) const {
|
||||
std::cerr << "Pose2Config::equals not implemented!" << std::endl;
|
||||
throw "Pose2Config::equals not implemented!";
|
||||
}
|
||||
Pose2Config(const Pose2Config &config) :values_(config.values_) {}
|
||||
|
||||
void print(const std::string &s) const {
|
||||
std::cout << "Pose2Config " << s << ", size " << values_.size() << "\n";
|
||||
std::string j; Pose2 v;
|
||||
FOREACH_PAIR(j, v, values_) {
|
||||
v.print(j + ": ");
|
||||
}
|
||||
}
|
||||
virtual ~Pose2Config() {}
|
||||
|
||||
/**
|
||||
* Add a delta config, needed for use in NonlinearOptimizer
|
||||
*/
|
||||
Pose2Config exmap(const VectorConfig& delta) const {
|
||||
Pose2Config newConfig;
|
||||
std::string j; Pose2 vj;
|
||||
FOREACH_PAIR(j, vj, values_) {
|
||||
if (delta.contains(j)) {
|
||||
const Vector& dj = delta[j];
|
||||
//check_size(j,vj,dj);
|
||||
newConfig.insert(j, vj.exmap(dj));
|
||||
} else {
|
||||
newConfig.insert(j, vj);
|
||||
}
|
||||
}
|
||||
return newConfig;
|
||||
}
|
||||
const Pose2& get(std::string key) const;
|
||||
|
||||
iterator begin() const { return values_.begin(); }
|
||||
iterator end() const { return values_.end(); }
|
||||
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;
|
||||
|
||||
/**
|
||||
* Add a delta config, needed for use in NonlinearOptimizer
|
||||
*/
|
||||
Pose2Config exmap(const VectorConfig& delta) const;
|
||||
|
||||
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();}
|
||||
};
|
||||
} // namespace
|
||||
|
|
|
|||
|
|
@ -6,9 +6,12 @@
|
|||
|
||||
/*STL/C++*/
|
||||
#include <iostream>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "Pose2Factor.h"
|
||||
|
|
@ -16,9 +19,9 @@
|
|||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace boost;
|
||||
|
||||
TEST( Pose2Factor, constructor )
|
||||
/* ************************************************************************* */
|
||||
TEST( Pose2Factor, linearize )
|
||||
{
|
||||
// create a factor between unknown poses p1 and p2
|
||||
Pose2 measured(2,2,M_PI_2);
|
||||
|
|
@ -36,10 +39,14 @@ TEST( Pose2Factor, constructor )
|
|||
config.insert("p1",p1);
|
||||
config.insert("p2",p2);
|
||||
|
||||
// Linearize
|
||||
boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
|
||||
|
||||
// expected
|
||||
// expected linearization
|
||||
// we need the minus signs below as "inverse_square_root"
|
||||
// uses SVD and the sign is simply arbitrary (but still a square root!)
|
||||
Matrix square_root_inverse_covariance = Matrix_(3,3,
|
||||
-2.0, 0.0, 0.0,
|
||||
0.0, -2.0, 0.0,
|
||||
0.0, 0.0, -10.0
|
||||
);
|
||||
Matrix expectedH1 = Matrix_(3,3,
|
||||
0.0,-1.0,-2.1,
|
||||
1.0, 0.0,-2.1,
|
||||
|
|
@ -50,53 +57,57 @@ TEST( Pose2Factor, constructor )
|
|||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0
|
||||
);
|
||||
// we need the minus signs as inverse_square_root uses SVD and sign is simply arbitrary (still a ssquare root!)
|
||||
Matrix square_root_inverse_covariance = Matrix_(3,3,
|
||||
-2.0, 0.0, 0.0,
|
||||
0.0, -2.0, 0.0,
|
||||
0.0, 0.0, -10.0
|
||||
);
|
||||
GaussianFactor expected(
|
||||
"p1", square_root_inverse_covariance*expectedH1,
|
||||
"p2", square_root_inverse_covariance*expectedH2,
|
||||
Vector_(3, 0.1, -0.1, 0.0), 1.0);
|
||||
|
||||
// Actual linearization
|
||||
boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config);
|
||||
CHECK(assert_equal(expected,*actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool poseCompare(const std::string& key,
|
||||
const gtsam::Pose2Config& feasible,
|
||||
const gtsam::Pose2Config& input) {
|
||||
return feasible.get(key).equals(input.get(key));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Pose2Factor, optimize) {
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
Pose2Graph fg;
|
||||
shared_ptr<Pose2Config> config = shared_ptr<Pose2Config>(new Pose2Config());
|
||||
Pose2Config feasible;
|
||||
feasible.insert("p0", Pose2(0,0,0));
|
||||
fg.push_back(Pose2Graph::sharedFactor(
|
||||
new NonlinearEquality<Pose2Config>("p0", feasible, Pose2().dim(), poseCompare)));
|
||||
config->insert("p0", Pose2(0,0,0));
|
||||
fg.push_back(Pose2Graph::sharedFactor(
|
||||
new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3,
|
||||
0.5, 0.0, 0.0,
|
||||
0.0, 0.5, 0.0,
|
||||
0.0, 0.0, 0.5))));
|
||||
config->insert("p1", Pose2(0,0,0));
|
||||
Ordering ordering;
|
||||
ordering.push_back("p0");
|
||||
ordering.push_back("p1");
|
||||
NonlinearOptimizer<Pose2Graph, Pose2Config> optimizer(fg, ordering, config);
|
||||
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15);
|
||||
Pose2 actual0 = optimizer.config()->get("p0");
|
||||
Pose2 actual1 = optimizer.config()->get("p1");
|
||||
Pose2 expected0 = Pose2(0,0,0);
|
||||
Pose2 expected1 = Pose2(1,2,M_PI_2);
|
||||
CHECK(assert_equal(expected0, actual0));
|
||||
CHECK(assert_equal(expected1, actual1));
|
||||
}
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose2Config> initial =
|
||||
boost::shared_ptr<Pose2Config>(new Pose2Config());
|
||||
initial->insert("p0", Pose2(0,0,0));
|
||||
initial->insert("p1", Pose2(0,0,0));
|
||||
|
||||
// Choose an ordering and optimize
|
||||
Ordering ordering;
|
||||
ordering += "p0","p1";
|
||||
NonlinearOptimizer<Pose2Graph, Pose2Config> optimizer(fg, ordering, initial);
|
||||
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15);
|
||||
|
||||
// Check with expected config
|
||||
Pose2Config expected;
|
||||
expected.insert("p0", Pose2(0,0,0));
|
||||
expected.insert("p1", Pose2(1,2,M_PI_2));
|
||||
CHECK(assert_equal(expected, *optimizer.config()));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
|
|
|
|||
Loading…
Reference in New Issue