unit tests for new Urban* classes

release/4.3a0
Frank Dellaert 2009-12-18 01:24:28 +00:00
parent d75a602546
commit 7dfc67860c
11 changed files with 659 additions and 120 deletions

View File

@ -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,14 +581,46 @@
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<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>
@ -606,6 +628,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>
@ -613,6 +636,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>

View File

@ -123,35 +123,28 @@ timeGaussianFactorGraph: LDFLAGS += smallExample.o -L.libs -lgtsam -L../CppUnitL
# Nonlinear inference
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h
headers += SQPOptimizer.h SQPOptimizer-inl.h
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
headers += NonlinearEquality.h
headers += Pose2Config.h Pose2Factor.h
headers += UrbanGraph.h
sources += NonlinearFactor.cpp
sources += Pose2Graph.cpp
sources += UrbanConfig.cpp UrbanMeasurement.cpp
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer testNonlinearEquality testSQP testNonlinearConstraint testSQPOptimizer testPose2Factor testPose2Graph testUrbanGraph
check_PROGRAMS += testNonlinearFactor testNonlinearFactorGraph testNonlinearOptimizer
testNonlinearFactor_SOURCES = $(example) testNonlinearFactor.cpp
testNonlinearFactor_LDADD = libgtsam.la
testNonlinearConstraint_SOURCES = $(example) testNonlinearConstraint.cpp
testNonlinearConstraint_LDADD = libgtsam.la
testNonlinearFactorGraph_SOURCES = $(example) testNonlinearFactorGraph.cpp
testNonlinearFactorGraph_LDADD = libgtsam.la
testNonlinearOptimizer_SOURCES = $(example) testNonlinearOptimizer.cpp
testNonlinearOptimizer_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la
# Nonlinear constraints
headers += SQPOptimizer.h SQPOptimizer-inl.h
headers += NonlinearConstraint.h NonlinearConstraint-inl.h
headers += NonlinearEquality.h
check_PROGRAMS += testNonlinearConstraint testNonlinearEquality testSQP testSQPOptimizer
testNonlinearConstraint_SOURCES = testNonlinearConstraint.cpp
testNonlinearConstraint_LDADD = libgtsam.la
testNonlinearEquality_SOURCES = testNonlinearEquality.cpp
testNonlinearEquality_LDADD = libgtsam.la
testSQP_SOURCES = $(example) testSQP.cpp
testSQP_LDADD = libgtsam.la
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
testPose2Factor_LDADD = libgtsam.la
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
testPose2Graph_LDADD = libgtsam.la
testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp
testUrbanGraph_LDADD = libgtsam.la
testSQPOptimizer_SOURCES = $(example) testSQPOptimizer.cpp
testSQPOptimizer_LDADD = libgtsam.la
# geometry
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Rot3.cpp Pose3.cpp Cal3_S2.cpp
@ -184,6 +177,28 @@ testSimulated3D_SOURCES = testSimulated3D.cpp
testSimulated3D_LDADD = libgtsam.la
check_PROGRAMS += testSimulated3D
# Pose constraints
headers += Pose2Config.h Pose2Factor.h
sources += Pose3Factor.cpp Pose2Graph.cpp
check_PROGRAMS += testPose2Factor testPose2Graph testPose3Factor
testPose2Factor_SOURCES = $(example) testPose2Factor.cpp
testPose2Factor_LDADD = libgtsam.la
testPose2Graph_SOURCES = $(example) testPose2Graph.cpp
testPose2Graph_LDADD = libgtsam.la
testPose3Factor_SOURCES = $(example) testPose3Factor.cpp
testPose3Factor_LDADD = libgtsam.la
# Urban
headers += UrbanGraph.h
sources += UrbanConfig.cpp UrbanMeasurement.cpp
check_PROGRAMS += testUrbanConfig testUrbanMeasurement testUrbanGraph
testUrbanConfig_SOURCES = $(example) testUrbanConfig.cpp
testUrbanConfig_LDADD = libgtsam.la
testUrbanMeasurement_SOURCES = $(example) testUrbanMeasurement.cpp
testUrbanMeasurement_LDADD = libgtsam.la
testUrbanGraph_SOURCES = $(example) testUrbanGraph.cpp
testUrbanGraph_LDADD = libgtsam.la
# Robot Control example system
sources += ControlConfig.cpp ControlPoint.cpp ControlGraph.cpp
check_PROGRAMS += testControlConfig testControlPoint testControlGraph

View File

@ -5,22 +5,19 @@
* @author Viorela Ila
*/
#include "UrbanConfig.h"
#include "Pose3Factor.h"
#include "Pose3.h"
#include "SimpleCamera.h"
using namespace std;
namespace gtsam{
namespace gtsam {
/* ************************************************************************* */
/* ************************************************************************* *
Pose3Factor::Pose3Factor() {
/// Arbitrary values
robotPoseNumber_ = 1;
robotPoseName_ = symbol('x',robotPoseNumber_);
keys_.push_back(robotPoseName_);
}
/* ************************************************************************* */
/* ************************************************************************* *
Pose3Factor::Pose3Factor(const Point2& z, double sigma, int cn, int ln)
: NonlinearFactor<Pose3Config>(z.vector(), sigma)
{
@ -32,13 +29,13 @@ Pose3Factor::Pose3Factor(const Point2& z, double sigma, int cn, int ln)
keys_.push_back(landmarkName_);
}
/* ************************************************************************* */
/* ************************************************************************* *
void Pose3Factor::print(const std::string& s) const {
printf("%s %s %s\n", s.c_str(), robotPoseName_.c_str(), landmarkName_.c_str());
gtsam::print(this->z_, s+".z");
}
/* ************************************************************************* */
/* ************************************************************************* *
bool Pose3Factor::equals(const Pose3Factor& p, double tol) const {
if (&p == NULL) return false;
if (robotPoseNumber_ != p.robotPoseNumber_ || landmarkNumber_ != p.landmarkNumber_) return false;
@ -46,6 +43,7 @@ bool Pose3Factor::equals(const Pose3Factor& p, double tol) const {
return true;
}
/* ************************************************************************* *
// TODO Implement transformPoint2_from
// the difference here that we have a 2d point in a 3D coordinate
Vector Pose3Factor::predict(const Pose3Config& c) const {
@ -54,13 +52,14 @@ Vector Pose3Factor::predict(const Pose3Config& c) const {
return transformPoint2_from(SimpleCamera(*K_,pose), landmark).vector();
}
/* ************************************************************************* */
/* ************************************************************************* *
Vector Pose3Factor::error_vector(const Pose3Config& c) const {
// Right-hand-side b = (z - h(x))/sigma
Point2 h = predict(c);
return (this->z_ - h);
}
/* ************************************************************************* *
GaussianFactor::shared_ptr Pose3Factor::linearize(const Pose3Config& c) const
{
// get arguments from config

View File

@ -13,59 +13,68 @@
#include "VectorConfig.h"
#include "Pose3.h"
#include "Matrix.h"
#include "ostream"
namespace gtsam {
template <class Config>
class Pose3Factor : public NonlinearFactor<Config> {
private:
typedef VectorConfig Config;
class Pose3Factor: public NonlinearFactor<Config> {
private:
std::string key1_, key2_; /** The keys of the two poses, order matters */
Pose3 measured_;
Matrix square_root_inverse_covariance_; /** sqrt(inv(measurement_covariance)) */
public:
public:
typedef boost::shared_ptr<Pose3Factor> shared_ptr; // shorthand for a smart pointer to a factor
Pose3Factor(const std::string& key1, const std::string& key2,
const Pose3& measured, const Matrix& measurement_covariance): key1_(key1),key2_(key2),measured_(measured) {
square_root_inverse_covariance_ = inverse_square_root(measurement_covariance);
const Pose3& measured, const Matrix& measurement_covariance) :
key1_(key1), key2_(key2), measured_(measured) {
square_root_inverse_covariance_ = inverse_square_root(
measurement_covariance);
}
/** implement functions needed for Testable */
void print(const std::string& name) const {
std::cout << name << std::endl;
std::cout << "Factor "<< std::endl;
std::cout << "key1 "<< key1_<<std::endl;
std::cout << "key2 "<< key2_<<std::endl;
std::cout << "Factor " << std::endl;
std::cout << "key1 " << key1_ << std::endl;
std::cout << "key2 " << key2_ << std::endl;
measured_.print("measured ");
gtsam::print(square_root_inverse_covariance_,"MeasurementCovariance");
gtsam::print(square_root_inverse_covariance_, "MeasurementCovariance");
}
bool equals(const NonlinearFactor<Config>& expected, double tol) const {
return false;
}
bool equals(const NonlinearFactor<Config>& expected, double tol) const {return false;}
/** implement functions needed to derive from Factor */
Vector error_vector(const Config& config) const {
//z-h
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
return (measured_ - between(p1,p2)).vector();
return (measured_ - between(p1, p2)).vector();
}
std::list<std::string> keys() const { std::list<std::string> l; return l; }
std::size_t size() const { return 2;}
std::list<std::string> keys() const {
std::list<std::string> l;
return l;
}
std::size_t size() const {
return 2;
}
/** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Config& config) const {
Pose3 p1 = config.get(key1_), p2 = config.get(key2_);
Vector b = (measured_ - between(p1,p2)).vector();
Matrix H1 = Dbetween1(p1,p2);
Matrix H2 = Dbetween2(p1,p2);
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
key1_, square_root_inverse_covariance_ * H1,
key2_, square_root_inverse_covariance_ * H2,
b, 1.0));
Vector b = (measured_ - between(p1, p2)).vector();
Matrix H1 = Dbetween1(p1, p2);
Matrix H2 = Dbetween2(p1, p2);
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_,
square_root_inverse_covariance_ * H1, key2_,
square_root_inverse_covariance_ * H2, b, 1.0));
return linearized;
}
};
};
} /// namespace gtsam

130
cpp/testPose3Factor Executable file
View File

@ -0,0 +1,130 @@
#! /bin/sh
# testPose3Factor - temporary wrapper script for .libs/testPose3Factor
# Generated by ltmain.sh (GNU libtool) 2.2.6
#
# The testPose3Factor program cannot be directly executed until all the libtool
# libraries that it depends on are installed.
#
# This wrapper script should never be moved out of the build directory.
# If it is, it will not operate correctly.
# Sed substitution that helps us do robust quoting. It backslashifies
# metacharacters that are still active within double-quoted strings.
Xsed='/usr/bin/sed -e 1s/^X//'
sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
# Be Bourne compatible
if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
emulate sh
NULLCMD=:
# Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
# is contrary to our usage. Disable this feature.
alias -g '${1+"$@"}'='"$@"'
setopt NO_GLOB_SUBST
else
case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
fi
BIN_SH=xpg4; export BIN_SH # for Tru64
DUALCASE=1; export DUALCASE # for MKS sh
# The HP-UX ksh and POSIX shell print the target directory to stdout
# if CDPATH is set.
(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
relink_command=""
# This environment variable determines our operation mode.
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
# install mode needs the following variables:
generated_by_libtool_version='2.2.6'
notinst_deplibs=' libgtsam.la'
else
# When we are sourced in execute mode, $file and $ECHO are already set.
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
ECHO="/bin/echo"
file="$0"
# Make sure echo works.
if test "X$1" = X--no-reexec; then
# Discard the --no-reexec flag, and continue.
shift
elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
# Yippee, $ECHO works!
:
else
# Restart under the correct shell, and then maybe $ECHO will work.
exec /bin/sh "$0" --no-reexec ${1+"$@"}
fi
fi
# Find the directory that this script lives in.
thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
test "x$thisdir" = "x$file" && thisdir=.
# Follow symbolic links until we get to the real thisdir.
file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'`
while test -n "$file"; do
destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
# If there was a directory component, then change thisdir.
if test "x$destdir" != "x$file"; then
case "$destdir" in
[\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
*) thisdir="$thisdir/$destdir" ;;
esac
fi
file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'`
done
# Usually 'no', except on cygwin/mingw when embedded into
# the cwrapper.
WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
# special case for '.'
if test "$thisdir" = "."; then
thisdir=`pwd`
fi
# remove .libs from thisdir
case "$thisdir" in
*[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
.libs ) thisdir=. ;;
esac
fi
# Try to get the absolute directory name.
absdir=`cd "$thisdir" && pwd`
test -n "$absdir" && thisdir="$absdir"
program='testPose3Factor'
progdir="$thisdir/.libs"
if test -f "$progdir/$program"; then
# Add our own library path to DYLD_LIBRARY_PATH
DYLD_LIBRARY_PATH="/Users/dellaert/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
# Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH
# The second colon is a workaround for a bug in BeOS R4 sed
DYLD_LIBRARY_PATH=`$ECHO "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
export DYLD_LIBRARY_PATH
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
# Run the actual program with our arguments.
exec "$progdir/$program" ${1+"$@"}
$ECHO "$0: cannot exec $program $*" 1>&2
exit 1
fi
else
# The program doesn't exist.
$ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
$ECHO "This script is just a wrapper for $program." 1>&2
/bin/echo "See the libtool documentation for more information." 1>&2
exit 1
fi
fi

23
cpp/testPose3Factor.cpp Normal file
View File

@ -0,0 +1,23 @@
/**
* @file testPose3Pose.cpp
* @brief Unit tests for Pose3Factor Class
* @authors Frank Dellaert
**/
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include "Pose3Factor.h"
using namespace std;
using namespace gtsam;
TEST( Pose3Factor, constructor )
{
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

130
cpp/testUrbanConfig Executable file
View File

@ -0,0 +1,130 @@
#! /bin/sh
# testUrbanConfig - temporary wrapper script for .libs/testUrbanConfig
# Generated by ltmain.sh (GNU libtool) 2.2.6
#
# The testUrbanConfig program cannot be directly executed until all the libtool
# libraries that it depends on are installed.
#
# This wrapper script should never be moved out of the build directory.
# If it is, it will not operate correctly.
# Sed substitution that helps us do robust quoting. It backslashifies
# metacharacters that are still active within double-quoted strings.
Xsed='/usr/bin/sed -e 1s/^X//'
sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
# Be Bourne compatible
if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
emulate sh
NULLCMD=:
# Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
# is contrary to our usage. Disable this feature.
alias -g '${1+"$@"}'='"$@"'
setopt NO_GLOB_SUBST
else
case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
fi
BIN_SH=xpg4; export BIN_SH # for Tru64
DUALCASE=1; export DUALCASE # for MKS sh
# The HP-UX ksh and POSIX shell print the target directory to stdout
# if CDPATH is set.
(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
relink_command=""
# This environment variable determines our operation mode.
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
# install mode needs the following variables:
generated_by_libtool_version='2.2.6'
notinst_deplibs=' libgtsam.la'
else
# When we are sourced in execute mode, $file and $ECHO are already set.
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
ECHO="/bin/echo"
file="$0"
# Make sure echo works.
if test "X$1" = X--no-reexec; then
# Discard the --no-reexec flag, and continue.
shift
elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
# Yippee, $ECHO works!
:
else
# Restart under the correct shell, and then maybe $ECHO will work.
exec /bin/sh "$0" --no-reexec ${1+"$@"}
fi
fi
# Find the directory that this script lives in.
thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
test "x$thisdir" = "x$file" && thisdir=.
# Follow symbolic links until we get to the real thisdir.
file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'`
while test -n "$file"; do
destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
# If there was a directory component, then change thisdir.
if test "x$destdir" != "x$file"; then
case "$destdir" in
[\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
*) thisdir="$thisdir/$destdir" ;;
esac
fi
file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'`
done
# Usually 'no', except on cygwin/mingw when embedded into
# the cwrapper.
WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
# special case for '.'
if test "$thisdir" = "."; then
thisdir=`pwd`
fi
# remove .libs from thisdir
case "$thisdir" in
*[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
.libs ) thisdir=. ;;
esac
fi
# Try to get the absolute directory name.
absdir=`cd "$thisdir" && pwd`
test -n "$absdir" && thisdir="$absdir"
program='testUrbanConfig'
progdir="$thisdir/.libs"
if test -f "$progdir/$program"; then
# Add our own library path to DYLD_LIBRARY_PATH
DYLD_LIBRARY_PATH="/Users/dellaert/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
# Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH
# The second colon is a workaround for a bug in BeOS R4 sed
DYLD_LIBRARY_PATH=`$ECHO "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
export DYLD_LIBRARY_PATH
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
# Run the actual program with our arguments.
exec "$progdir/$program" ${1+"$@"}
$ECHO "$0: cannot exec $program $*" 1>&2
exit 1
fi
else
# The program doesn't exist.
$ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
$ECHO "This script is just a wrapper for $program." 1>&2
/bin/echo "See the libtool documentation for more information." 1>&2
exit 1
fi
fi

23
cpp/testUrbanConfig.cpp Normal file
View File

@ -0,0 +1,23 @@
/*
* @file testUrbanConfig.cpp
* @brief Tests for the CitySLAM configuration class
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include <UrbanConfig.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( UrbanConfig, update_with_large_delta)
{
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

View File

@ -1,7 +1,7 @@
#! /bin/sh
# testUrbanGraph - temporary wrapper script for .libs/testUrbanGraph
# Generated by ltmain.sh - GNU libtool 1.5.22 (1.1220.2.365 2005/12/18 22:14:06)
# Generated by ltmain.sh (GNU libtool) 2.2.6
#
# The testUrbanGraph program cannot be directly executed until all the libtool
# libraries that it depends on are installed.
@ -12,7 +12,21 @@
# Sed substitution that helps us do robust quoting. It backslashifies
# metacharacters that are still active within double-quoted strings.
Xsed='/usr/bin/sed -e 1s/^X//'
sed_quote_subst='s/\([\\`\\"$\\\\]\)/\\\1/g'
sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
# Be Bourne compatible
if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
emulate sh
NULLCMD=:
# Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
# is contrary to our usage. Disable this feature.
alias -g '${1+"$@"}'='"$@"'
setopt NO_GLOB_SUBST
else
case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
fi
BIN_SH=xpg4; export BIN_SH # for Tru64
DUALCASE=1; export DUALCASE # for MKS sh
# The HP-UX ksh and POSIX shell print the target directory to stdout
# if CDPATH is set.
@ -22,34 +36,35 @@ relink_command=""
# This environment variable determines our operation mode.
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
# install mode needs the following variable:
# install mode needs the following variables:
generated_by_libtool_version='2.2.6'
notinst_deplibs=' libgtsam.la'
else
# When we are sourced in execute mode, $file and $echo are already set.
# When we are sourced in execute mode, $file and $ECHO are already set.
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
echo="/bin/echo"
ECHO="/bin/echo"
file="$0"
# Make sure echo works.
if test "X$1" = X--no-reexec; then
# Discard the --no-reexec flag, and continue.
shift
elif test "X`($echo '\t') 2>/dev/null`" = 'X\t'; then
# Yippee, $echo works!
elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
# Yippee, $ECHO works!
:
else
# Restart under the correct shell, and then maybe $echo will work.
# Restart under the correct shell, and then maybe $ECHO will work.
exec /bin/sh "$0" --no-reexec ${1+"$@"}
fi
fi
# Find the directory that this script lives in.
thisdir=`$echo "X$file" | $Xsed -e 's%/[^/]*$%%'`
thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
test "x$thisdir" = "x$file" && thisdir=.
# Follow symbolic links until we get to the real thisdir.
file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'`
while test -n "$file"; do
destdir=`$echo "X$file" | $Xsed -e 's%/[^/]*$%%'`
destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
# If there was a directory component, then change thisdir.
if test "x$destdir" != "x$file"; then
@ -59,10 +74,26 @@ else
esac
fi
file=`$echo "X$file" | $Xsed -e 's%^.*/%%'`
file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'`
done
# Usually 'no', except on cygwin/mingw when embedded into
# the cwrapper.
WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
# special case for '.'
if test "$thisdir" = "."; then
thisdir=`pwd`
fi
# remove .libs from thisdir
case "$thisdir" in
*[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
.libs ) thisdir=. ;;
esac
fi
# Try to get the absolute directory name.
absdir=`cd "$thisdir" && pwd`
test -n "$absdir" && thisdir="$absdir"
@ -73,11 +104,11 @@ else
if test -f "$progdir/$program"; then
# Add our own library path to DYLD_LIBRARY_PATH
DYLD_LIBRARY_PATH="/Users/vila/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
DYLD_LIBRARY_PATH="/Users/dellaert/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
# Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH
# The second colon is a workaround for a bug in BeOS R4 sed
DYLD_LIBRARY_PATH=`$echo "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
DYLD_LIBRARY_PATH=`$ECHO "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
export DYLD_LIBRARY_PATH
@ -86,13 +117,13 @@ else
exec "$progdir/$program" ${1+"$@"}
$echo "$0: cannot exec $program ${1+"$@"}"
$ECHO "$0: cannot exec $program $*" 1>&2
exit 1
fi
else
# The program doesn't exist.
$echo "$0: error: \`$progdir/$program' does not exist" 1>&2
$echo "This script is just a wrapper for $program." 1>&2
$ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
$ECHO "This script is just a wrapper for $program." 1>&2
/bin/echo "See the libtool documentation for more information." 1>&2
exit 1
fi

130
cpp/testUrbanMeasurement Executable file
View File

@ -0,0 +1,130 @@
#! /bin/sh
# testUrbanMeasurement - temporary wrapper script for .libs/testUrbanMeasurement
# Generated by ltmain.sh (GNU libtool) 2.2.6
#
# The testUrbanMeasurement program cannot be directly executed until all the libtool
# libraries that it depends on are installed.
#
# This wrapper script should never be moved out of the build directory.
# If it is, it will not operate correctly.
# Sed substitution that helps us do robust quoting. It backslashifies
# metacharacters that are still active within double-quoted strings.
Xsed='/usr/bin/sed -e 1s/^X//'
sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
# Be Bourne compatible
if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
emulate sh
NULLCMD=:
# Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
# is contrary to our usage. Disable this feature.
alias -g '${1+"$@"}'='"$@"'
setopt NO_GLOB_SUBST
else
case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
fi
BIN_SH=xpg4; export BIN_SH # for Tru64
DUALCASE=1; export DUALCASE # for MKS sh
# The HP-UX ksh and POSIX shell print the target directory to stdout
# if CDPATH is set.
(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
relink_command=""
# This environment variable determines our operation mode.
if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
# install mode needs the following variables:
generated_by_libtool_version='2.2.6'
notinst_deplibs=' libgtsam.la'
else
# When we are sourced in execute mode, $file and $ECHO are already set.
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
ECHO="/bin/echo"
file="$0"
# Make sure echo works.
if test "X$1" = X--no-reexec; then
# Discard the --no-reexec flag, and continue.
shift
elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
# Yippee, $ECHO works!
:
else
# Restart under the correct shell, and then maybe $ECHO will work.
exec /bin/sh "$0" --no-reexec ${1+"$@"}
fi
fi
# Find the directory that this script lives in.
thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
test "x$thisdir" = "x$file" && thisdir=.
# Follow symbolic links until we get to the real thisdir.
file=`ls -ld "$file" | /usr/bin/sed -n 's/.*-> //p'`
while test -n "$file"; do
destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
# If there was a directory component, then change thisdir.
if test "x$destdir" != "x$file"; then
case "$destdir" in
[\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
*) thisdir="$thisdir/$destdir" ;;
esac
fi
file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
file=`ls -ld "$thisdir/$file" | /usr/bin/sed -n 's/.*-> //p'`
done
# Usually 'no', except on cygwin/mingw when embedded into
# the cwrapper.
WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
# special case for '.'
if test "$thisdir" = "."; then
thisdir=`pwd`
fi
# remove .libs from thisdir
case "$thisdir" in
*[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
.libs ) thisdir=. ;;
esac
fi
# Try to get the absolute directory name.
absdir=`cd "$thisdir" && pwd`
test -n "$absdir" && thisdir="$absdir"
program='testUrbanMeasurement'
progdir="$thisdir/.libs"
if test -f "$progdir/$program"; then
# Add our own library path to DYLD_LIBRARY_PATH
DYLD_LIBRARY_PATH="/Users/dellaert/borg/gtsam/cpp/.libs:$DYLD_LIBRARY_PATH"
# Some systems cannot cope with colon-terminated DYLD_LIBRARY_PATH
# The second colon is a workaround for a bug in BeOS R4 sed
DYLD_LIBRARY_PATH=`$ECHO "X$DYLD_LIBRARY_PATH" | $Xsed -e 's/::*$//'`
export DYLD_LIBRARY_PATH
if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
# Run the actual program with our arguments.
exec "$progdir/$program" ${1+"$@"}
$ECHO "$0: cannot exec $program $*" 1>&2
exit 1
fi
else
# The program doesn't exist.
$ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
$ECHO "This script is just a wrapper for $program." 1>&2
/bin/echo "See the libtool documentation for more information." 1>&2
exit 1
fi
fi

View File

@ -0,0 +1,25 @@
/**********************************************************
Written by Frank Dellaert, Dec 2009
**********************************************************/
#include <CppUnitLite/TestHarness.h>
#include "UrbanConfig.h"
#include "UrbanMeasurement.h"
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
TEST( UrbanMeasurement, linearize )
{
}
/* ************************************************************************* */
int main() {
TestResult tr;
TestRegistry::runAllTests(tr);
return 0;
}
/* ************************************************************************* */