Merge remote-tracking branch 'svn/trunk' into windows
Conflicts: gtsam/nonlinear/Marginals.hrelease/4.3a0
Richard Roberts
commit
bcfe39f4ae
16
.cproject
16
.cproject
|
|
@ -689,18 +689,26 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="nonlinear.testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>nonlinear.testValues.run</buildTarget>
|
||||
<buildTarget>testValues.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="nonlinear.testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>nonlinear.testOrdering.run</buildTarget>
|
||||
<buildTarget>testOrdering.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testKey.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -64,7 +64,7 @@ int main(int argc, char** argv) {
|
|||
result.print("\nFinal result:\n ");
|
||||
|
||||
// Query the marginals
|
||||
Marginals marginals(graph, result);
|
||||
Marginals marginals = graph.marginals(result);
|
||||
cout.precision(2);
|
||||
cout << "\nP1:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(1)) << endl;
|
||||
cout << "\nP2:\n" << marginals.marginalCovariance(pose2SLAM::PoseKey(2)) << endl;
|
||||
|
|
|
|||
|
|
@ -43,8 +43,26 @@ initialEstimate.print(sprintf('\nInitial estimate:\n '));
|
|||
result = graph.optimize(initialEstimate);
|
||||
result.print(sprintf('\nFinal result:\n '));
|
||||
|
||||
%% Use an explicit Optimizer object so we can query the marginals
|
||||
% marginals = gtsamMarginals(graph, result);
|
||||
% marginals.marginalCovariance(pose2SLAMPoseKey(1))
|
||||
% marginals.marginalCovariance(pose2SLAMPoseKey(2))
|
||||
% marginals.marginalCovariance(pose2SLAMPoseKey(3))
|
||||
%% Query the marginals
|
||||
marginals = graph.marginals(result);
|
||||
x{1}=gtsamSymbol('x',1); P{1}=marginals.marginalCovariance(x{1}.key)
|
||||
x{2}=gtsamSymbol('x',2); P{2}=marginals.marginalCovariance(x{2}.key)
|
||||
x{3}=gtsamSymbol('x',3); P{3}=marginals.marginalCovariance(x{3}.key)
|
||||
|
||||
%% Plot Trajectory
|
||||
figure(1)
|
||||
clf
|
||||
X=[];Y=[];
|
||||
for i=1:3
|
||||
pose_i = result.pose(i);
|
||||
X=[X;pose_i.x];
|
||||
Y=[Y;pose_i.y];
|
||||
end
|
||||
plot(X,Y,'b*-');
|
||||
|
||||
%% Plot Covariance Ellipses
|
||||
hold on
|
||||
for i=1:3
|
||||
pose_i = result.pose(i);
|
||||
covarianceEllipse([pose_i.x;pose_i.y],P{i},'g')
|
||||
end
|
||||
|
|
|
|||
|
|
@ -0,0 +1,34 @@
|
|||
function covarianceEllipse(x,P,color)
|
||||
% covarianceEllipse: plot a Gaussian as an uncertainty ellipse
|
||||
% Based on Maybeck Vol 1, page 366
|
||||
% k=2.296 corresponds to 1 std, 68.26% of all probability
|
||||
% k=11.82 corresponds to 3 std, 99.74% of all probability
|
||||
%
|
||||
% covarianceEllipse(x,P,color)
|
||||
% it is assumed x and y are the first two components of state x
|
||||
|
||||
[e,s] = eig(P(1:2,1:2));
|
||||
s1 = s(1,1);
|
||||
s2 = s(2,2);
|
||||
k = 2.296;
|
||||
[ex,ey] = ellipse( sqrt(s1*k)*e(:,1), sqrt(s2*k)*e(:,2), x(1:2) );
|
||||
line(ex,ey,'color',color);
|
||||
|
||||
function [x,y] = ellipse(a,b,c);
|
||||
% ellipse: return the x and y coordinates for an ellipse
|
||||
% [x,y] = ellipse(a,b,c);
|
||||
% a, and b are the axes. c is the center
|
||||
|
||||
global ellipse_x ellipse_y
|
||||
if ~exist('elipse_x')
|
||||
q =0:2*pi/25:2*pi;
|
||||
ellipse_x = cos(q);
|
||||
ellipse_y = sin(q);
|
||||
end
|
||||
|
||||
points = a*ellipse_x + b*ellipse_y;
|
||||
x = c(1) + points(1,:);
|
||||
y = c(2) + points(2,:);
|
||||
end
|
||||
|
||||
end
|
||||
38
gtsam.h
38
gtsam.h
|
|
@ -347,13 +347,14 @@ class GaussianFactorGraph {
|
|||
|
||||
// Building the graph
|
||||
void add(gtsam::JacobianFactor* factor);
|
||||
void add(Vector b);
|
||||
void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
|
||||
void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
|
||||
const gtsam::SharedDiagonal& model);
|
||||
void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
|
||||
Vector b, const gtsam::SharedDiagonal& model);
|
||||
void add(gtsam::HessianFactor* factor);
|
||||
// all these won't work as MATLAB can't handle overloading
|
||||
// void add(Vector b);
|
||||
// void add(int key1, Matrix A1, Vector b, const gtsam::SharedDiagonal& model);
|
||||
// void add(int key1, Matrix A1, int key2, Matrix A2, Vector b,
|
||||
// const gtsam::SharedDiagonal& model);
|
||||
// void add(int key1, Matrix A1, int key2, Matrix A2, int key3, Matrix A3,
|
||||
// Vector b, const gtsam::SharedDiagonal& model);
|
||||
// void add(gtsam::HessianFactor* factor);
|
||||
|
||||
// error and probability
|
||||
double error(const gtsam::VectorValues& c) const;
|
||||
|
|
@ -380,7 +381,7 @@ class GaussianSequentialSolver {
|
|||
|
||||
class KalmanFilter {
|
||||
KalmanFilter(size_t n);
|
||||
gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
|
||||
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
|
||||
gtsam::GaussianDensity* init(Vector x0, Matrix P0);
|
||||
void print(string s) const;
|
||||
static int step(gtsam::GaussianDensity* p);
|
||||
|
|
@ -400,6 +401,12 @@ class KalmanFilter {
|
|||
// nonlinear
|
||||
//*************************************************************************
|
||||
|
||||
class Symbol {
|
||||
Symbol(char c, size_t j);
|
||||
void print(string s) const;
|
||||
size_t key() const;
|
||||
};
|
||||
|
||||
class Ordering {
|
||||
Ordering();
|
||||
void print(string s) const;
|
||||
|
|
@ -407,20 +414,21 @@ class Ordering {
|
|||
void push_back(size_t key);
|
||||
};
|
||||
|
||||
//Andrew says: Required definitions for Marginal arguments
|
||||
class NonlinearFactorGraph {
|
||||
NonlinearFactorGraph();
|
||||
//This need to be populated with whatever functions might be needed.
|
||||
};
|
||||
|
||||
class Values {
|
||||
Values();
|
||||
//Same here
|
||||
void print(string s) const;
|
||||
bool exists(size_t j) const;
|
||||
};
|
||||
|
||||
// Frank says: this does not work. Why not?
|
||||
class Marginals {
|
||||
Marginals(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& solution);
|
||||
void print(string s) const;
|
||||
Matrix marginalCovariance(size_t variable) const;
|
||||
Matrix marginalInformation(size_t variable) const;
|
||||
};
|
||||
|
||||
}///\namespace gtsam
|
||||
|
|
@ -471,7 +479,7 @@ class Odometry {
|
|||
}///\namespace planarSLAM
|
||||
|
||||
//*************************************************************************
|
||||
// gtsam::Pose2SLAM
|
||||
// Pose2SLAM
|
||||
//*************************************************************************
|
||||
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
|
@ -481,6 +489,7 @@ class Values {
|
|||
Values();
|
||||
void print(string s) const;
|
||||
void insertPose(int key, const gtsam::Pose2& pose);
|
||||
gtsam::Symbol poseKey(int i);
|
||||
gtsam::Pose2 pose(int i);
|
||||
};
|
||||
|
||||
|
|
@ -497,7 +506,8 @@ class Graph {
|
|||
void addPrior(int key, const gtsam::Pose2& pose, const gtsam::SharedNoiseModel& noiseModel);
|
||||
void addPoseConstraint(int key, const gtsam::Pose2& pose);
|
||||
void addOdometry(int key1, int key2, const gtsam::Pose2& odometry, const gtsam::SharedNoiseModel& noiseModel);
|
||||
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate);
|
||||
pose2SLAM::Values optimize(const pose2SLAM::Values& initialEstimate) const;
|
||||
gtsam::Marginals marginals(const pose2SLAM::Values& solution) const;
|
||||
};
|
||||
|
||||
}///\namespace pose2SLAM
|
||||
|
|
|
|||
|
|
@ -37,7 +37,7 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class CONDITIONAL>
|
||||
void BayesNet<CONDITIONAL>::print(const string& s) const {
|
||||
cout << s << ":\n";
|
||||
cout << s;
|
||||
BOOST_REVERSE_FOREACH(sharedConditional conditional,conditionals_)
|
||||
conditional->print();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -43,6 +43,14 @@ Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution,
|
|||
bayesTree_ = *GaussianMultifrontalSolver(graph_, true).eliminate();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void Marginals::print(const std::string& str, const KeyFormatter& keyFormatter) const {
|
||||
ordering_.print(str+"Ordering: ", keyFormatter);
|
||||
graph_.print(str+"Graph: ");
|
||||
values_.print(str+"Solution: ", keyFormatter);
|
||||
bayesTree_.print(str+"Bayes Tree: ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Marginals::marginalCovariance(Key variable) const {
|
||||
return marginalInformation(variable).inverse();
|
||||
|
|
|
|||
|
|
@ -40,6 +40,16 @@ public:
|
|||
QR
|
||||
};
|
||||
|
||||
protected:
|
||||
|
||||
GaussianFactorGraph graph_;
|
||||
Ordering ordering_;
|
||||
Values values_;
|
||||
Factorization factorization_;
|
||||
GaussianBayesTree bayesTree_;
|
||||
|
||||
public:
|
||||
|
||||
/** Construct a marginals class.
|
||||
* @param graph The factor graph defining the full joint density on all variables.
|
||||
* @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
|
||||
|
|
@ -47,6 +57,9 @@ public:
|
|||
*/
|
||||
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
|
||||
|
||||
/** print */
|
||||
void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
|
||||
|
||||
/** Compute the marginal covariance of a single variable */
|
||||
Matrix marginalCovariance(Key variable) const;
|
||||
|
||||
|
|
@ -60,14 +73,6 @@ public:
|
|||
|
||||
/** Compute the joint marginal information of several variables */
|
||||
JointMarginal jointMarginalInformation(const std::vector<Key>& variables) const;
|
||||
|
||||
protected:
|
||||
|
||||
GaussianFactorGraph graph_;
|
||||
Values values_;
|
||||
Ordering ordering_;
|
||||
Factorization factorization_;
|
||||
GaussianBayesTree bayesTree_;
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -76,7 +81,17 @@ protected:
|
|||
class JointMarginal {
|
||||
|
||||
protected:
|
||||
typedef SymmetricBlockView<Matrix> BlockView;
|
||||
|
||||
typedef SymmetricBlockView<Matrix> BlockView;
|
||||
|
||||
Matrix fullMatrix_;
|
||||
BlockView blockView_;
|
||||
Ordering indices_;
|
||||
|
||||
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
|
||||
fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
|
||||
|
||||
friend class Marginals;
|
||||
|
||||
public:
|
||||
/** A block view of the joint marginal - this stores a reference to the
|
||||
|
|
@ -107,16 +122,6 @@ public:
|
|||
|
||||
/** Assignment operator */
|
||||
JointMarginal& operator=(const JointMarginal& rhs);
|
||||
|
||||
protected:
|
||||
Matrix fullMatrix_;
|
||||
BlockView blockView_;
|
||||
Ordering indices_;
|
||||
|
||||
JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const Ordering& indices) :
|
||||
fullMatrix_(fullMatrix), blockView_(fullMatrix_, dims.begin(), dims.end()), indices_(indices) {}
|
||||
|
||||
friend class Marginals;
|
||||
};
|
||||
|
||||
} /* namespace gtsam */
|
||||
|
|
|
|||
|
|
@ -18,8 +18,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <list>
|
||||
#include <iostream>
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
|
||||
#include <boost/mpl/char.hpp>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
|
@ -28,7 +28,8 @@
|
|||
#include <boost/lambda/construct.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
|
||||
#include <gtsam/nonlinear/Key.h>
|
||||
#include <list>
|
||||
#include <iostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -70,8 +71,8 @@ public:
|
|||
j_ = key & indexMask;
|
||||
}
|
||||
|
||||
/** Cast to integer */
|
||||
operator Key() const {
|
||||
/** return Key (integer) representation */
|
||||
Key key() const {
|
||||
const size_t keyBits = sizeof(Key) * 8;
|
||||
const size_t chrBits = sizeof(unsigned char) * 8;
|
||||
const size_t indexBits = keyBits - chrBits;
|
||||
|
|
@ -83,9 +84,12 @@ public:
|
|||
return key;
|
||||
}
|
||||
|
||||
/** Cast to integer */
|
||||
operator Key() const { return key(); }
|
||||
|
||||
// Testable Requirements
|
||||
void print(const std::string& s = "") const {
|
||||
std::cout << s << ": " << (std::string) (*this) << std::endl;
|
||||
std::cout << s << (std::string) (*this) << std::endl;
|
||||
}
|
||||
bool equals(const Symbol& expected, double tol = 0.0) const {
|
||||
return (*this) == expected;
|
||||
|
|
|
|||
|
|
@ -24,8 +24,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <gtsam/base/Value.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
|
||||
#include <boost/pool/pool_alloc.hpp>
|
||||
#include <boost/ptr_container/ptr_map.hpp>
|
||||
|
|
@ -37,10 +39,8 @@
|
|||
#include <boost/iterator_adaptors.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
|
||||
#include <gtsam/base/Value.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/nonlinear/Ordering.h>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
|
|||
|
|
@ -27,11 +27,11 @@ using namespace gtsam;
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(Key, KeySymbolConversion) {
|
||||
Symbol expected('j', 4);
|
||||
Key key(expected);
|
||||
Symbol original('j', 4);
|
||||
Key key(original);
|
||||
EXPECT(assert_equal(key, original.key()))
|
||||
Symbol actual(key);
|
||||
|
||||
EXPECT(assert_equal(expected, actual))
|
||||
EXPECT(assert_equal(original, actual))
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -36,12 +36,12 @@ using pose2SLAM::PoseKey;
|
|||
|
||||
// common measurement covariance
|
||||
static double sx=0.5, sy=0.5,st=0.1;
|
||||
static noiseModel::Gaussian::shared_ptr covariance(
|
||||
noiseModel::Gaussian::Covariance(Matrix_(3, 3,
|
||||
static Matrix cov(Matrix_(3, 3,
|
||||
sx*sx, 0.0, 0.0,
|
||||
0.0, sy*sy, 0.0,
|
||||
0.0, 0.0, st*st
|
||||
)));
|
||||
));
|
||||
static noiseModel::Gaussian::shared_ptr covariance(noiseModel::Gaussian::Covariance(cov));
|
||||
//static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3));
|
||||
|
||||
const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1);
|
||||
|
|
@ -175,6 +175,12 @@ TEST(Pose2SLAM, optimize) {
|
|||
|
||||
// Check marginals
|
||||
Marginals marginals = fg.marginals(actual);
|
||||
// Matrix expectedP0 = Infinity, as we have a pose constraint !?
|
||||
// Matrix actualP0 = marginals.marginalCovariance(pose2SLAM::PoseKey(0));
|
||||
// EQUALITY(expectedP0, actualP0);
|
||||
Matrix expectedP1 = cov; // the second pose really should have just the noise covariance
|
||||
Matrix actualP1 = marginals.marginalCovariance(pose2SLAM::PoseKey(1));
|
||||
EQUALITY(expectedP1, actualP1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -12,7 +12,6 @@ set(tests_local_libs
|
|||
# exclude certain files
|
||||
# note the source dir on each
|
||||
set (tests_exclude
|
||||
"${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp"
|
||||
#"${CMAKE_CURRENT_SOURCE_DIR}/testOccupancyGrid.cpp"
|
||||
)
|
||||
|
||||
|
|
|
|||
|
|
@ -286,84 +286,38 @@ TEST_UNSAFE( OccupancyGrid, Test1) {
|
|||
//Build a small grid and test optimization
|
||||
|
||||
//Build small grid
|
||||
double width = 20; //meters
|
||||
double height = 20; //meters
|
||||
double resolution = 0.2; //meters
|
||||
double width = 3; //meters
|
||||
double height = 2; //meters
|
||||
double resolution = 0.5; //meters
|
||||
OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
|
||||
|
||||
//Add measurements
|
||||
// Pose2 pose(0,0,0);
|
||||
// double range = 4.499765;
|
||||
//
|
||||
// occupancyGrid.addPrior(0, 0.7);
|
||||
// EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
|
||||
//
|
||||
// occupancyGrid.addLaser(pose, range);
|
||||
// EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
|
||||
Pose2 pose(0,0,0);
|
||||
double range = 1;
|
||||
|
||||
//add lasers
|
||||
int n_frames = 1;
|
||||
int n_lasers_per_frame = 640;
|
||||
char laser_list_file[1000];
|
||||
occupancyGrid.addPrior(0, 0.7);
|
||||
EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
|
||||
|
||||
occupancyGrid.addLaser(pose, range);
|
||||
EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
|
||||
|
||||
OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
|
||||
EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
|
||||
|
||||
for(int i = 0; i < n_frames; i++){
|
||||
sprintf(laser_list_file, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/Data/ScanLinesAsLasers/KinectRecording9/laser_list%.4d", i);
|
||||
FILE *fptr = fopen(laser_list_file,"r");
|
||||
double x,y, theta;
|
||||
double range, angle;
|
||||
fscanf(fptr, "%lf %lf %lf", &x, &y, &theta);
|
||||
occupancy[16] = 1;
|
||||
EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
|
||||
for(int j = 0; j < n_lasers_per_frame; j++){
|
||||
fscanf(fptr, "%lf %lf", &range, &angle);
|
||||
//if(j == 159){
|
||||
Pose2 pose(x,y, theta+angle);
|
||||
occupancy[15] = 1;
|
||||
EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
|
||||
occupancyGrid.addLaser(pose, range);
|
||||
//}
|
||||
}
|
||||
fclose(fptr);
|
||||
|
||||
}
|
||||
|
||||
|
||||
// OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
|
||||
// EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
//
|
||||
//
|
||||
// occupancy[16] = 1;
|
||||
// EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
//
|
||||
// occupancy[15] = 1;
|
||||
// EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
//
|
||||
// occupancy[16] = 0;
|
||||
// EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
occupancy[16] = 0;
|
||||
EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
|
||||
|
||||
//run MCMC
|
||||
OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
|
||||
//EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
|
||||
//select a cell at a random to flip
|
||||
|
||||
|
||||
printf("\n");
|
||||
for(size_t i = 0, it = 0; i < occupancyGrid.height(); i++){
|
||||
for(size_t j = 0; j < occupancyGrid.width(); j++, it++){
|
||||
printf("%.2lf ", occupancyMarginals[it]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
char marginalsOutput[1000];
|
||||
sprintf(marginalsOutput, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/marginals.txt");
|
||||
FILE *fptr = fopen(marginalsOutput, "w");
|
||||
fprintf(fptr, "%d %d\n", occupancyGrid.width(), occupancyGrid.height());
|
||||
|
||||
for(int i = 0; i < occupancyMarginals.size(); i++){
|
||||
fprintf(fptr, "%lf ", occupancyMarginals[i]);
|
||||
}
|
||||
fclose(fptr);
|
||||
EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,72 +0,0 @@
|
|||
/**
|
||||
* @file testPose2SLAMwSPCG
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using namespace pose2SLAM;
|
||||
|
||||
const double tol = 1e-5;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(testPose2SLAMwSPCG, example1) {
|
||||
|
||||
/* generate synthetic data */
|
||||
const SharedNoiseModel sigma(noiseModel::Unit::Create(0.1));
|
||||
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
|
||||
|
||||
// create a 3 by 3 grid
|
||||
// x3 x6 x9
|
||||
// x2 x5 x8
|
||||
// x1 x4 x7
|
||||
Graph graph;
|
||||
graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ;
|
||||
graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ;
|
||||
graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ;
|
||||
graph.addPrior(x1, Pose2(0,0,0), sigma) ;
|
||||
|
||||
Values initial;
|
||||
initial.insert(x1, Pose2( 0, 0, 0));
|
||||
initial.insert(x2, Pose2( 0, 2.1, 0.01));
|
||||
initial.insert(x3, Pose2( 0, 3.9,-0.01));
|
||||
initial.insert(x4, Pose2(2.1,-0.1, 0));
|
||||
initial.insert(x5, Pose2(1.9, 2.1, 0.02));
|
||||
initial.insert(x6, Pose2(2.0, 3.9,-0.02));
|
||||
initial.insert(x7, Pose2(4.0, 0.1, 0.03 ));
|
||||
initial.insert(x8, Pose2(3.9, 2.1, 0.01));
|
||||
initial.insert(x9, Pose2(4.1, 3.9,-0.01));
|
||||
|
||||
Values expected;
|
||||
expected.insert(x1, Pose2(0.0, 0.0, 0.0));
|
||||
expected.insert(x2, Pose2(0.0, 2.0, 0.0));
|
||||
expected.insert(x3, Pose2(0.0, 4.0, 0.0));
|
||||
expected.insert(x4, Pose2(2.0, 0.0, 0.0));
|
||||
expected.insert(x5, Pose2(2.0, 2.0, 0.0));
|
||||
expected.insert(x6, Pose2(2.0, 4.0, 0.0));
|
||||
expected.insert(x7, Pose2(4.0, 0.0, 0.0 ));
|
||||
expected.insert(x8, Pose2(4.0, 2.0, 0.0));
|
||||
expected.insert(x9, Pose2(4.0, 4.0, 0.0));
|
||||
|
||||
Values actual = optimizeSPCG(graph, initial);
|
||||
|
||||
EXPECT(assert_equal(expected, actual, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -206,39 +206,53 @@ string unwrap<string>(const mxArray* array) {
|
|||
return str;
|
||||
}
|
||||
|
||||
// Check for 64-bit, as Mathworks says mxGetScalar only good for 32 bit
|
||||
template <typename T>
|
||||
T myGetScalar(const mxArray* array) {
|
||||
switch (mxGetClassID(array)) {
|
||||
case mxINT64_CLASS:
|
||||
return (T) *(int64_t*) mxGetData(array);
|
||||
case mxUINT64_CLASS:
|
||||
return (T) *(uint64_t*) mxGetData(array);
|
||||
default:
|
||||
// hope for the best!
|
||||
return (T) mxGetScalar(array);
|
||||
}
|
||||
}
|
||||
|
||||
// specialization to bool
|
||||
template<>
|
||||
bool unwrap<bool>(const mxArray* array) {
|
||||
checkScalar(array,"unwrap<bool>");
|
||||
return mxGetScalar(array) != 0.0;
|
||||
return myGetScalar<bool>(array);
|
||||
}
|
||||
|
||||
// specialization to bool
|
||||
template<>
|
||||
char unwrap<char>(const mxArray* array) {
|
||||
checkScalar(array,"unwrap<char>");
|
||||
return (char)mxGetScalar(array);
|
||||
}
|
||||
|
||||
// specialization to size_t
|
||||
template<>
|
||||
size_t unwrap<size_t>(const mxArray* array) {
|
||||
checkScalar(array,"unwrap<size_t>");
|
||||
return (size_t)mxGetScalar(array);
|
||||
return myGetScalar<char>(array);
|
||||
}
|
||||
|
||||
// specialization to int
|
||||
template<>
|
||||
int unwrap<int>(const mxArray* array) {
|
||||
checkScalar(array,"unwrap<int>");
|
||||
return (int)mxGetScalar(array);
|
||||
return myGetScalar<int>(array);
|
||||
}
|
||||
|
||||
// specialization to size_t
|
||||
template<>
|
||||
size_t unwrap<size_t>(const mxArray* array) {
|
||||
checkScalar(array, "unwrap<size_t>");
|
||||
return myGetScalar<size_t>(array);
|
||||
}
|
||||
|
||||
// specialization to double
|
||||
template<>
|
||||
double unwrap<double>(const mxArray* array) {
|
||||
checkScalar(array,"unwrap<double>");
|
||||
return (double)mxGetScalar(array);
|
||||
return myGetScalar<double>(array);
|
||||
}
|
||||
|
||||
// specialization to Eigen vector
|
||||
|
|
|
|||
Loading…
Reference in New Issue