wrapping PlanarSLAMExample_easy
Frank Dellaert
parent
1bdc8b5951
commit
f62a6bda1e
12
.cproject
12
.cproject
|
|
@ -769,6 +769,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testSimulated2D.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1720,10 +1728,10 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="PlanarSLAMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="PlanarSLAMExample_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>PlanarSLAMExample.run</buildTarget>
|
||||
<buildTarget>PlanarSLAMExample_easy.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -86,8 +86,10 @@ int main(int argc, char** argv) {
|
|||
initialEstimate.print("initial estimate");
|
||||
|
||||
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||
Values result = optimize<Graph, Values>(graph, initialEstimate);
|
||||
result.print("final result");
|
||||
// Values result = optimize<Graph, Values>(graph, initialEstimate);
|
||||
// result.print("final result");
|
||||
boost::shared_ptr<Values> result = graph.optimize(initialEstimate);
|
||||
result->print("final result");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,75 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Simple robotics example using the pre-built planar SLAM domain
|
||||
% @author Alex Cunningham
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
%% Assumptions
|
||||
% - All values are axis aligned
|
||||
% - Robot poses are facing along the X axis (horizontal, to the right in images)
|
||||
% - We have bearing and range information for measurements
|
||||
% - We have full odometry for measurements
|
||||
% - The robot and landmarks are on a grid, moving 2 meters each step
|
||||
% - Landmarks are 2 meters away from the robot trajectory
|
||||
|
||||
%% Create keys for variables
|
||||
x1 = 1; x2 = 2; x3 = 3;
|
||||
l1 = 1; l2 = 2;
|
||||
|
||||
%% Create graph container and add factors to it
|
||||
graph = PlanarSLAMGraph;
|
||||
|
||||
%% Add prior
|
||||
% gaussian for prior
|
||||
prior_model = SharedDiagonal([0.3; 0.3; 0.1]);
|
||||
prior_measurement = Pose2(0.0, 0.0, 0.0); % prior at origin
|
||||
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
|
||||
|
||||
%% Add odometry
|
||||
% general noisemodel for odometry
|
||||
odom_model = SharedDiagonal([0.2; 0.2; 0.1]);
|
||||
odom_measurement = Pose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
|
||||
graph.addOdometry(x1, x2, odom_measurement, odom_model);
|
||||
graph.addOdometry(x2, x3, odom_measurement, odom_model);
|
||||
|
||||
%% Add measurements
|
||||
% general noisemodel for measurements
|
||||
meas_model = SharedDiagonal([0.1; 0.2]);
|
||||
|
||||
% create the measurement values - indices are (pose id, landmark id)
|
||||
degrees = pi/180;
|
||||
bearing11 = Rot2(45*degrees);
|
||||
bearing21 = Rot2(90*degrees);
|
||||
bearing32 = Rot2(90*degrees);
|
||||
range11 = sqrt(4+4);
|
||||
range21 = 2.0;
|
||||
range32 = 2.0;
|
||||
|
||||
% create bearing/range factors and add them
|
||||
graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
|
||||
graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
|
||||
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
|
||||
|
||||
% print
|
||||
graph.print('full graph');
|
||||
|
||||
%% Initialize to noisy points
|
||||
initialEstimate = PlanarSLAMValues;
|
||||
initialEstimate.insertPose(x1, Pose2(0.5, 0.0, 0.2));
|
||||
initialEstimate.insertPose(x2, Pose2(2.3, 0.1,-0.2));
|
||||
initialEstimate.insertPose(x3, Pose2(4.1, 0.1, 0.1));
|
||||
initialEstimate.insertPoint(l1, Landmark2(1.8, 2.1));
|
||||
initialEstimate.insertPoint(l2, Landmark2(4.1, 1.8));
|
||||
|
||||
initialEstimate.print('initial estimate');
|
||||
|
||||
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
|
||||
result = graph.optimize(initialEstimate);
|
||||
result.print('final result');
|
||||
42
gtsam.h
42
gtsam.h
|
|
@ -19,6 +19,11 @@ class Point3 {
|
|||
|
||||
class Rot2 {
|
||||
Rot2();
|
||||
Rot2(double theta);
|
||||
void print(string s) const;
|
||||
bool equals(const Rot2& pose, double tol) const;
|
||||
double c() const;
|
||||
double s() const;
|
||||
};
|
||||
|
||||
class Pose2 {
|
||||
|
|
@ -106,3 +111,40 @@ class GaussianFactorGraph {
|
|||
void combine(const GaussianFactorGraph& lfg);
|
||||
};
|
||||
|
||||
class Landmark2 {
|
||||
Landmark2();
|
||||
Landmark2(double x, double y);
|
||||
void print(string s) const;
|
||||
double x();
|
||||
double y();
|
||||
};
|
||||
|
||||
class Ordering{
|
||||
void print(string s) const;
|
||||
bool equals(const Ordering& ord, double tol) const;
|
||||
};
|
||||
|
||||
class PlanarSLAMValues {
|
||||
PlanarSLAMValues();
|
||||
void print(string s) const;
|
||||
void insertPose(int key, const Pose2& pose);
|
||||
void insertPoint(int key, const Point2& point);
|
||||
};
|
||||
|
||||
class PlanarSLAMGraph {
|
||||
PlanarSLAMGraph();
|
||||
double error(const PlanarSLAMValues& c) const;
|
||||
Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
|
||||
void print(string s) const;
|
||||
void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model);
|
||||
void addPoseConstraint(size_t i, const Pose2& p);
|
||||
void addOdometry(size_t i, size_t j, const Pose2& z,
|
||||
const SharedNoiseModel& model);
|
||||
void addBearing(size_t i, size_t j, const Rot2& z,
|
||||
const SharedNoiseModel& model);
|
||||
void addRange(size_t i, size_t j, double z, const SharedNoiseModel& model);
|
||||
void addBearingRange(size_t i, size_t j, const Rot2& z1, double z2,
|
||||
const SharedNoiseModel& model);
|
||||
PlanarSLAMValues* optimize(const PlanarSLAMValues& initialEstimate);
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -14,10 +14,12 @@
|
|||
* @brief 2D Pose
|
||||
*/
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/base/Lie-inl.h>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -55,7 +57,7 @@ namespace gtsam {
|
|||
assert(xi.size() == 3);
|
||||
Point2 v(xi(0),xi(1));
|
||||
double w = xi(2);
|
||||
if (fabs(w) < 1e-10)
|
||||
if (std::abs(w) < 1e-10)
|
||||
return Pose2(xi[0], xi[1], xi[2]);
|
||||
else {
|
||||
Rot2 R(Rot2::fromAngle(w));
|
||||
|
|
@ -70,7 +72,7 @@ namespace gtsam {
|
|||
const Rot2& R = p.r();
|
||||
const Point2& t = p.t();
|
||||
double w = R.theta();
|
||||
if (fabs(w) < 1e-10)
|
||||
if (std::abs(w) < 1e-10)
|
||||
return Vector_(3, t.x(), t.y(), w);
|
||||
else {
|
||||
double c_1 = R.c()-1.0, s = R.s();
|
||||
|
|
@ -173,13 +175,13 @@ namespace gtsam {
|
|||
const Rot2& R1 = r_, R2 = p2.r();
|
||||
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
|
||||
|
||||
// Assert that R1 and R2 are normalized
|
||||
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
|
||||
|
||||
// Calculate delta rotation = between(R1,R2)
|
||||
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
|
||||
Rot2 R(Rot2::atan2(s,c)); // normalizes
|
||||
|
||||
// Assert that R1 and R2 are normalized
|
||||
assert(fabs(c1*c1 + s1*s1 - 1.0) < 1e-5 && fabs(c2*c2 + s2*s2 - 1.0) < 1e-5);
|
||||
|
||||
// Calculate delta translation = unrotate(R1, dt);
|
||||
Point2 dt = p2.t() - t_;
|
||||
double x = dt.x(), y = dt.y();
|
||||
|
|
@ -228,16 +230,16 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (!H1 && !H2) return transform_to(point).norm();
|
||||
Point2 d = transform_to(point, H1, H2);
|
||||
double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
|
||||
double x = d.x(), y = d.y(), d2 = x * x + y * y, r = sqrt(d2);
|
||||
Matrix D_result_d;
|
||||
if(fabs(n) > 1e-10)
|
||||
D_result_d = Matrix_(1, 2, x / n, y / n);
|
||||
if(std::abs(r) > 1e-10)
|
||||
D_result_d = Matrix_(1, 2, x / r, y / r);
|
||||
else {
|
||||
D_result_d = Matrix_(1,2, 1.0, 1.0);
|
||||
}
|
||||
if (H1) *H1 = D_result_d * (*H1);
|
||||
if (H2) *H2 = D_result_d * (*H2);
|
||||
return n;
|
||||
return r;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -40,7 +40,9 @@ namespace gtsam {
|
|||
double c_, s_;
|
||||
|
||||
/** private constructor from cos/sin */
|
||||
inline Rot2(double c, double s) : c_(c), s_(s) {}
|
||||
inline Rot2(double c, double s) :
|
||||
c_(c), s_(s) {
|
||||
}
|
||||
|
||||
/** normalize to make sure cos and sin form unit vector */
|
||||
Rot2& normalize();
|
||||
|
|
@ -48,22 +50,29 @@ namespace gtsam {
|
|||
public:
|
||||
|
||||
/** default constructor, zero rotation */
|
||||
Rot2() : c_(1.0), s_(0.0) {}
|
||||
Rot2() :
|
||||
c_(1.0), s_(0.0) {
|
||||
}
|
||||
|
||||
/// Constructor from angle in radians == exponential map at identity
|
||||
Rot2(double theta) :
|
||||
c_(cos(theta)), s_(sin(theta)) {
|
||||
}
|
||||
|
||||
/** "named constructors" */
|
||||
|
||||
/** Named constructor from angle == exponential map at identity - theta is in radians*/
|
||||
/// Named constructor from angle in radians
|
||||
static Rot2 fromAngle(double theta) {
|
||||
return Rot2(cos(theta), sin(theta));
|
||||
return Rot2(theta);
|
||||
}
|
||||
|
||||
/** Named constructor from angle in degrees */
|
||||
/// Named constructor from angle in degrees
|
||||
static Rot2 fromDegrees(double theta) {
|
||||
const double degree = M_PI / 180;
|
||||
return fromAngle(theta * degree);
|
||||
}
|
||||
|
||||
/** Named constructor from cos(theta),sin(theta) pair, will *not* normalize! */
|
||||
/// Named constructor from cos(theta),sin(theta) pair, will *not* normalize!
|
||||
static Rot2 fromCosSin(double c, double s);
|
||||
|
||||
/**
|
||||
|
|
@ -73,13 +82,16 @@ namespace gtsam {
|
|||
* @param H optional reference for Jacobian
|
||||
* @return 2D rotation \in SO(2)
|
||||
*/
|
||||
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H=boost::none);
|
||||
static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
|
||||
boost::none);
|
||||
|
||||
/** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
|
||||
static Rot2 atan2(double y, double x);
|
||||
|
||||
/** return angle (RADIANS) */
|
||||
double theta() const { return ::atan2(s_,c_); }
|
||||
double theta() const {
|
||||
return ::atan2(s_, c_);
|
||||
}
|
||||
|
||||
/** return angle (DEGREES) */
|
||||
double degrees() const {
|
||||
|
|
@ -88,10 +100,14 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** return cos */
|
||||
inline double c() const { return c_; }
|
||||
inline double c() const {
|
||||
return c_;
|
||||
}
|
||||
|
||||
/** return sin */
|
||||
inline double s() const { return s_; }
|
||||
inline double s() const {
|
||||
return s_;
|
||||
}
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "theta") const;
|
||||
|
|
@ -100,17 +116,20 @@ namespace gtsam {
|
|||
bool equals(const Rot2& R, double tol = 1e-9) const;
|
||||
|
||||
/** dimension of the variable - used to autodetect sizes */
|
||||
inline static size_t Dim() { return dimension; }
|
||||
inline static size_t Dim() {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/** Lie requirements */
|
||||
|
||||
/** Dimensionality of the tangent space */
|
||||
inline size_t dim() const { return dimension; }
|
||||
inline size_t dim() const {
|
||||
return dimension;
|
||||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
inline Rot2 compose(const Rot2& R1,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
inline Rot2 compose(const Rot2& R1, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
if (H1) *H1 = eye(1);
|
||||
if (H2) *H2 = eye(1);
|
||||
return *this * R1;
|
||||
|
|
@ -118,8 +137,10 @@ namespace gtsam {
|
|||
|
||||
/** Expmap around identity - create a rotation from an angle */
|
||||
static Rot2 Expmap(const Vector& v) {
|
||||
if (zero(v)) return (Rot2());
|
||||
else return Rot2::fromAngle(v(0));
|
||||
if (zero(v))
|
||||
return (Rot2());
|
||||
else
|
||||
return Rot2::fromAngle(v(0));
|
||||
}
|
||||
|
||||
/** Logmap around identity - return the angle of the rotation */
|
||||
|
|
@ -128,15 +149,18 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/** Binary expmap */
|
||||
inline Rot2 expmap(const Vector& v) const { return *this * Expmap(v); }
|
||||
inline Rot2 expmap(const Vector& v) const {
|
||||
return *this * Expmap(v);
|
||||
}
|
||||
|
||||
/** Binary Logmap */
|
||||
inline Vector logmap(const Rot2& p2) const { return Logmap(between(p2));}
|
||||
inline Vector logmap(const Rot2& p2) const {
|
||||
return Logmap(between(p2));
|
||||
}
|
||||
|
||||
/** Between using the default implementation */
|
||||
inline Rot2 between(const Rot2& p2,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
inline Rot2 between(const Rot2& p2, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
|
||||
if (H1) *H1 = -eye(1);
|
||||
if (H2) *H2 = eye(1);
|
||||
return between_default(*this, p2);
|
||||
|
|
@ -149,7 +173,9 @@ namespace gtsam {
|
|||
Matrix transpose() const;
|
||||
|
||||
/** The inverse rotation - negative angle */
|
||||
Rot2 inverse() const { return Rot2(c_, -s_);}
|
||||
Rot2 inverse() const {
|
||||
return Rot2(c_, -s_);
|
||||
}
|
||||
|
||||
/** Compose - make a new rotation by adding angles */
|
||||
Rot2 operator*(const Rot2& R) const {
|
||||
|
|
@ -160,23 +186,27 @@ namespace gtsam {
|
|||
* rotate point from rotated coordinate frame to
|
||||
* world = R*p
|
||||
*/
|
||||
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
|
||||
Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/** syntactic sugar for rotate */
|
||||
inline Point2 operator*(const Point2& p) const {return rotate(p);}
|
||||
inline Point2 operator*(const Point2& p) const {
|
||||
return rotate(p);
|
||||
}
|
||||
|
||||
/**
|
||||
* rotate point from world to rotated
|
||||
* frame = R'*p
|
||||
*/
|
||||
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 =
|
||||
boost::none, boost::optional<Matrix&> H2 = boost::none) const;
|
||||
Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const;
|
||||
|
||||
/**
|
||||
* Creates a unit vector as a Point2
|
||||
*/
|
||||
inline Point2 unit() const { return Point2(c_, s_); }
|
||||
inline Point2 unit() const {
|
||||
return Point2(c_, s_);
|
||||
}
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
@ -187,6 +217,6 @@ namespace gtsam {
|
|||
ar & BOOST_SERIALIZATION_NVP(s_);
|
||||
}
|
||||
|
||||
}; // Rot2
|
||||
};
|
||||
|
||||
} // gtsam
|
||||
|
|
|
|||
|
|
@ -13,6 +13,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -30,6 +30,8 @@ TEST( Rot2, constructors_and_angle)
|
|||
{
|
||||
double c=cos(0.1), s=sin(0.1);
|
||||
DOUBLES_EQUAL(0.1,R.theta(),1e-9);
|
||||
CHECK(assert_equal(R,Rot2(0.1)));
|
||||
CHECK(assert_equal(R,Rot2::fromAngle(0.1)));
|
||||
CHECK(assert_equal(R,Rot2::fromCosSin(c,s)));
|
||||
CHECK(assert_equal(R,Rot2::atan2(s*5,c*5)));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,9 +16,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/geometry/concepts.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -58,12 +58,12 @@ namespace gtsam {
|
|||
/** h(x)-z -> between(z,h(x)) for Rot2 manifold */
|
||||
Vector evaluateError(const Pose& pose, const Point& point,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Rot2 hx = pose.bearing(point, H1, H2);
|
||||
Rot hx = pose.bearing(point, H1, H2);
|
||||
return Rot::Logmap(z_.between(hx));
|
||||
}
|
||||
|
||||
/** return the measured */
|
||||
inline const Rot2 measured() const {
|
||||
inline const Rot measured() const {
|
||||
return z_;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,27 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Landmark2.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef gtsam::Point2 Landmark2;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -50,6 +50,11 @@ check_PROGRAMS += tests/testPose2SLAM
|
|||
sources += planarSLAM.cpp
|
||||
check_PROGRAMS += tests/testPlanarSLAM
|
||||
|
||||
# MATLAB Wrap headers for planarSLAM
|
||||
headers += Landmark2.h
|
||||
headers += PlanarSLAMGraph.h
|
||||
headers += PlanarSLAMValues.h
|
||||
|
||||
# 3D Pose constraints
|
||||
sources += pose3SLAM.cpp
|
||||
check_PROGRAMS += tests/testPose3SLAM
|
||||
|
|
|
|||
|
|
@ -0,0 +1,29 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file PlanarSLAMGraph.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/PlanarSLAM.h>
|
||||
#include <gtsam/slam/PlanarSLAMValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef gtsam::planarSLAM::Graph PlanarSLAMGraph;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,27 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file PlanarSLAMValues.h
|
||||
* @brief Re-created on Feb 22, 2010 for compatibility with MATLAB
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/slam/PlanarSLAM.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef gtsam::planarSLAM::Values PlanarSLAMValues;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -17,16 +17,17 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/slam/RangeFactor.h>
|
||||
#include <gtsam/slam/BearingFactor.h>
|
||||
#include <gtsam/slam/BearingRangeFactor.h>
|
||||
#include <gtsam/nonlinear/TupleValues.h>
|
||||
#include <gtsam/nonlinear/NonlinearEquality.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
||||
// We use gtsam namespace for generally useful factors
|
||||
namespace gtsam {
|
||||
|
|
@ -46,8 +47,25 @@ namespace gtsam {
|
|||
/// Typedef for LieValues structure with PointKey type
|
||||
typedef LieValues<PointKey> PointValues;
|
||||
|
||||
/// Typedef for the TupleValues2 to use PoseKeys and PointKeys
|
||||
typedef TupleValues2<PoseValues, PointValues> Values;
|
||||
/// Values class, inherited from TupleValues2, using PoseKeys and PointKeys
|
||||
struct Values: public TupleValues2<PoseValues, PointValues> {
|
||||
|
||||
/// Default constructor
|
||||
Values() {}
|
||||
|
||||
/// Copy constructor
|
||||
Values(const TupleValues2<PoseValues, PointValues>& values) :
|
||||
TupleValues2<PoseValues, PointValues>(values) {
|
||||
}
|
||||
|
||||
// Convenience for MATLAB wrapper, which does not allow for identically named methods
|
||||
|
||||
/// insert a pose
|
||||
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); }
|
||||
|
||||
/// insert a point
|
||||
void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); }
|
||||
};
|
||||
|
||||
/**
|
||||
* List of typedefs for factors
|
||||
|
|
@ -96,6 +114,15 @@ namespace gtsam {
|
|||
/// Creates a factor with a Rot2 between a PoseKey i and PointKey j for difference in rotation and location
|
||||
void addBearingRange(const PoseKey& i, const PointKey& j,
|
||||
const Rot2& z1, double z2, const SharedNoiseModel& model);
|
||||
|
||||
/// Optimize, mostly here for MATLAB
|
||||
boost::shared_ptr<Values> optimize(const Values& initialEstimate) {
|
||||
typedef NonlinearOptimizer<Graph, Values> Optimizer;
|
||||
// NonlinearOptimizationParameters::LAMBDA
|
||||
boost::shared_ptr<Values> result(
|
||||
new Values(*Optimizer::optimizeGN(*this, initialEstimate)));
|
||||
return result;
|
||||
}
|
||||
};
|
||||
|
||||
/// Optimizer
|
||||
|
|
|
|||
|
|
@ -210,6 +210,7 @@ void Module::matlab_code(const string& toolboxPath,
|
|||
cls.matlab_methods(classPath,nameSpace);
|
||||
|
||||
// add lines to make m-file
|
||||
ofs << "%% " << cls.name << endl;
|
||||
ofs << "cd(toolboxpath)" << endl;
|
||||
cls.matlab_make_fragment(ofs, toolboxPath, mexFlags);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,4 +24,7 @@ METHOD (AND CONSTRUCTOR) ARGUMENTS
|
|||
generation of the correct conversion routines unwrap<Vector> and unwrap<Matrix>
|
||||
- passing classes as arguments works, provided they are passed by reference.
|
||||
This triggers a call to unwrap_shared_ptr
|
||||
- GTSAM specific: keys will be cast automatically from strings via GTSAM_magic. This
|
||||
allows us to not have to declare all key types in MATLAB. Just replace key arguments with
|
||||
the (non-const, non-reference) string type
|
||||
|
||||
Loading…
Reference in New Issue