3D Pose SLAM + removed obsolete Urban files that somehow re-appeared in CitySLAM
Frank Dellaert
parent
c051be9edf
commit
049cea6964
|
|
@ -600,10 +600,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3Factor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testPose3Config.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3Factor.run</buildTarget>
|
||||
<buildTarget>testPose3Config.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -719,10 +719,10 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose3Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testPose3SLAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testPose3Graph.run</buildTarget>
|
||||
<buildTarget>testPose3SLAM.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -214,15 +214,15 @@ testPlanarSLAM_SOURCES = $(example) testPlanarSLAM.cpp
|
|||
testPlanarSLAM_LDADD = libgtsam.la
|
||||
|
||||
# 3D Pose constraints
|
||||
headers += Pose3Factor.h
|
||||
sources += Pose3Config.cpp Pose3Graph.cpp
|
||||
check_PROGRAMS += testPose3Factor testPose3Config testPose3Graph
|
||||
headers +=
|
||||
sources += pose3SLAM.cpp
|
||||
check_PROGRAMS += testPose3Factor testPose3Config testPose3SLAM
|
||||
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
|
||||
testPose3SLAM_SOURCES = $(example) testPose3SLAM.cpp
|
||||
testPose3SLAM_LDADD = libgtsam.la
|
||||
|
||||
# Cameras
|
||||
sources += CalibratedCamera.cpp SimpleCamera.cpp
|
||||
|
|
@ -280,3 +280,12 @@ CXXLINK = $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=link \
|
|||
# rule to run an executable
|
||||
%.run: % libgtsam.la
|
||||
./$^
|
||||
|
||||
# to compile colamd and cppunitlite first when make all and make install.
|
||||
# The part after external_libs is copied from automatically generated Makefile when without the following line
|
||||
libgtsam.la: external_libs $(libgtsam_la_OBJECTS) $(libgtsam_la_DEPENDENCIES)
|
||||
$(libgtsam_la_LINK) -rpath $(libdir) $(libgtsam_la_OBJECTS) $(libgtsam_la_LIBADD) $(LIBS)
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
/**
|
||||
* @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<Symbol<Pose3,'x'>,Pose3>;
|
||||
template size_t dim(const Pose3Config& c);
|
||||
template Pose3Config expmap(const Pose3Config& c, const Vector& delta);
|
||||
template Pose3Config expmap(const Pose3Config& 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) {
|
||||
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(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
} // namespace
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
/**
|
||||
* @file Pose3Config.cpp
|
||||
* @brief Configuration of 3D poses
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Pose3.h"
|
||||
#include "LieConfig.h"
|
||||
#include "Key.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Pose3Config is now simply a typedef
|
||||
*/
|
||||
typedef LieConfig<Symbol<Pose3,'x'>,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);
|
||||
|
||||
} // namespace
|
||||
|
|
@ -1,19 +0,0 @@
|
|||
/**
|
||||
* @file Pose3Factor.H
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Pose3Config.h"
|
||||
#include "BetweenFactor.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* A Factor for 3D pose measurements
|
||||
* This is just a typedef now
|
||||
*/
|
||||
typedef BetweenFactor<Pose3Config,Pose3Config::Key,Pose3> Pose3Factor;
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
@ -1,36 +0,0 @@
|
|||
/**
|
||||
* @file Pose3Graph.cpp
|
||||
* @brief A factor graph for the 2D PoseSLAM problem
|
||||
* @authors Frank Dellaert, Viorela Ila
|
||||
*/
|
||||
|
||||
#include "Pose3Graph.h"
|
||||
#include "FactorGraph-inl.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "graph-inl.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 NonlinearEquality<Pose3Config, Pose3Config::Key, Pose3> ;
|
||||
template class NonlinearOptimizer<Pose3Graph, Pose3Config> ;
|
||||
|
||||
bool Pose3Graph::equals(const Pose3Graph& p, double tol) const {
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add an equality constraint on a pose
|
||||
* @param key of pose
|
||||
* @param pose which pose to constrain it to
|
||||
*/
|
||||
void Pose3Graph::addConstraint(const Pose3Config::Key& key, const Pose3& pose) {
|
||||
push_back(sharedFactor(new NonlinearEquality<Pose3Config, Pose3Config::Key,
|
||||
Pose3> (key, pose)));
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -1,54 +0,0 @@
|
|||
/**
|
||||
* @file Pose3Graph.h
|
||||
* @brief A factor graph for the 3D PoseSLAM problem
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Pose3Factor.h"
|
||||
#include "NonlinearFactorGraph.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 Pose3Config::Key& key1, const Pose3Config::Key& key2,
|
||||
const Pose3& measured, const Matrix& covariance) {
|
||||
push_back(sharedFactor(new Pose3Factor(key1, key2, measured, covariance)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Add an equality constraint on a pose
|
||||
* @param key of pose
|
||||
* @param pose which pose to constrain it to
|
||||
*/
|
||||
void addConstraint(const Pose3Config::Key& key, const Pose3& pose =Pose3());
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
@ -29,20 +29,21 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Graph::addConstraint(const Key& i, const Pose2& p) {
|
||||
sharedFactor factor(new Constraint(i, p));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::addPrior(const Key& i, const Pose2& p, const Matrix& cov) {
|
||||
sharedFactor factor(new Prior(i, p, cov));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) {
|
||||
sharedFactor factor(new Odometry(i, j, z, cov));
|
||||
void Graph::addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov) {
|
||||
sharedFactor factor(new Constraint(i, j, z, cov));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::addHardConstraint(const Key& i, const Pose2& p) {
|
||||
sharedFactor factor(new HardConstraint(i, p));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // pose2SLAM
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/**
|
||||
* @file pose2SLAM.h
|
||||
* @brief: bearing/range measurements in 2D plane
|
||||
* @brief: 2D Pose SLAM
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
|
|
@ -35,14 +35,14 @@ namespace gtsam {
|
|||
|
||||
// Factors
|
||||
typedef PriorFactor<Config, Key, Pose2> Prior;
|
||||
typedef BetweenFactor<Config, Key, Pose2> Odometry;
|
||||
typedef NonlinearEquality<Config, Key, Pose2> Constraint;
|
||||
typedef BetweenFactor<Config, Key, Pose2> Constraint;
|
||||
typedef NonlinearEquality<Config, Key, Pose2> HardConstraint;
|
||||
|
||||
// Graph
|
||||
struct Graph: public NonlinearFactorGraph<Config> {
|
||||
void addConstraint(const Key& i, const Pose2& p);
|
||||
void addPrior(const Key& i, const Pose2& p, const Matrix& cov);
|
||||
void add(const Key& i, const Key& j, const Pose2& z, const Matrix& cov);
|
||||
void addConstraint(const Key& i, const Key& j, const Pose2& z, const Matrix& cov);
|
||||
void addHardConstraint(const Key& i, const Pose2& p);
|
||||
};
|
||||
|
||||
// Optimizer
|
||||
|
|
@ -53,10 +53,9 @@ namespace gtsam {
|
|||
/**
|
||||
* Backwards compatibility
|
||||
*/
|
||||
typedef pose2SLAM::Prior Pose2Prior;
|
||||
typedef pose2SLAM::Odometry Pose2Factor;
|
||||
typedef pose2SLAM::Constraint Pose2Constraint;
|
||||
typedef pose2SLAM::Config Pose2Config;
|
||||
typedef pose2SLAM::Prior Pose2Prior;
|
||||
typedef pose2SLAM::Constraint Pose2Factor;
|
||||
typedef pose2SLAM::Graph Pose2Graph;
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
/**
|
||||
* @file pose3SLAM.cpp
|
||||
* @brief: bearing/range measurements in 2D plane
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#include "pose3SLAM.h"
|
||||
#include "LieConfig-inl.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
|
||||
// Use pose3SLAM namespace for specific SLAM instance
|
||||
namespace gtsam {
|
||||
|
||||
using namespace pose3SLAM;
|
||||
INSTANTIATE_LIE_CONFIG(Key, Pose3)
|
||||
INSTANTIATE_NONLINEAR_FACTOR_GRAPH(Config)
|
||||
INSTANTIATE_NONLINEAR_OPTIMIZER(Graph, Config)
|
||||
|
||||
namespace pose3SLAM {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Config circle(size_t n, double R) {
|
||||
Config 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(i, Pose3(R0 * Rot3::yaw(-theta), Point3(cos(theta),sin(theta),0)));
|
||||
return x;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Graph::addPrior(const Key& i, const Pose3& p, const Matrix& cov) {
|
||||
sharedFactor factor(new Prior(i, p, cov));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, const Matrix& cov) {
|
||||
sharedFactor factor(new Constraint(i, j, z, cov));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
void Graph::addHardConstraint(const Key& i, const Pose3& p) {
|
||||
sharedFactor factor(new HardConstraint(i, p));
|
||||
push_back(factor);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // pose3SLAM
|
||||
|
||||
} // gtsam
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
/**
|
||||
* @file pose3SLAM.h
|
||||
* @brief: 3D Pose SLAM
|
||||
* @authors Frank Dellaert
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Key.h"
|
||||
#include "Pose3.h"
|
||||
#include "LieConfig.h"
|
||||
#include "PriorFactor.h"
|
||||
#include "BetweenFactor.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "NonlinearFactorGraph.h"
|
||||
#include "NonlinearOptimizer.h"
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Use pose3SLAM namespace for specific SLAM instance
|
||||
namespace pose3SLAM {
|
||||
|
||||
// Keys and Config
|
||||
typedef Symbol<Pose3, 'x'> Key;
|
||||
typedef LieConfig<Key, Pose3> Config;
|
||||
|
||||
/**
|
||||
* Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0)
|
||||
* @param n number of poses
|
||||
* @param R radius of circle
|
||||
* @param c character to use for keys
|
||||
* @return circle of n 3D poses
|
||||
*/
|
||||
Config circle(size_t n, double R);
|
||||
|
||||
// Factors
|
||||
typedef PriorFactor<Config, Key, Pose3> Prior;
|
||||
typedef BetweenFactor<Config, Key, Pose3> Constraint;
|
||||
typedef NonlinearEquality<Config, Key, Pose3> HardConstraint;
|
||||
|
||||
// Graph
|
||||
struct Graph: public NonlinearFactorGraph<Config> {
|
||||
void addPrior(const Key& i, const Pose3& p, const Matrix& cov);
|
||||
void addConstraint(const Key& i, const Key& j, const Pose3& z, const Matrix& cov);
|
||||
void addHardConstraint(const Key& i, const Pose3& p);
|
||||
};
|
||||
|
||||
// Optimizer
|
||||
typedef NonlinearOptimizer<Graph, Config> Optimizer;
|
||||
|
||||
} // pose3SLAM
|
||||
|
||||
/**
|
||||
* Backwards compatibility
|
||||
*/
|
||||
typedef pose3SLAM::Config Pose3Config;
|
||||
typedef pose3SLAM::Prior Pose3Prior;
|
||||
typedef pose3SLAM::Constraint Pose3Factor;
|
||||
typedef pose3SLAM::Graph Pose3Graph;
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
|
|
@ -43,8 +43,8 @@ TEST( Graph, composePoses )
|
|||
{
|
||||
Pose2Graph graph;
|
||||
Matrix cov = eye(3);
|
||||
graph.add(1,2, Pose2(2.0, 0.0, 0.0), cov);
|
||||
graph.add(2,3, Pose2(3.0, 0.0, 0.0), cov);
|
||||
graph.addConstraint(1,2, Pose2(2.0, 0.0, 0.0), cov);
|
||||
graph.addConstraint(2,3, Pose2(3.0, 0.0, 0.0), cov);
|
||||
|
||||
PredecessorMap<Pose2Config::Key> tree;
|
||||
tree.insert(1,2);
|
||||
|
|
|
|||
|
|
@ -107,8 +107,8 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
|
|||
Pose2Graph graph;
|
||||
Matrix cov = eye(3);
|
||||
Matrix cov2 = eye(3) * 1e-10;
|
||||
graph.add(1,2, Pose2(1.,0.,0.), cov);
|
||||
graph.addPrior(1, Pose2(0.,0.,0.), cov2);
|
||||
graph.addConstraint(1,2, Pose2(1.,0.,0.), cov);
|
||||
|
||||
VectorConfig zeros;
|
||||
zeros.insert("x1",zero(3));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,130 @@
|
|||
#! /bin/sh
|
||||
|
||||
# testNoiseModel - temporary wrapper script for .libs/testNoiseModel
|
||||
# Generated by ltmain.sh (GNU libtool) 2.2.6
|
||||
#
|
||||
# The testNoiseModel 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='testNoiseModel'
|
||||
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
|
||||
|
|
@ -0,0 +1,130 @@
|
|||
#! /bin/sh
|
||||
|
||||
# testPose2Prior - temporary wrapper script for .libs/testPose2Prior
|
||||
# Generated by ltmain.sh (GNU libtool) 2.2.6
|
||||
#
|
||||
# The testPose2Prior 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='testPose2Prior'
|
||||
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
|
||||
|
|
@ -34,7 +34,7 @@ TEST( Pose2Graph, constructor )
|
|||
Pose2 measured(2,2,M_PI_2);
|
||||
Pose2Factor constraint(1,2,measured, covariance);
|
||||
Pose2Graph graph;
|
||||
graph.add(1,2,measured, covariance);
|
||||
graph.addConstraint(1,2,measured, covariance);
|
||||
// get the size of the graph
|
||||
size_t actual = graph.size();
|
||||
// verify
|
||||
|
|
@ -50,7 +50,7 @@ TEST( Pose2Graph, linerization )
|
|||
Pose2 measured(2,2,M_PI_2);
|
||||
Pose2Factor constraint(1,2,measured, covariance);
|
||||
Pose2Graph graph;
|
||||
graph.add(1,2,measured, covariance);
|
||||
graph.addConstraint(1,2,measured, covariance);
|
||||
|
||||
// Choose a linearization point
|
||||
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
|
||||
|
|
@ -86,8 +86,8 @@ TEST(Pose2Graph, optimize) {
|
|||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
shared_ptr<Pose2Graph> fg(new Pose2Graph);
|
||||
fg->addConstraint(0, Pose2(0,0,0));
|
||||
fg->add(0, 1, Pose2(1,2,M_PI_2), covariance);
|
||||
fg->addHardConstraint(0, Pose2(0,0,0));
|
||||
fg->addConstraint(0, 1, Pose2(1,2,M_PI_2), covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
|
||||
|
|
@ -120,14 +120,14 @@ TEST(Pose2Graph, optimizeCircle) {
|
|||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
shared_ptr<Pose2Graph> fg(new Pose2Graph);
|
||||
fg->addConstraint(0, p0);
|
||||
fg->addHardConstraint(0, p0);
|
||||
Pose2 delta = between(p0,p1);
|
||||
fg->add(0, 1, delta, covariance);
|
||||
fg->add(1,2, delta, covariance);
|
||||
fg->add(2,3, delta, covariance);
|
||||
fg->add(3,4, delta, covariance);
|
||||
fg->add(4,5, delta, covariance);
|
||||
fg->add(5, 0, delta, covariance);
|
||||
fg->addConstraint(0, 1, delta, covariance);
|
||||
fg->addConstraint(1,2, delta, covariance);
|
||||
fg->addConstraint(2,3, delta, covariance);
|
||||
fg->addConstraint(3,4, delta, covariance);
|
||||
fg->addConstraint(4,5, delta, covariance);
|
||||
fg->addConstraint(5, 0, delta, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose2Config> initial(new Pose2Config());
|
||||
|
|
@ -159,37 +159,34 @@ TEST(Pose2Graph, optimizeCircle) {
|
|||
/* ************************************************************************* */
|
||||
// test optimization with 6 poses arranged in a hexagon and a loop closure
|
||||
TEST(Pose2Graph, findMinimumSpanningTree) {
|
||||
typedef Pose2Config::Key Key;
|
||||
|
||||
Pose2Graph G, T, C;
|
||||
Matrix cov = eye(3);
|
||||
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov)));
|
||||
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov)));
|
||||
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov)));
|
||||
G.addConstraint(1, 2, Pose2(0.,0.,0.), cov);
|
||||
G.addConstraint(1, 3, Pose2(0.,0.,0.), cov);
|
||||
G.addConstraint(2, 3, Pose2(0.,0.,0.), cov);
|
||||
|
||||
PredecessorMap<Key> tree = G.findMinimumSpanningTree<Key, Pose2Factor>();
|
||||
CHECK(tree[Key(1)] == Key(1));
|
||||
CHECK(tree[Key(2)] == Key(1));
|
||||
CHECK(tree[Key(3)] == Key(1));
|
||||
PredecessorMap<pose2SLAM::Key> tree =
|
||||
G.findMinimumSpanningTree<pose2SLAM::Key, Pose2Factor>();
|
||||
CHECK(tree[1] == 1);
|
||||
CHECK(tree[2] == 1);
|
||||
CHECK(tree[3] == 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// test optimization with 6 poses arranged in a hexagon and a loop closure
|
||||
TEST(Pose2Graph, split) {
|
||||
typedef Pose2Config::Key Key;
|
||||
|
||||
Pose2Graph G, T, C;
|
||||
Matrix cov = eye(3);
|
||||
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(2), Pose2(0.,0.,0.), cov)));
|
||||
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(1), Key(3), Pose2(0.,0.,0.), cov)));
|
||||
G.push_back(Pose2Graph::sharedFactor(new Pose2Factor(Key(2), Key(3), Pose2(0.,0.,0.), cov)));
|
||||
G.addConstraint(1, 2, Pose2(0.,0.,0.), cov);
|
||||
G.addConstraint(1, 3, Pose2(0.,0.,0.), cov);
|
||||
G.addConstraint(2, 3, Pose2(0.,0.,0.), cov);
|
||||
|
||||
PredecessorMap<Key> tree;
|
||||
tree.insert(Key(1),Key(2));
|
||||
tree.insert(Key(2),Key(2));
|
||||
tree.insert(Key(3),Key(2));
|
||||
PredecessorMap<pose2SLAM::Key> tree;
|
||||
tree.insert(1,2);
|
||||
tree.insert(2,2);
|
||||
tree.insert(3,2);
|
||||
|
||||
G.split<Key, Pose2Factor>(tree, T, C);
|
||||
G.split<pose2SLAM::Key, Pose2Factor>(tree, T, C);
|
||||
LONGS_EQUAL(2, T.size());
|
||||
LONGS_EQUAL(1, C.size());
|
||||
}
|
||||
|
|
|
|||
|
|
@ -6,7 +6,7 @@
|
|||
#include <iostream>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "Pose3Config.h"
|
||||
#include "pose3SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -28,7 +28,7 @@ TEST( Pose3Config, pose3Circle )
|
|||
expected.insert(2, Pose3(R3, Point3(-1, 0, 0)));
|
||||
expected.insert(3, Pose3(R4, Point3( 0,-1, 0)));
|
||||
|
||||
Pose3Config actual = pose3Circle(4,1.0);
|
||||
Pose3Config actual = pose3SLAM::circle(4,1.0);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
|
|
@ -49,7 +49,7 @@ TEST( Pose3Config, expmap )
|
|||
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),delta);
|
||||
Pose3Config actual = expmap(pose3SLAM::circle(4,1.0),delta);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -9,8 +9,7 @@
|
|||
using namespace boost::assign;
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include "Pose3Factor.h"
|
||||
#include "LieConfig-inl.h"
|
||||
#include "pose3SLAM.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,130 @@
|
|||
#! /bin/sh
|
||||
|
||||
# testPose3SLAM - temporary wrapper script for .libs/testPose3SLAM
|
||||
# Generated by ltmain.sh (GNU libtool) 2.2.6
|
||||
#
|
||||
# The testPose3SLAM 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='testPose3SLAM'
|
||||
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
|
||||
|
|
@ -13,9 +13,7 @@ using namespace boost::assign;
|
|||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "Pose3Graph.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
#include "NonlinearEquality.h"
|
||||
#include "pose3SLAM.h"
|
||||
#include "Ordering.h"
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -29,19 +27,19 @@ static Matrix covariance = eye(6);
|
|||
TEST(Pose3Graph, optimizeCircle) {
|
||||
|
||||
// Create a hexagon of poses
|
||||
Pose3Config hexagon = pose3Circle(6,1.0);
|
||||
Pose3Config hexagon = pose3SLAM::circle(6,1.0);
|
||||
Pose3 p0 = hexagon[0], p1 = hexagon[1];
|
||||
|
||||
// create a Pose graph with one equality constraint and one measurement
|
||||
shared_ptr<Pose3Graph> fg(new Pose3Graph);
|
||||
fg->addConstraint(0, p0);
|
||||
fg->addHardConstraint(0, p0);
|
||||
Pose3 delta = between(p0,p1);
|
||||
fg->add(0,1, delta, covariance);
|
||||
fg->add(1,2, delta, covariance);
|
||||
fg->add(2,3, delta, covariance);
|
||||
fg->add(3,4, delta, covariance);
|
||||
fg->add(4,5, delta, covariance);
|
||||
fg->add(5,0, delta, covariance);
|
||||
fg->addConstraint(0,1, delta, covariance);
|
||||
fg->addConstraint(1,2, delta, covariance);
|
||||
fg->addConstraint(2,3, delta, covariance);
|
||||
fg->addConstraint(3,4, delta, covariance);
|
||||
fg->addConstraint(4,5, delta, covariance);
|
||||
fg->addConstraint(5,0, delta, covariance);
|
||||
|
||||
// Create initial config
|
||||
boost::shared_ptr<Pose3Config> initial(new Pose3Config());
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/
|
||||
|
||||
# for maximum performance on Intel Core2 platform:
|
||||
#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -ftree-vectorize O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static
|
||||
#./configure --prefix=$HOME --with-toolbox=$HOME/toolbox/ --with-boost=/opt/local/include/ CXXFLAGS=" -g -O3 -march=nocona -mtune=generic -DNDEBUG" --disable-static
|
||||
Loading…
Reference in New Issue