Merge remote-tracking branch 'origin/develop' into feature/Feature/FixedValues
dellaert
commit
bac79bee12
79
gtsam.h
79
gtsam.h
|
|
@ -629,28 +629,13 @@ class Cal3_S2 {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
class Cal3DS2 {
|
||||
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||
virtual class Cal3DS2_Base {
|
||||
// Standard Constructors
|
||||
Cal3DS2();
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
|
||||
Cal3DS2(Vector v);
|
||||
Cal3DS2_Base();
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
|
||||
|
||||
// Manifold
|
||||
static size_t Dim();
|
||||
size_t dim() const;
|
||||
gtsam::Cal3DS2 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
|
||||
|
||||
// Action on Point2
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
|
||||
// TODO: D2d functions that start with an uppercase letter
|
||||
|
||||
// Standard Interface
|
||||
double fx() const;
|
||||
|
|
@ -658,14 +643,66 @@ class Cal3DS2 {
|
|||
double skew() const;
|
||||
double px() const;
|
||||
double py() const;
|
||||
Vector vector() const;
|
||||
Vector k() const;
|
||||
//Matrix K() const; //FIXME: Uppercase
|
||||
double k1() const;
|
||||
double k2() const;
|
||||
|
||||
// Action on Point2
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
|
||||
// Standard Constructors
|
||||
Cal3DS2();
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
|
||||
Cal3DS2(Vector v);
|
||||
|
||||
// Testable
|
||||
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
|
||||
|
||||
// Manifold
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
gtsam::Cal3DS2 retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
|
||||
// Standard Constructors
|
||||
Cal3Unified();
|
||||
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
|
||||
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
|
||||
Cal3Unified(Vector v);
|
||||
|
||||
// Testable
|
||||
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
|
||||
|
||||
// Standard Interface
|
||||
double xi() const;
|
||||
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
|
||||
|
||||
// Manifold
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
gtsam::Cal3Unified retract(Vector v) const;
|
||||
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
};
|
||||
|
||||
#include <gtsam/geometry/Cal3_S2Stereo.h>
|
||||
class Cal3_S2Stereo {
|
||||
// Standard Constructors
|
||||
Cal3_S2Stereo();
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const ;
|
||||
virtual void print(const std::string& s = "") const ;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
|
||||
|
|
@ -89,10 +89,20 @@ public:
|
|||
/// Return dimensions of calibration manifold object
|
||||
static size_t Dim() { return 9; } //TODO: make a final dimension variable
|
||||
|
||||
/// @}
|
||||
/// @name Clone
|
||||
/// @{
|
||||
|
||||
/// @return a deep copy of this object
|
||||
virtual boost::shared_ptr<Base> clone() const {
|
||||
return boost::shared_ptr<Base>(new Cal3DS2(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
|
|
|
|||
|
|
@ -45,9 +45,6 @@ protected:
|
|||
double p1_, p2_ ; // tangential distortion
|
||||
|
||||
public:
|
||||
Matrix3 K() const ;
|
||||
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
|
||||
Vector9 vector() const ;
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -59,6 +56,8 @@ public:
|
|||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
|
||||
|
||||
virtual ~Cal3DS2_Base() {}
|
||||
|
||||
/// @}
|
||||
/// @name Advanced Constructors
|
||||
/// @{
|
||||
|
|
@ -70,7 +69,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const ;
|
||||
virtual void print(const std::string& s = "") const ;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
|
||||
|
|
@ -106,6 +105,15 @@ public:
|
|||
/// Second tangential distortion coefficient
|
||||
inline double p2() const { return p2_;}
|
||||
|
||||
/// return calibration matrix -- not really applicable
|
||||
Matrix3 K() const;
|
||||
|
||||
/// return distortion parameter vector
|
||||
Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
|
||||
|
||||
/// Return all parameters as a vector
|
||||
Vector9 vector() const;
|
||||
|
||||
/**
|
||||
* convert intrinsic coordinates xy to (distorted) image coordinates uv
|
||||
* @param p point in intrinsic coordinates
|
||||
|
|
@ -126,9 +134,19 @@ public:
|
|||
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||
Matrix29 D2d_calibration(const Point2& p) const ;
|
||||
|
||||
private:
|
||||
/// @}
|
||||
/// @name Clone
|
||||
/// @{
|
||||
|
||||
/// @return a deep copy of this object
|
||||
virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
|
||||
return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
/// @name Advanced Interface
|
||||
/// @{
|
||||
|
||||
|
|
|
|||
|
|
@ -50,9 +50,8 @@ private:
|
|||
double xi_; // mirror parameter
|
||||
|
||||
public:
|
||||
enum { dimension = 10 };
|
||||
|
||||
Vector10 vector() const ;
|
||||
enum { dimension = 10 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -77,7 +76,7 @@ public:
|
|||
/// @{
|
||||
|
||||
/// print with optional string
|
||||
void print(const std::string& s = "") const ;
|
||||
virtual void print(const std::string& s = "") const ;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3Unified& K, double tol = 10e-9) const;
|
||||
|
|
@ -125,6 +124,11 @@ public:
|
|||
/// Return dimensions of calibration manifold object
|
||||
static size_t Dim() { return 10; } //TODO: make a final dimension variable
|
||||
|
||||
/// Return all parameters as a vector
|
||||
Vector10 vector() const ;
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
||||
/** Serialization function */
|
||||
|
|
|
|||
|
|
@ -8,6 +8,8 @@ function h = covarianceEllipse(x,P,color, k)
|
|||
% it is assumed x and y are the first two components of state x
|
||||
% k is scaling for std deviations, defaults to 1 std
|
||||
|
||||
hold on
|
||||
|
||||
[e,s] = eig(P(1:2,1:2));
|
||||
s1 = s(1,1);
|
||||
s2 = s(2,2);
|
||||
|
|
@ -32,4 +34,4 @@ h = line(ex,ey,'color',color);
|
|||
y = c(2) + points(2,:);
|
||||
end
|
||||
|
||||
end
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
function covarianceEllipse3D(c,P)
|
||||
function sc = covarianceEllipse3D(c,P,scale)
|
||||
% covarianceEllipse3D plots 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
|
||||
|
|
@ -6,10 +6,16 @@ function covarianceEllipse3D(c,P)
|
|||
%
|
||||
% Modified from http://www.mathworks.com/matlabcentral/newsreader/view_thread/42966
|
||||
|
||||
hold on
|
||||
|
||||
[e,s] = svd(P);
|
||||
k = 11.82;
|
||||
radii = k*sqrt(diag(s));
|
||||
|
||||
if exist('scale', 'var')
|
||||
radii = radii * scale;
|
||||
end
|
||||
|
||||
% generate data for "unrotated" ellipsoid
|
||||
[xc,yc,zc] = ellipsoid(0,0,0,radii(1),radii(2),radii(3),8);
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,89 @@
|
|||
function [visiblePoints] = cylinderSampleProjection(K, pose, imageSize, cylinders)
|
||||
|
||||
% Input:
|
||||
% Output:
|
||||
% visiblePoints: data{k} 3D Point in overal point clouds with index k
|
||||
% Z{k} 2D measurements in overal point clouds with index k
|
||||
% index {i}{j}
|
||||
% i: the cylinder index;
|
||||
% j: the point index on the cylinder;
|
||||
%
|
||||
% @Description: Project sampled points on cylinder to camera frame
|
||||
% @Authors: Zhaoyang Lv
|
||||
|
||||
import gtsam.*
|
||||
|
||||
camera = SimpleCamera(pose, K);
|
||||
|
||||
%% memory allocation
|
||||
cylinderNum = length(cylinders);
|
||||
|
||||
%% check visiblity of points on each cylinder
|
||||
pointCloudIndex = 0;
|
||||
visiblePointIdx = 1;
|
||||
for i = 1:cylinderNum
|
||||
|
||||
pointNum = length(cylinders{i}.Points);
|
||||
|
||||
% to check point visibility
|
||||
for j = 1:pointNum
|
||||
|
||||
pointCloudIndex = pointCloudIndex + 1;
|
||||
|
||||
% Cheirality Exception
|
||||
sampledPoint3 = cylinders{i}.Points{j};
|
||||
sampledPoint3local = pose.transform_to(sampledPoint3);
|
||||
if sampledPoint3local.z <= 0
|
||||
continue;
|
||||
end
|
||||
Z2d = camera.project(sampledPoint3);
|
||||
|
||||
% ignore points not visible in the scene
|
||||
if Z2d.x < 0 || Z2d.x >= imageSize.x || Z2d.y < 0 || Z2d.y >= imageSize.y
|
||||
continue;
|
||||
end
|
||||
|
||||
% ignore points occluded
|
||||
% use a simple math hack to check occlusion:
|
||||
% 1. All points in front of cylinders' surfaces are visible
|
||||
% 2. For points behind the cylinders' surfaces, the cylinder
|
||||
visible = true;
|
||||
for k = 1:cylinderNum
|
||||
|
||||
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
|
||||
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
|
||||
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
|
||||
|
||||
% Condition 1: all points in front of the cylinders'
|
||||
% surfaces are visible
|
||||
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
|
||||
continue;
|
||||
else
|
||||
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
|
||||
if projectedRay > 0
|
||||
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
|
||||
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
|
||||
rayCylinderToPoint(2) > cylinders{k}.radius
|
||||
continue;
|
||||
else
|
||||
visible = false;
|
||||
break;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
if visible
|
||||
visiblePoints.data{visiblePointIdx} = sampledPoint3;
|
||||
visiblePoints.Z{visiblePointIdx} = Z2d;
|
||||
visiblePoints.cylinderIdx{visiblePointIdx} = i;
|
||||
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
|
||||
visiblePointIdx = visiblePointIdx + 1;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
function [visiblePoints] = cylinderSampleProjectionStereo(K, pose, imageSize, cylinders)
|
||||
|
||||
import gtsam.*
|
||||
|
||||
%% memory allocation
|
||||
cylinderNum = length(cylinders);
|
||||
|
||||
visiblePoints.data = cell(1);
|
||||
visiblePoints.Z = cell(1);
|
||||
visiblePoints.cylinderIdx = cell(1);
|
||||
visiblePoints.overallIdx = cell(1);
|
||||
|
||||
%% check visiblity of points on each cylinder
|
||||
pointCloudIndex = 0;
|
||||
visiblePointIdx = 1;
|
||||
for i = 1:cylinderNum
|
||||
|
||||
pointNum = length(cylinders{i}.Points);
|
||||
|
||||
% to check point visibility
|
||||
for j = 1:pointNum
|
||||
|
||||
pointCloudIndex = pointCloudIndex + 1;
|
||||
|
||||
% For Cheirality Exception
|
||||
sampledPoint3 = cylinders{i}.Points{j};
|
||||
sampledPoint3local = pose.transform_to(sampledPoint3);
|
||||
if sampledPoint3local.z < 0
|
||||
continue;
|
||||
end
|
||||
|
||||
% measurements
|
||||
Z.du = K.fx() * K.baseline() / sampledPoint3local.z;
|
||||
Z.uL = K.fx() * sampledPoint3local.x / sampledPoint3local.z + K.px();
|
||||
Z.uR = Z.uL + Z.du;
|
||||
Z.v = K.fy() / sampledPoint3local.z + K.py();
|
||||
|
||||
% ignore points not visible in the scene
|
||||
if Z.uL < 0 || Z.uL >= imageSize.x || ...
|
||||
Z.uR < 0 || Z.uR >= imageSize.x || ...
|
||||
Z.v < 0 || Z.v >= imageSize.y
|
||||
continue;
|
||||
end
|
||||
|
||||
% too small disparity may call indeterminant system exception
|
||||
if Z.du < 0.6
|
||||
continue;
|
||||
end
|
||||
|
||||
% ignore points occluded
|
||||
% use a simple math hack to check occlusion:
|
||||
% 1. All points in front of cylinders' surfaces are visible
|
||||
% 2. For points behind the cylinders' surfaces, the cylinder
|
||||
visible = true;
|
||||
for k = 1:cylinderNum
|
||||
|
||||
rayCameraToPoint = pose.translation().between(sampledPoint3).vector();
|
||||
rayCameraToCylinder = pose.translation().between(cylinders{k}.centroid).vector();
|
||||
rayCylinderToPoint = cylinders{k}.centroid.between(sampledPoint3).vector();
|
||||
|
||||
% Condition 1: all points in front of the cylinders'
|
||||
% surfaces are visible
|
||||
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
|
||||
continue;
|
||||
else
|
||||
projectedRay = dot(rayCameraToCylinder, rayCameraToPoint) / norm(rayCameraToCylinder);
|
||||
if projectedRay > 0
|
||||
%rayCylinderToProjected = rayCameraToCylinder - norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
|
||||
if rayCylinderToPoint(1) > cylinders{k}.radius && ...
|
||||
rayCylinderToPoint(2) > cylinders{k}.radius
|
||||
continue;
|
||||
else
|
||||
visible = false;
|
||||
break;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
if visible
|
||||
visiblePoints.data{visiblePointIdx} = sampledPoint3;
|
||||
visiblePoints.Z{visiblePointIdx} = Z;
|
||||
visiblePoints.cylinderIdx{visiblePointIdx} = i;
|
||||
visiblePoints.overallIdx{visiblePointIdx} = pointCloudIndex;
|
||||
visiblePointIdx = visiblePointIdx + 1;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
|
||||
%
|
||||
% @author: Zhaoyang Lv
|
||||
import gtsam.*
|
||||
% calculate the cylinder area
|
||||
area = 2 * pi * radius * height;
|
||||
|
||||
pointsNum = round(area * density);
|
||||
|
||||
points3 = cell(pointsNum, 1);
|
||||
|
||||
% sample the points
|
||||
for i = 1:pointsNum
|
||||
theta = 2 * pi * rand;
|
||||
x = radius * cos(theta) + baseCentroid.x;
|
||||
y = radius * sin(theta) + baseCentroid.y;
|
||||
z = height * rand;
|
||||
points3{i,1} = Point3([x,y,z]');
|
||||
end
|
||||
|
||||
cylinder.area = area;
|
||||
cylinder.radius = radius;
|
||||
cylinder.height = height;
|
||||
cylinder.Points = points3;
|
||||
cylinder.centroid = Point3(baseCentroid.x, baseCentroid.y, height/2);
|
||||
end
|
||||
|
|
@ -1,18 +1,20 @@
|
|||
function plotCamera(pose, axisLength)
|
||||
hold on
|
||||
|
||||
C = pose.translation().vector();
|
||||
R = pose.rotation().matrix();
|
||||
|
||||
xAxis = C+R(:,1)*axisLength;
|
||||
L = [C xAxis]';
|
||||
line(L(:,1),L(:,2),L(:,3),'Color','r');
|
||||
h_x = line(L(:,1),L(:,2),L(:,3),'Color','r');
|
||||
|
||||
yAxis = C+R(:,2)*axisLength;
|
||||
L = [C yAxis]';
|
||||
line(L(:,1),L(:,2),L(:,3),'Color','g');
|
||||
h_y = line(L(:,1),L(:,2),L(:,3),'Color','g');
|
||||
|
||||
zAxis = C+R(:,3)*axisLength;
|
||||
L = [C zAxis]';
|
||||
line(L(:,1),L(:,2),L(:,3),'Color','b');
|
||||
h_z = line(L(:,1),L(:,2),L(:,3),'Color','b');
|
||||
|
||||
axis equal
|
||||
end
|
||||
end
|
||||
|
|
|
|||
|
|
@ -0,0 +1,35 @@
|
|||
function plotCylinderSamples(cylinders, options, figID)
|
||||
% plot the cylinders on the given field
|
||||
% @author: Zhaoyang Lv
|
||||
|
||||
figure(figID);
|
||||
|
||||
holdstate = ishold;
|
||||
hold on
|
||||
|
||||
num = size(cylinders, 1);
|
||||
|
||||
sampleDensity = 120;
|
||||
|
||||
for i = 1:num
|
||||
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
|
||||
|
||||
X = X + cylinders{i}.centroid.x;
|
||||
Y = Y + cylinders{i}.centroid.y;
|
||||
Z = Z * cylinders{i}.height;
|
||||
|
||||
cylinderHandle = surf(X,Y,Z);
|
||||
set(cylinderHandle, 'FaceAlpha', 0.5);
|
||||
hold on
|
||||
end
|
||||
|
||||
axis equal
|
||||
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, 20]);
|
||||
|
||||
grid on
|
||||
|
||||
if ~holdstate
|
||||
hold off
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,177 @@
|
|||
function plotFlyingResults(pts3d, poses, posesCov, cylinders, options)
|
||||
% plot the visible points on the cylinders and trajectories
|
||||
%
|
||||
% author: Zhaoyang Lv
|
||||
|
||||
import gtsam.*
|
||||
|
||||
figID = 1;
|
||||
figure(figID);
|
||||
set(gcf, 'Position', [80,1,1800,1000]);
|
||||
|
||||
|
||||
%% plot all the cylinders and sampled points
|
||||
|
||||
axis equal
|
||||
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
|
||||
xlabel('X (m)');
|
||||
ylabel('Y (m)');
|
||||
zlabel('Height (m)');
|
||||
|
||||
h = cameratoolbar('Show');
|
||||
|
||||
if options.camera.IS_MONO
|
||||
h_title = title('Quadrotor Flight Simulation with Monocular Camera');
|
||||
else
|
||||
h_title = title('Quadrotor Flight Simulation with Stereo Camera');
|
||||
end
|
||||
|
||||
text(100,1750,0, sprintf('Flying Speed: %0.1f\n', options.speed))
|
||||
|
||||
view([30, 30]);
|
||||
|
||||
hlight = camlight('headlight');
|
||||
lighting gouraud
|
||||
|
||||
if(options.writeVideo)
|
||||
videoObj = VideoWriter('Camera_Flying_Example.avi');
|
||||
videoObj.Quality = 100;
|
||||
videoObj.FrameRate = options.camera.fps;
|
||||
open(videoObj);
|
||||
end
|
||||
|
||||
|
||||
sampleDensity = 120;
|
||||
cylinderNum = length(cylinders);
|
||||
h_cylinder = cell(cylinderNum);
|
||||
for i = 1:cylinderNum
|
||||
|
||||
hold on
|
||||
|
||||
[X,Y,Z] = cylinder(cylinders{i}.radius, sampleDensity * cylinders{i}.radius * cylinders{i}.height);
|
||||
|
||||
X = X + cylinders{i}.centroid.x;
|
||||
Y = Y + cylinders{i}.centroid.y;
|
||||
Z = Z * cylinders{i}.height;
|
||||
|
||||
h_cylinder{i} = surf(X,Y,Z);
|
||||
set(h_cylinder{i}, 'FaceColor', [0 0 1], 'FaceAlpha', 0.2);
|
||||
h_cylinder{i}.AmbientStrength = 0.8;
|
||||
|
||||
end
|
||||
|
||||
%% plot trajectories and points
|
||||
posesSize = length(poses);
|
||||
pointSize = length(pts3d);
|
||||
for i = 1:posesSize
|
||||
if i > 1
|
||||
hold on
|
||||
plot3([poses{i}.x; poses{i-1}.x], [poses{i}.y; poses{i-1}.y], [poses{i}.z; poses{i-1}.z], '-b');
|
||||
end
|
||||
|
||||
if exist('h_pose_cov', 'var')
|
||||
delete(h_pose_cov);
|
||||
end
|
||||
|
||||
%plotCamera(poses{i}, 3);
|
||||
|
||||
gRp = poses{i}.rotation().matrix(); % rotation from pose to global
|
||||
C = poses{i}.translation().vector();
|
||||
axisLength = 3;
|
||||
|
||||
xAxis = C+gRp(:,1)*axisLength;
|
||||
L = [C xAxis]';
|
||||
line(L(:,1),L(:,2),L(:,3),'Color','r');
|
||||
|
||||
yAxis = C+gRp(:,2)*axisLength;
|
||||
L = [C yAxis]';
|
||||
line(L(:,1),L(:,2),L(:,3),'Color','g');
|
||||
|
||||
zAxis = C+gRp(:,3)*axisLength;
|
||||
L = [C zAxis]';
|
||||
line(L(:,1),L(:,2),L(:,3),'Color','b');
|
||||
|
||||
pPp = posesCov{i}(4:6,4:6); % covariance matrix in pose coordinate frame
|
||||
gPp = gRp*pPp*gRp'; % convert the covariance matrix to global coordinate frame
|
||||
h_pose_cov = gtsam.covarianceEllipse3D(C, gPp, options.plot.covarianceScale);
|
||||
|
||||
if exist('h_point', 'var')
|
||||
for j = 1:pointSize
|
||||
delete(h_point{j});
|
||||
end
|
||||
end
|
||||
if exist('h_point_cov', 'var')
|
||||
for j = 1:pointSize
|
||||
delete(h_point_cov{j});
|
||||
end
|
||||
end
|
||||
|
||||
h_point = cell(pointSize, 1);
|
||||
h_point_cov = cell(pointSize, 1);
|
||||
for j = 1:pointSize
|
||||
if ~isempty(pts3d{j}.cov{i})
|
||||
hold on
|
||||
h_point{j} = plot3(pts3d{j}.data.x, pts3d{j}.data.y, pts3d{j}.data.z);
|
||||
h_point_cov{j} = gtsam.covarianceEllipse3D([pts3d{j}.data.x; pts3d{j}.data.y; pts3d{j}.data.z], ...
|
||||
pts3d{j}.cov{i}, options.plot.covarianceScale);
|
||||
end
|
||||
end
|
||||
|
||||
axis equal
|
||||
axis([0, options.fieldSize.x, 0, options.fieldSize.y, 0, options.height + 30]);
|
||||
|
||||
drawnow
|
||||
|
||||
if options.writeVideo
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame);
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
if exist('h_pose_cov', 'var')
|
||||
delete(h_pose_cov);
|
||||
end
|
||||
|
||||
% wait for two seconds
|
||||
pause(2);
|
||||
|
||||
%% change views angle
|
||||
for i = 30 : i : 90
|
||||
view([30, i]);
|
||||
|
||||
if options.writeVideo
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame);
|
||||
end
|
||||
|
||||
drawnow
|
||||
end
|
||||
|
||||
% changing perspective
|
||||
|
||||
|
||||
%% camera flying through video
|
||||
camzoom(0.8);
|
||||
for i = 1 : posesSize
|
||||
|
||||
hold on
|
||||
|
||||
campos([poses{i}.x, poses{i}.y, poses{i}.z]);
|
||||
camtarget([options.fieldSize.x/2, options.fieldSize.y/2, 0]);
|
||||
camlight(hlight, 'headlight');
|
||||
|
||||
if options.writeVideo
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame);
|
||||
end
|
||||
|
||||
drawnow
|
||||
end
|
||||
|
||||
%%close video
|
||||
if(options.writeVideo)
|
||||
close(videoObj);
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,113 @@
|
|||
function pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
|
||||
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
|
||||
% After creation of the factor graph for each track, linearize it around ground truth.
|
||||
% There is no optimization
|
||||
% @author: Zhaoyang Lv
|
||||
|
||||
import gtsam.*
|
||||
|
||||
%% create graph
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
%% create the noise factors
|
||||
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
|
||||
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
|
||||
measurementNoiseSigma = 1.0;
|
||||
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
|
||||
|
||||
cameraPosesNum = length(cameraPoses);
|
||||
|
||||
%% add measurements and initial camera & points values
|
||||
pointsNum = 0;
|
||||
cylinderNum = length(cylinders);
|
||||
points3d = cell(0);
|
||||
for i = 1:cylinderNum
|
||||
cylinderPointsNum = length(cylinders{i}.Points);
|
||||
pointsNum = pointsNum + cylinderPointsNum;
|
||||
for j = 1:cylinderPointsNum
|
||||
points3d{end+1}.data = cylinders{i}.Points{j};
|
||||
points3d{end}.Z = cell(0);
|
||||
points3d{end}.camConstraintIdx = cell(0);
|
||||
points3d{end}.added = cell(0);
|
||||
points3d{end}.visiblity = false;
|
||||
points3d{end}.cov = cell(cameraPosesNum);
|
||||
end
|
||||
end
|
||||
|
||||
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
|
||||
|
||||
%% initialize graph and values
|
||||
initialEstimate = Values;
|
||||
for i = 1:pointsNum
|
||||
point_j = points3d{i}.data.retract(0.1*randn(3,1));
|
||||
initialEstimate.insert(symbol('p', i), point_j);
|
||||
end
|
||||
|
||||
pts3d = cell(cameraPosesNum, 1);
|
||||
cameraPosesCov = cell(cameraPosesNum, 1);
|
||||
marginals = Values;
|
||||
for i = 1:cameraPosesNum
|
||||
cameraPose = cameraPoses{i};
|
||||
pts3d{i} = cylinderSampleProjection(K, cameraPose, imageSize, cylinders);
|
||||
|
||||
measurementNum = length(pts3d{i}.Z);
|
||||
for j = 1:measurementNum
|
||||
index = pts3d{i}.overallIdx{j};
|
||||
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
|
||||
points3d{index}.camConstraintIdx{end+1} = i;
|
||||
points3d{index}.added{end+1} = false;
|
||||
|
||||
if length(points3d{index}.Z) < 2
|
||||
continue;
|
||||
else
|
||||
for k = 1:length(points3d{index}.Z)
|
||||
if ~points3d{index}.added{k}
|
||||
graph.add(GenericProjectionFactorCal3_S2(points3d{index}.Z{k}, ...
|
||||
measurementNoise, symbol('x', points3d{index}.camConstraintIdx{k}), ...
|
||||
symbol('p', index), K) );
|
||||
points3d{index}.added{k} = true;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
points3d{index}.visiblity = true;
|
||||
end
|
||||
|
||||
pose_i = cameraPoses{i}.retract(0.1*randn(6,1));
|
||||
initialEstimate.insert(symbol('x', i), pose_i);
|
||||
|
||||
marginals = Marginals(graph, initialEstimate);
|
||||
|
||||
for j = 1:pointsNum
|
||||
if points3d{j}.visiblity
|
||||
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p',j));
|
||||
end
|
||||
end
|
||||
|
||||
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x',i));
|
||||
|
||||
end
|
||||
|
||||
%% Print the graph
|
||||
graph.print(sprintf('\nFactor graph:\n'));
|
||||
|
||||
%% Plot the result
|
||||
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
|
||||
|
||||
%% get all the points track information
|
||||
for i = 1:pointsNum
|
||||
if ~points3d{i}.visiblity
|
||||
continue;
|
||||
end
|
||||
|
||||
pts2dTracksMono.pt3d{end+1} = points3d{i}.data;
|
||||
pts2dTracksMono.Z{end+1} = points3d{i}.Z;
|
||||
|
||||
if length(points3d{i}.Z) == 1
|
||||
%pts2dTracksMono.cov{i} singular matrix
|
||||
else
|
||||
pts2dTracksMono.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
function [pts2dTracksStereo, initialEstimate] = points2DTrackStereo(K, cameraPoses, options, cylinders)
|
||||
% Assess how accurately we can reconstruct points from a particular monocular camera setup.
|
||||
% After creation of the factor graph for each track, linearize it around ground truth.
|
||||
% There is no optimization
|
||||
%
|
||||
% @author: Zhaoyang Lv
|
||||
|
||||
import gtsam.*
|
||||
|
||||
%% create graph
|
||||
graph = NonlinearFactorGraph;
|
||||
|
||||
%% create the noise factors
|
||||
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
|
||||
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
|
||||
stereoNoise = noiseModel.Isotropic.Sigma(3, 0.05);
|
||||
|
||||
cameraPosesNum = length(cameraPoses);
|
||||
|
||||
%% add measurements and initial camera & points values
|
||||
pointsNum = 0;
|
||||
cylinderNum = length(cylinders);
|
||||
points3d = cell(0);
|
||||
for i = 1:cylinderNum
|
||||
cylinderPointsNum = length(cylinders{i}.Points);
|
||||
pointsNum = pointsNum + length(cylinders{i}.Points);
|
||||
for j = 1:cylinderPointsNum
|
||||
points3d{end+1}.data = cylinders{i}.Points{j};
|
||||
points3d{end}.Z = cell(0);
|
||||
points3d{end}.cameraConstraint = cell(0);
|
||||
points3d{end}.visiblity = false;
|
||||
points3d{end}.cov = cell(cameraPosesNum);
|
||||
end
|
||||
end
|
||||
|
||||
graph.add(PriorFactorPose3(symbol('x', 1), cameraPoses{1}, posePriorNoise));
|
||||
|
||||
%% initialize graph and values
|
||||
initialEstimate = Values;
|
||||
for i = 1:pointsNum
|
||||
point_j = points3d{i}.data.retract(0.05*randn(3,1));
|
||||
initialEstimate.insert(symbol('p', i), point_j);
|
||||
end
|
||||
|
||||
pts3d = cell(cameraPosesNum, 1);
|
||||
cameraPosesCov = cell(cameraPosesNum, 1);
|
||||
for i = 1:cameraPosesNum
|
||||
pts3d{i} = cylinderSampleProjectionStereo(K, cameraPoses{i}, options.camera.resolution, cylinders);
|
||||
|
||||
if isempty(pts3d{i}.Z)
|
||||
continue;
|
||||
end
|
||||
|
||||
measurementNum = length(pts3d{i}.Z);
|
||||
for j = 1:measurementNum
|
||||
index = pts3d{i}.overallIdx{j};
|
||||
points3d{index}.Z{end+1} = pts3d{i}.Z{j};
|
||||
points3d{index}.cameraConstraint{end+1} = i;
|
||||
points3d{index}.visiblity = true;
|
||||
|
||||
graph.add(GenericStereoFactor3D(StereoPoint2(pts3d{i}.Z{j}.uL, pts3d{i}.Z{j}.uR, pts3d{i}.Z{j}.v), ...
|
||||
stereoNoise, symbol('x', i), symbol('p', index), K));
|
||||
end
|
||||
|
||||
pose_i = cameraPoses{i}.retract(poseNoiseSigmas);
|
||||
initialEstimate.insert(symbol('x', i), pose_i);
|
||||
|
||||
%% linearize the graph
|
||||
marginals = Marginals(graph, initialEstimate);
|
||||
|
||||
for j = 1:pointsNum
|
||||
if points3d{j}.visiblity
|
||||
points3d{j}.cov{i} = marginals.marginalCovariance(symbol('p', j));
|
||||
end
|
||||
end
|
||||
|
||||
cameraPosesCov{i} = marginals.marginalCovariance(symbol('x', i));
|
||||
end
|
||||
|
||||
%% Plot the result
|
||||
plotFlyingResults(points3d, cameraPoses, cameraPosesCov, cylinders, options);
|
||||
|
||||
%% get all the 2d points track information
|
||||
pts2dTracksStereo.pt3d = cell(0);
|
||||
pts2dTracksStereo.Z = cell(0);
|
||||
pts2dTracksStereo.cov = cell(0);
|
||||
for i = 1:pointsNum
|
||||
if ~points3d{i}.visiblity
|
||||
continue;
|
||||
end
|
||||
|
||||
pts2dTracksStereo.pt3d{end+1} = points3d{i}.data;
|
||||
pts2dTracksStereo.Z{end+1} = points3d{i}.Z;
|
||||
pts2dTracksStereo.cov{end+1} = marginals.marginalCovariance(symbol('p', i));
|
||||
end
|
||||
|
||||
%
|
||||
% %% plot the result with covariance ellipses
|
||||
% plotFlyingResults(pts2dTracksStereo.pt3d, pts2dTracksStereo.cov, cameraPoses, cameraPosesCov, cylinders, options);
|
||||
|
||||
end
|
||||
|
|
@ -0,0 +1,179 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 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 A camera flying example through a field of cylinder landmarks
|
||||
% @author Zhaoyang Lv
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
|
||||
|
||||
clear all;
|
||||
clc;
|
||||
clf;
|
||||
|
||||
import gtsam.*
|
||||
|
||||
% test or run
|
||||
options.enableTests = false;
|
||||
|
||||
%% cylinder options
|
||||
% the number of cylinders in the field
|
||||
options.cylinder.cylinderNum = 15; % pls be smaller than 20
|
||||
% cylinder size
|
||||
options.cylinder.radius = 3; % pls be smaller than 5
|
||||
options.cylinder.height = 10;
|
||||
% point density on cylinder
|
||||
options.cylinder.pointDensity = 0.1;
|
||||
|
||||
%% camera options
|
||||
% parameters set according to the stereo camera:
|
||||
% http://www.matrix-vision.com/USB2.0-single-board-camera-mvbluefox-mlc.html
|
||||
|
||||
% set up monocular camera or stereo camera
|
||||
options.camera.IS_MONO = false;
|
||||
% the field of view of camera
|
||||
options.camera.fov = 120;
|
||||
% fps for image
|
||||
options.camera.fps = 25;
|
||||
% camera pixel resolution
|
||||
options.camera.resolution = Point2(752, 480);
|
||||
% camera horizon
|
||||
options.camera.horizon = 60;
|
||||
% camera baseline
|
||||
options.camera.baseline = 0.05;
|
||||
% camera focal length
|
||||
options.camera.f = round(options.camera.resolution.x * options.camera.horizon / ...
|
||||
options.camera.fov);
|
||||
% camera focal baseline
|
||||
options.camera.fB = options.camera.f * options.camera.baseline;
|
||||
% camera disparity
|
||||
options.camera.disparity = options.camera.fB / options.camera.horizon;
|
||||
% Monocular Camera Calibration
|
||||
options.camera.monoK = Cal3_S2(options.camera.f, options.camera.f, 0, ...
|
||||
options.camera.resolution.x/2, options.camera.resolution.y/2);
|
||||
% Stereo Camera Calibration
|
||||
options.camera.stereoK = Cal3_S2Stereo(options.camera.f, options.camera.f, 0, ...
|
||||
options.camera.resolution.x/2, options.camera.resolution.y/2, options.camera.disparity);
|
||||
|
||||
% write video output
|
||||
options.writeVideo = true;
|
||||
% the testing field size (unit: meter)
|
||||
options.fieldSize = Point2([100, 100]');
|
||||
% camera flying speed (unit: meter / second)
|
||||
options.speed = 20;
|
||||
% camera flying height
|
||||
options.height = 30;
|
||||
|
||||
%% ploting options
|
||||
% display covariance scaling factor, default to be 1.
|
||||
% The covariance visualization default models 99% of all probablity
|
||||
options.plot.covarianceScale = 1;
|
||||
% plot the trajectory covariance
|
||||
options.plot.DISP_TRAJ_COV = true;
|
||||
% plot points covariance
|
||||
options.plot.POINTS_COV = true;
|
||||
|
||||
%% This is for tests
|
||||
if options.enableTests
|
||||
% test1: visibility test in monocular camera
|
||||
cylinders{1}.centroid = Point3(30, 50, 5);
|
||||
cylinders{2}.centroid = Point3(50, 50, 5);
|
||||
cylinders{3}.centroid = Point3(70, 50, 5);
|
||||
|
||||
for i = 1:3
|
||||
cylinders{i}.radius = 5;
|
||||
cylinders{i}.height = 10;
|
||||
|
||||
cylinders{i}.Points{1} = cylinders{i}.centroid.compose(Point3(-cylinders{i}.radius, 0, 0));
|
||||
cylinders{i}.Points{2} = cylinders{i}.centroid.compose(Point3(cylinders{i}.radius, 0, 0));
|
||||
end
|
||||
|
||||
camera = SimpleCamera.Lookat(Point3(10, 50, 10), ...
|
||||
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
|
||||
Point3([0,0,1]'), options.monoK);
|
||||
|
||||
pose = camera.pose;
|
||||
prjMonoResult = cylinderSampleProjection(options.camera.monoK, pose, ...
|
||||
options.camera.resolution, cylinders);
|
||||
|
||||
% test2: visibility test in stereo camera
|
||||
prjStereoResult = cylinderSampleProjectionStereo(options.camera.stereoK, ...
|
||||
pose, options.camera.resolution, cylinders);
|
||||
end
|
||||
|
||||
%% generate a set of cylinders and point samples on cylinders
|
||||
cylinderNum = options.cylinder.cylinderNum;
|
||||
cylinders = cell(cylinderNum, 1);
|
||||
baseCentroid = cell(cylinderNum, 1);
|
||||
theta = 0;
|
||||
i = 1;
|
||||
while i <= cylinderNum
|
||||
theta = theta + 2*pi/10;
|
||||
x = 40 * rand * cos(theta) + options.fieldSize.x/2;
|
||||
y = 20 * rand * sin(theta) + options.fieldSize.y/2;
|
||||
baseCentroid{i} = Point2([x, y]');
|
||||
|
||||
% prevent two cylinders interact with each other
|
||||
regenerate = false;
|
||||
for j = 1:i-1
|
||||
if i > 1 && baseCentroid{i}.dist(baseCentroid{j}) < options.cylinder.radius * 2
|
||||
regenerate = true;
|
||||
break;
|
||||
end
|
||||
end
|
||||
if regenerate
|
||||
continue;
|
||||
end
|
||||
|
||||
cylinders{i,1} = cylinderSampling(baseCentroid{i}, options.cylinder.radius, ...
|
||||
options.cylinder.height, options.cylinder.pointDensity);
|
||||
i = i+1;
|
||||
end
|
||||
|
||||
%% generate ground truth camera trajectories: a line
|
||||
KMono = Cal3_S2(525,525,0,320,240);
|
||||
cameraPoses = cell(0);
|
||||
theta = 0;
|
||||
t = Point3(5, 5, options.height);
|
||||
i = 0;
|
||||
while 1
|
||||
i = i+1;
|
||||
distance = options.speed / options.camera.fps;
|
||||
angle = 0.1*pi*(rand-0.5);
|
||||
inc_t = Point3(distance * cos(angle), ...
|
||||
distance * sin(angle), 0);
|
||||
t = t.compose(inc_t);
|
||||
|
||||
if t.x > options.fieldSize.x - 10 || t.y > options.fieldSize.y - 10;
|
||||
break;
|
||||
end
|
||||
|
||||
%t = Point3([(i-1)*(options.fieldSize.x - 10)/options.poseNum + 10, ...
|
||||
% 15, 10]');
|
||||
camera = SimpleCamera.Lookat(t, ...
|
||||
Point3(options.fieldSize.x/2, options.fieldSize.y/2, 0), ...
|
||||
Point3([0,0,1]'), options.camera.monoK);
|
||||
cameraPoses{end+1} = camera.pose;
|
||||
end
|
||||
|
||||
%% set up camera and get measurements
|
||||
if options.camera.IS_MONO
|
||||
% use Monocular Camera
|
||||
pts2dTracksMono = points2DTrackMonocular(options.camera.monoK, cameraPoses, ...
|
||||
options.camera.resolution, cylinders);
|
||||
else
|
||||
% use Stereo Camera
|
||||
pts2dTracksStereo = points2DTrackStereo(options.camera.stereoK, ...
|
||||
cameraPoses, options, cylinders);
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
% test Cal3Unified
|
||||
import gtsam.*;
|
||||
|
||||
K = Cal3Unified;
|
||||
EXPECT('fx',K.fx()==1);
|
||||
EXPECT('fy',K.fy()==1);
|
||||
|
||||
|
|
@ -1,17 +1,25 @@
|
|||
% Test runner script - runs each test
|
||||
|
||||
% display 'Starting: testPriorFactor'
|
||||
% testPriorFactor
|
||||
%% geometry
|
||||
display 'Starting: testCal3Unified'
|
||||
testCal3Unified
|
||||
|
||||
display 'Starting: testValues'
|
||||
testValues
|
||||
%% linear
|
||||
display 'Starting: testKalmanFilter'
|
||||
testKalmanFilter
|
||||
|
||||
display 'Starting: testJacobianFactor'
|
||||
testJacobianFactor
|
||||
|
||||
display 'Starting: testKalmanFilter'
|
||||
testKalmanFilter
|
||||
%% nonlinear
|
||||
display 'Starting: testValues'
|
||||
testValues
|
||||
|
||||
%% SLAM
|
||||
display 'Starting: testPriorFactor'
|
||||
testPriorFactor
|
||||
|
||||
%% examples
|
||||
display 'Starting: testLocalizationExample'
|
||||
testLocalizationExample
|
||||
|
||||
|
|
@ -36,6 +44,7 @@ testStereoVOExample
|
|||
display 'Starting: testVisualISAMExample'
|
||||
testVisualISAMExample
|
||||
|
||||
%% MATLAB specific
|
||||
display 'Starting: testUtilities'
|
||||
testUtilities
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1,2 @@
|
|||
*.m~
|
||||
*.avi
|
||||
|
|
|
|||
Loading…
Reference in New Issue