diff --git a/gtsam.h b/gtsam.h index 1fbc0f907..c5528309e 100644 --- a/gtsam.h +++ b/gtsam.h @@ -629,28 +629,13 @@ class Cal3_S2 { void serialize() const; }; -#include -class Cal3DS2 { +#include +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 +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 +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 class Cal3_S2Stereo { // Standard Constructors Cal3_S2Stereo(); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 0cb914ce3..6ae8ec14b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -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 clone() const { + return boost::shared_ptr(new Cal3DS2(*this)); + } + + /// @} + private: - /// @} /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 37c156743..d4f4cabe5 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -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 clone() const { + return boost::shared_ptr(new Cal3DS2_Base(*this)); + } /// @} + +private: + /// @name Advanced Interface /// @{ diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 624d7dbb4..2127fb200 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -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 */ diff --git a/matlab/+gtsam/covarianceEllipse.m b/matlab/+gtsam/covarianceEllipse.m index cdc239bb2..829487dc6 100644 --- a/matlab/+gtsam/covarianceEllipse.m +++ b/matlab/+gtsam/covarianceEllipse.m @@ -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 \ No newline at end of file +end diff --git a/matlab/+gtsam/covarianceEllipse3D.m b/matlab/+gtsam/covarianceEllipse3D.m index 9682a9fc1..4364e0fe4 100644 --- a/matlab/+gtsam/covarianceEllipse3D.m +++ b/matlab/+gtsam/covarianceEllipse3D.m @@ -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); diff --git a/matlab/+gtsam/cylinderSampleProjection.m b/matlab/+gtsam/cylinderSampleProjection.m new file mode 100644 index 000000000..0abd9cc3c --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjection.m @@ -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 diff --git a/matlab/+gtsam/cylinderSampleProjectionStereo.m b/matlab/+gtsam/cylinderSampleProjectionStereo.m new file mode 100644 index 000000000..6512231e8 --- /dev/null +++ b/matlab/+gtsam/cylinderSampleProjectionStereo.m @@ -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 diff --git a/matlab/+gtsam/cylinderSampling.m b/matlab/+gtsam/cylinderSampling.m new file mode 100644 index 000000000..dcaa9c721 --- /dev/null +++ b/matlab/+gtsam/cylinderSampling.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/plotCamera.m b/matlab/+gtsam/plotCamera.m index ba352b757..d0d1f45b7 100644 --- a/matlab/+gtsam/plotCamera.m +++ b/matlab/+gtsam/plotCamera.m @@ -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 \ No newline at end of file +end diff --git a/matlab/+gtsam/plotCylinderSamples.m b/matlab/+gtsam/plotCylinderSamples.m new file mode 100644 index 000000000..ec1795dea --- /dev/null +++ b/matlab/+gtsam/plotCylinderSamples.m @@ -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 diff --git a/matlab/+gtsam/plotFlyingResults.m b/matlab/+gtsam/plotFlyingResults.m new file mode 100644 index 000000000..5d4a287c4 --- /dev/null +++ b/matlab/+gtsam/plotFlyingResults.m @@ -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 \ No newline at end of file diff --git a/matlab/+gtsam/points2DTrackMonocular.m b/matlab/+gtsam/points2DTrackMonocular.m new file mode 100644 index 000000000..fc48f587e --- /dev/null +++ b/matlab/+gtsam/points2DTrackMonocular.m @@ -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 diff --git a/matlab/+gtsam/points2DTrackStereo.m b/matlab/+gtsam/points2DTrackStereo.m new file mode 100644 index 000000000..60c9f1295 --- /dev/null +++ b/matlab/+gtsam/points2DTrackStereo.m @@ -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 diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m new file mode 100644 index 000000000..36b7635e2 --- /dev/null +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -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 + + + + + + + diff --git a/matlab/gtsam_tests/testCal3Unified.m b/matlab/gtsam_tests/testCal3Unified.m new file mode 100644 index 000000000..498c65343 --- /dev/null +++ b/matlab/gtsam_tests/testCal3Unified.m @@ -0,0 +1,7 @@ +% test Cal3Unified +import gtsam.*; + +K = Cal3Unified; +EXPECT('fx',K.fx()==1); +EXPECT('fy',K.fy()==1); + diff --git a/matlab/gtsam_tests/test_gtsam.m b/matlab/gtsam_tests/test_gtsam.m index 1a6856a9a..4cbe42364 100644 --- a/matlab/gtsam_tests/test_gtsam.m +++ b/matlab/gtsam_tests/test_gtsam.m @@ -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 diff --git a/matlab/unstable_examples/.gitignore b/matlab/unstable_examples/.gitignore index 6d725d9bc..c47168f67 100644 --- a/matlab/unstable_examples/.gitignore +++ b/matlab/unstable_examples/.gitignore @@ -1 +1,2 @@ *.m~ +*.avi