diff --git a/matlab/gtsam_examples/CameraFlyingExample.m b/matlab/gtsam_examples/CameraFlyingExample.m new file mode 100644 index 000000000..018ffbb4a --- /dev/null +++ b/matlab/gtsam_examples/CameraFlyingExample.m @@ -0,0 +1,28 @@ +clear all; +clc; +import gtsam.* + +cylinder_num = 10; +cylinders = cell(cylinder_num, 1); + +% generate a set of cylinders +for i = 1:cylinder_num + cylinder_center = Point2([10, 5 * i]'); + cylinders{i,1} = cylinderSampling(cylinder_center, 1, 5, 30); +end + +% visibility validation +%camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift); + +K = Cal3_S2(525,525,0,320,240); +cam_pose = Pose3(); +cam = SimpleCamera(cam_pose, K); + +% the projections of all visible 3D points +visiblePoints3 = cylinderSampleProjection(cam, cam_pose, cylinders); + +% + +% + + diff --git a/matlab/gtsam_examples/cylinderSampleProjection.m b/matlab/gtsam_examples/cylinderSampleProjection.m new file mode 100644 index 000000000..30b7d97c1 --- /dev/null +++ b/matlab/gtsam_examples/cylinderSampleProjection.m @@ -0,0 +1,18 @@ +function [] = cylinderSampleProjection(Cam, Pose3, cylinders) + + cylinder_num = size(cylinders, 1); + for i = 1:cylinder_num + cylinder = cylinders{i}; + + point_num = size(cylinder.Points, 1); + % to be finished + + % for j = 1:point_num +% +% cylinderPoints = +% +% end + + end + +end \ No newline at end of file diff --git a/matlab/gtsam_examples/cylinderSampling.m b/matlab/gtsam_examples/cylinderSampling.m new file mode 100644 index 000000000..3e6a6d9a8 --- /dev/null +++ b/matlab/gtsam_examples/cylinderSampling.m @@ -0,0 +1,22 @@ +function [cylinder] = cylinderSampling(Point2, radius, height, density) +% + import gtsam.* + % calculate the cylinder area + A = 2 * pi * radius * height; + + PointsNum = round(A * density); + + Points3 = cell(PointsNum, 1); + + % sample the points + for i = 1:PointsNum + theta = 2 * pi * rand; + x = radius * cos(theta) + Point2.x; + y = radius * sin(theta) + Point2.y; + z = height * rand; + Points3{i,1} = Point3([x,y,z]'); + end + + cylinder.area = A; + cylinder.Points = Points3; +end \ No newline at end of file