12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

function3 add graph measurement and initial estimate

release/4.3a0
lvzhaoyang 2015-01-12 16:10:49 -05:00
parent b45e81725b
commit a8bf2a4da1
6 changed files with 131 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
matlab/+gtsam/cylinderSampleProjection.m
View File

@ -1,29 +1,46 @@
function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders)
% Project sampled points on cylinder to camera frame
% Authors: Zhaoyang Lv
function [visiblePoints] = cylinderSampleProjection(camera, 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.*
cylinder_num = size(cylinders, 1);
%% memory allocation
cylinderNum = length(cylinders);
visiblePoints.index = cell(cylinderNum);
%camera = SimpleCamera(cameraPose, K);
camera = SimpleCamera.Lookat(cameraPose.translation(), cylinders{10}.centroid, Point3([0,0,1]'), K);
pointCloudNum = 0;
for i = 1:cylinderNum
pointCloudNum = pointCloudNum + length(cylinders{i}.Points);
visiblePoints.index{i} = cell(pointCloudNum);
end
visiblePoints.data = cell(pointCloudNum);
visiblePoints = {};
visiblePointsCylinderIdx = [];
%% check visiblity of points on each cylinder
pointCloudIndex = 0;
for i = 1:cylinderNum
for i = 1:cylinder_num
point_num = size( cylinders{i}.Points, 1);
pointNum = length(cylinders{i}.Points);
% to check point visibility
for j = 1:point_num
for j = 1:pointNum
pointCloudIndex = pointCloudIndex + 1;
sampledPoint3 = cylinders{i}.Points{j};
measurements2d = camera.project(sampledPoint3);
Z2d = camera.project(sampledPoint3);
% ignore points not visible in the scene
if measurements2d.x < 0 || measurements2d.x >= imageSize.x ...
|| measurements2d.y < 0 || measurements2d.y >= imageSize.y
if Z2d.x < 0 || Z2d.x >= imageSize.x ...
|| Z2d.y < 0 || Z2d.y >= imageSize.y
continue;
end
@ -31,7 +48,7 @@ function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K,
% 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
for k = 1:cylinder_num
for k = 1:cylinderNum
rayCameraToPoint = cameraPose.translation().between(sampledPoint3).vector();
rayCameraToCylinder = cameraPose.translation().between(cylinders{i}.centroid).vector();
@ -40,8 +57,9 @@ function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K,
% Condition 1: all points in front of the cylinders'
% surfaces are visible
if dot(rayCylinderToPoint, rayCameraToCylinder) < 0
visiblePoints{end+1} = sampledPoint3;
visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i];
visiblePoints.data{pointCloudIndex} = sampledPoint3;
visiblePoints.Z{pointCloudIndex} = Z2d;
visiblePoints.index{i}{j} = pointCloudIndex;
continue;
end
@ -51,8 +69,9 @@ function [visiblePoints, visiblePointsCylinderIdx] = cylinderSampleProjection(K,
rayCylinderToProjected = norm(projectedRay) / norm(rayCameraToPoint) * rayCameraToPoint;
if rayCylinderToProjected(1) > cylinders{i}.radius && ...
rayCylinderToProjected(2) > cylinders{i}.radius
visiblePoints{end+1} = sampledPoint3;
visiblePointsCylinderIdx = [visiblePointsCylinderIdx, i];
visiblePoints.data{pointCloudIndex} = sampledPoints3;
visiblePoints.Z{pointCloudIndex} = Z2d;
visiblePoints.index{i}{j} = pointCloudIndex;
end
end

53
matlab/+gtsam/points2DTrackMonocular.m
View File

@ -1,4 +1,4 @@
function pts2dTracksmon = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders)
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
@ -9,32 +9,59 @@ import gtsam.*
%% create graph
graph = NonlinearFactorGraph;
%% add a constraint on the starting pose
pointNoiseSigma = 0.1;
poseNoiseSigmas = [0.001 0.001 0.001 0.1 0.1 0.1]';
%% add a constraint on the starting pose
posePriorNoise = noiseModel.Diagonal.Sigmas(poseNoiseSigmas);
firstPose = cameraPoses{1};
graph.add(PriorFactorPose3(symbol('x', l), firstPose, posePriorNoise));
cameraPosesNum = size(cameraPoses, 1);
cameraPosesNum = length(cameraPoses);
%% add measurements
%% add measurements and initial camera & points values
pointsNum = 0;
cylinderNum = length(cylinders);
for i = 1:cylinderNum
pointsNum = pointsNum + length(cylinders{i}.Points);
end
measurementNoise = noiseModel.Isotropic.Sigma(2, measurementNoiseSigma);
pts3d = {};
initialEstimate = Values;
for i = 1:cameraPosesNum
[visiblePoints3, visiblePointsCylinderIdx] = cylinderSampleProjection(K, cameraPoses{i}, imageSize, cylinders);
camera = SimpleCamera(K, cameraPoses{i});
pointsNum = size(visiblePoints, 1);
pts3d.pts{i} = cylinderSampleProjection(camera, imageSize, cylinders);
pts3d.camera{i} = camera;
%% not finished
%for j = 1:pointsNum
% graph.add();
%end
for j = 1:length(pts3d.pts{i}.Z)
graph.add(GenericProjectionFactorCal3_S2(pts3d.pts{i}.Z{j}, ...
measurementNoise, symbol('x', i), symbol('p', j), camera.K) );
point_j = pts3d.pts{i}.data{j}.retract(0.1*randn(3,1));
initialEstimate.insert(symbol('p', j), point_j);
end
pose_i = camera.pose.retract(0.1*randn(6,1));
initialEstimate.insert(symbole('x', i), pose_i);
end
%% Print the graph
graph.print(sprintf('\nFactor graph:\n'));
marginals = Marginals(graph, initialEstimate);
% should use all the points num to replace the num 100
for i = 1:100
marginals.marginalCovariance(symbol('p',i));
%% get all the 2d points track information
ptIdx = 0;
for i = 1:pointsNum
if isempty(pts3d.pts{i})
continue;
end
%pts2dTrackMono.pts2d = pts3d.pts{i}
pts2dTracksMono.cov{ptIdx} = marginals.marginalCovariance(symbol('p',i));
end
end

36
matlab/gtsam_examples/CameraFlyingExample.m
View File

@ -2,47 +2,49 @@ clear all;
clc;
import gtsam.*
%% generate a set of cylinders and Samples
fieldSize = Point2([100, 100]');
cylinder_num = 10;
cylinders = cell(cylinder_num, 1);
% generate a set of cylinders
fieldSize = Point2([100, 100]');
% random generate cylinders on the fields
for i = 1:cylinder_num
for i = 1:cylinderNum
baseCentroid = Point2([fieldSize.x * rand, fieldSize.y * rand]');
cylinders{i,1} = cylinderSampling(baseCentroid, 1, 5, 1);
end
% plot all the cylinders and sampled points
%% plot all the cylinders and sampled points
% now is plotting on a 100 * 100 field
figID = 1;
figure(figID);
plotCylinderSamples(cylinders, fieldSize, figID);
% visibility validation
% generate camera trajectories
%% generate camera trajectories
K = Cal3_S2(525,525,0,320,240);
imageSize = Point2([640, 480]');
poseNum = 10;
cameraPoses = cell(poseNum, 1);
cameraPoses{1} = Pose3();
cameras = cell(poseNum, 1);
for i = 2:poseNum
incRot = Rot3.RzRyRx(0,0,pi/4);
trans = Point3();
% To ensure there are landmarks in view, look at one randomly chosen cylinder
% each time.
for i = 1:poseNum
camera = SimpleCamera.Lookat(trans, cylinders{round(cylinderNum*rand)}.centroid, ...
Point3([0,0,1]'), K);
incT = Point3(5*rand, 5*rand, 5*rand);
cameraPoses{i} = cameraPoses{i-1}.compose(Pose3(incRot, incT));
trans = trans.compose(incT);
end
[visiblePoints3, ~] = cylinderSampleProjection(K, cameraPoses{1}, imageSize, cylinders);
%% visibility validation
visiblePoints3 = cylinderSampleProjection(camera, imageSize, cylinders);
plotPose3(cameraPoses{1}, 5 )
% plot all the projected points
plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
%% plot all the projected points
%plotProjectedCylinderSamples(visiblePoints3, cameraPoses{1}, figID);
%% setp up monocular camera and get measurements
pts2dTracksMono = points2DTrackMonocular(K, cameraPoses, imageSize, cylinders);
%% set up stereo camera and get measurements
%pts2dTracksStereo = points2DTrackStereo(K, cameraPoses, imageSize, cylinders);

41
matlab/gtsam_examples/cylinderSampleProjection.m
View File

@ -1,41 +0,0 @@
function [] = cylinderSampleProjection(K, cameraPose, imageSize, cylinders)
% Project sampled points on cylinder to camera frame
% Authors: Zhaoyang Lv
cylinder_num = size(cylinders, 1);
camera = SimpleCamera(cameraPose, K);
for i = 1:cylinder_num
point_num = size( cylinders{i}.Points, 1);
% to check point visibility
for j = 1:point_num
sampledPoint3 = cylinders{i}.Poinsts{j};
measurements2d = camera.project(sampledPoint3);
% ignore points not visible in the scene
if measurements2d.x < 0 || measurements.x >= imageSize.x ...
|| measurements2d.y < 0 || measurements.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
for k = 1:cylinder_num
rayCameraToPoint = sampledPoint3 - cameraPose.t;
rayCameraToCylinder = cylinders{i} - cameraPose.t;
projectedRay = dot(rayCameraToPoint, rayCameraToCylinder);
distCameraToCylinder = norm(rayCameraToCylinder);
end
end
end
end

25
matlab/gtsam_examples/cylinderSampling.m
View File

@ -1,25 +0,0 @@
function [cylinder] = cylinderSampling(baseCentroid, radius, height, density)
%
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
matlab/unstable_examples/.gitignore vendored
View File

@ -1 +1,2 @@
*.m~
*.avi