CloudRunner

... share your algorithms as a web service

Algorithm: Silhouette Measurements for Bayesian Object Tracking in Noisy Point Clouds

Description:
Prototype implementation of the approach, presented in the paper.
Paper: Faion, F., Baum, M., & Hanebeck, U. D. (2013). Silhouette Measurements for Bayesian Object Tracking in Noisy Point Clouds. Fusion 2013 (p. submitted). Istanbul, Turkey.
Tags: Bayesian Silhouettes Point Cloud
Depends On: "Object Tools" by florian.faion (latest version)
Usage: Algorithm is public.
Viewed 3585 times, called 151 times
Upload:
Empty star Empty star Empty star Empty star Empty star
0 votes
Florian Faion
07/01/2013 5:42 p.m. (version #4)

Select Version

Version #4: 07/01/2013 5:42 p.m.
No changelog available

Version #3: 03/14/2013 1:39 p.m.
No changelog available

Run Algorithm

:
number of cameras that measure silhouettes (blue)
:
number of depth sensors that measure point clouds (red)
:
time steps
:
circular movement of object in laps/12, i.e., 6 is a half circle, 12 is a full circle
:
scaling of object
: plot point cloud measurements
: plot silhouette measurements
: Allow cached result?
Close

Please Wait

Computation is running...

Plots

showing plot # of

Result Value(s)


		

Resource Usage

Execution of the algorithm took seconds.
Peak memory usage for running the algorithm was kilobytes (total usage: kilobytes).

Run ID / Link

ID of this run:
Link:

Result from Cache

Result has been delivered from cache and computed on (UTC).

Matlab log

(show)

		

Error

Using this algorithm in your local MATLAB environment is easy: Click here for instructions!

Usage Instructions for CloudRunner Client

  1. Download the CloudRunner Client by clicking here and place the downloaded file in your MATLAB working directory.

  2. Inside MATLAB, initialize the CloudRunner Client by calling CloudRunner:
    >> CloudRunner

    A login dialog will prompt for your CloudRunner mail address and password. For a start, you can leave the dialog empty and just click "Connect".

    Alternatively, you can provide the login credentials (or empty strings to skip login) as a parameter and hence skip the login dialog. This is useful when using CloudRunner in non-interactive scripts.
    >> CloudRunner('mail@example.com', 'password')

  3. Select this algorithm by its URL. Selecting an algorithm creates the lambda function that proxies calls to the algorithm to the server for execution:
    >> CloudRunnerSelect('http://www.cloudrunner.eu/algorithm/120/silhouette-measurements-for-bayesian-object-tracking-in-noisy-point-clouds/version/4/')

    For the sake of convenience, you can also use the algorithm ID instead of its URL for public algorithms.

  4. Call functions from the algorithm like any regular local function.

Note: You can find further information on the help page.

Source Code

File:

 1 function c = pEll2cEll(p)
 2 %% Convert ellipse in parameter representation p in covariance representation c 
 3 % Column vectors
 4 % p = [cx cy a b phi]'
 5 % c = [cx cy Cx Cy Cxy]'
 6 % R = [cos(p(5)) -sin(p(5))
 7 %      sin(p(5))  cos(p(5))];
 8 %         
 9 % C = R*(diag([p(3),p(4)]').^2)*R';
10 % 
11 % c = [p(1) p(2) C(1) C(4) C(2)]';
12 R11 = cos(p(5,:));
13 R12 = -sin(p(5,:));
14 
15 c(1:2,:) = p(1:2,:);
16 
17 p(3:4,:) = p(3:4,:).^2;
18 
19 c(3:5,:) = [(R11.^2).*p(3,:)+(R12.^2).*p(4,:)
20             (R11.^2).*p(4,:)+(R12.^2).*p(3,:)
21              -R11.*R12.*p(3,:)+R11.*R12.*p(4,:)];
22 end
  1 function Silhouette_Demonstration(nr_cameras, nr_depthsensors, steps, lap, scaling, plotPointCloud, plotSilhouette)
  2  % Implementation of Silhouette Measurements according to 
  3 % 
  4 % Florian Faion, Marcus Baum, Uwe D. Hanebeck,
  5 % Silhouette Measurements for Bayesian Object Tracking in Noisy Point Clouds,
  6 % Submitted to Fusion 2013, 2012.
  7 % 
  8 % Note that the implementation is not optimized for speed
  9 % 
 10 % AUTHOR: Florian Faion, 2013
 11 
 12 %% Load defaults
 13 rng('shuffle');
 14 if nargin==0
 15     nr_cameras = 1;
 16     nr_depthsensors = 1;
 17     steps = 300;
 18     lap = 12;
 19     scaling = 1;
 20     plotPointCloud = false;
 21     plotSilhouette = false;
 22 end
 23 
 24 %% Prepare Plotting
 25 close all
 26 hold on; axis equal
 27 xlabel('x in m');
 28 ylabel('y in m');
 29 hS = []; hP = [];
 30 
 31 %% Setup depth sensors
 32 % Centers
 33 C = [4  4 -4 -4
 34      4 -4  4 -4];
 35 % View Points  
 36 V = [0 0 0 0
 37      0 0 0 0];
 38 % initialize nr_depthsensors sensors
 39 for k = 1:nr_depthsensors
 40     % Calculate rotation matrix R
 41     R = v2R2(V(:,k)-C(:,k));
 42     % initialize Kinect2D object
 43     kinect(k) = Kinect2D();
 44     % compose homogeneous transformation matrix H
 45     H = rc2h(R,C(:,k));
 46     % set H as extrinsics
 47     kinect(k).setExtrinsics(H);
 48     % plot depthsensor in red
 49     kinect(k).color = 'r';
 50     kinect(k).plotView();
 51     set(kinect(k).h,'LineWidth',2);
 52 end
 53 
 54 clear C V R H;
 55 %% Setup cameras
 56 % Centers
 57 C = [-4  0 4 0
 58       0 -4 0 4];
 59 % View Points  
 60 V = [0 0 0 0
 61      0 0 0 0];
 62 % initialize nr_cameras cameras
 63 for k = 1:nr_cameras
 64     % Calculate rotation matrix R
 65     R = v2R2(V(:,k)-C(:,k));
 66     % initialize Camera2D object
 67     camera(k) = Camera2D();
 68     % compose homogeneous transformation matrix H
 69     H = rc2h(R,C(:,k));
 70     % set H as extrinsics
 71     camera(k).setExtrinsics(H);
 72     % plot cameras in blue
 73     camera(k).color = 'b';
 74     camera(k).plotView();
 75     set(camera(k).h,'LineWidth',2);
 76 end
 77 
 78 %% Initialize groundtruth ellipse
 79 ellipse = Ellipsoid2D();
 80 % ellipse state is [phi pos_x pos_y axis_a axis_b]';
 81 ellipse.setState([0 0 2 scaling*[.2 .1]]');
 82 % plot ellipsoid in black
 83 ellipse.color = 'k';
 84 ellipse.plot();
 85 set(ellipse.h,'LineWidth',2);
 86 %% Ellipsoid performs circular movement with parameters dphi and dpos
 87 % ellipsoid velocity of angle/timestep
 88 dphi = lap/12*(-2*pi)/steps;
 89 % ellipsoid distance/timestep
 90 dpos = lap/12*(4*pi)/steps;
 91 
 92 %% Initialize Estimator
 93 Filter = GF(); % Gauss Filter
 94 Filter.disableChecks();
 95 ellipse_estimation = Ellipsoid2D(); % Ellipse for plotting
 96 ellipse_estimation.color = 'g'; % plot in green
 97 % System model: random walk
 98 sysModel  = IdentitySystemModel();
 99 % Setup noise
100 predNoiseMean = zeros(5, 1);
101 % Covariance forces the shape to be less dynamic
102 predNoiseCov  = blkdiag(.005,...
103                         .002*eye(2),...
104                         .00001*eye(2));
105 % Measurement model
106 % 1. point cloud update
107 measModelPointCloud = EllipseMeasModelPointCloud();
108 % 2. silhouette update
109 measModelSilhouette = EllipseMeasModelSilhouette();
110 
111 %% Begin Estimation
112 for step = 1:steps
113     %% move groundtruth ellipse
114     ellipse.move_circular(dphi,dpos);
115     % plot updated groundtruth ellipse
116     ellipse.plot('update');
117     
118     %% Measure groundtruth ellipse with depth sensors
119     for k =  1:nr_depthsensors
120         % Noisy points: PointCloud.V; Uncertainty covariance: PointCloud.C
121         [PointCloud(k).V, PointCloud(k).C]= kinect(k).measurePoints(kinect(k).unprojectGlobalPixels(kinect(k).visiblePixels(ellipse.measureByCam(kinect(k)))));
122     end
123     %% Prepare point cloud for filter
124     % Combine point cloud from all sensors
125     measNoiseMeanPointCloud = cat(2,PointCloud(:).V);
126     % Combine point cloud covariances from all sensors
127     measNoiseCovPointCloud  = cat(3,PointCloud(:).C);
128     nP = size(measNoiseMeanPointCloud,2);
129     % reduce points to maxPCsize
130     maxPCsize = 20;
131     if nP > maxPCsize
132         n = round(linspace(1,nP,maxPCsize));
133         measNoiseMeanPointCloud = measNoiseMeanPointCloud(:,n);
134         measNoiseCovPointCloud = measNoiseCovPointCloud(:,:,n);
135     end
136     if nP ~= 0 
137     % measurement noise mean
138     measNoiseMeanPointCloud = reshape(measNoiseMeanPointCloud,[],1);
139     % measurement noise covariance
140     measNoiseCovPointCloud = num2cell(measNoiseCovPointCloud,[1,2]);
141     measNoiseCovPointCloud = blkdiag(measNoiseCovPointCloud{:});
142     % pseudo measurement: distance 0
143     measurementDistance = zeros(numel(measNoiseMeanPointCloud)/2,1); 
144     end
145     
146     %% Measure groundtruth ellipse with cameras
147     for k =  1:nr_cameras
148         % Silhouette.I: Left and right silhouette edge; Silhouette.valid:
149         % Found valid edges?; Silhouette.D: Depth of the edge (for plotting only)
150         [Silhouette(k).I Silhouette(k).valid Silhouette(k).D] = edgeFromSilhouette(ellipse.measureByCam(camera(k)));
151     end
152     
153     %% Prepare edges for filter
154     nS = sum([Silhouette(:).valid]); % count valid silhouettes
155     if nS ~= 0 
156     % Combine all valid silhouettes
157     measurementSilhouette = reshape([Silhouette([Silhouette(:).valid]).I],[],1);
158     % zero-mean measurement noise
159     measNoiseMeanSilhouette = zeros(numel(measurementSilhouette),1);
160     % Variance of silhouette edges
161     Var = 2;
162     measNoiseCovSilhouette = eye(numel(measNoiseMeanSilhouette));
163     measurementSilhouette = mvnrnd(measurementSilhouette',measNoiseCovSilhouette)';
164     end
165     
166     %% Initialize filter from first measurement (only in 1st step)
167     if step == 1
168         % initialize as circle at mean of first point cloud
169         x0 = [0  mean(cat(2,PointCloud(:).V),2)' .3 .3]';
170         C0 = .1*eye(5);
171         % Set state
172         Filter.setState(x0, C0)
173         ellipse_estimation.setState(Filter.getPointEstimation());
174         ellipse_estimation.plot('update');
175         set(ellipse_estimation.h,'LineWidth',2);   
176         drawnow;        
177     end
178     
179     %% Prediction step
180     Filter.predict(sysModel,predNoiseMean,predNoiseCov); 
181     
182     %% Measurement update
183     % for point cloud
184     if nP ~= 0 
185     Filter.update(measModelPointCloud,measurementDistance,measNoiseMeanPointCloud,measNoiseCovPointCloud); 
186     end
187     % for silhouette
188     if nS ~= 0 
189     Filter.update(measModelSilhouette,measurementSilhouette,measNoiseMeanSilhouette,measNoiseCovSilhouette,camera([Silhouette(:).valid]));
190     end
191     
192     %% Plotting
193     % write estimation to ellipse object
194     ellipse_estimation.setState(Filter.getPointEstimation());
195     ellipse_estimation.plot('update');
196     set(ellipse_estimation.h,'LineWidth',2);
197     
198     if plotPointCloud
199         delete(hP);
200         hP = plotP(gca,reshape(measNoiseMeanPointCloud,2,[]),'r');
201     end
202     
203     if plotSilhouette
204         delete(hS);
205         hS = [];
206         for k = 1:sum([Silhouette(:).valid])
207             id = find([Silhouette(:).valid]);
208             [~,C] = h2rc(camera(k).getExtrinsics);
209             I = camera(k).unprojectGlobalPixels([measurementSilhouette(2*(k-1)+[1 2])'; Silhouette(id(k)).D+1]);
210             hS = [hS plot([C(1) I(1,1)],[C(2) I(2,1)],'b--',[C(1) I(1,2)],[C(2) I(2,2)],'b--')];
211         end
212     end 
213     
214     
215     drawnow;
216     
217 end
218 
219 
220 end
221 
222 function [I valid D] = edgeFromSilhouette(F)
223 Imin = find(F(2,:),1,'first');
224 Imax = find(F(2,:),1,'last'); 
225 I = [Imin Imax]; 
226     if isempty(Imin)
227         I = [0 0]; valid = false;
228     else
229         D = F(2,I); valid = true;
230     end
231 end
 1 classdef EllipseMeasModelSilhouette < GenerativeMeasurementModel
 2     methods
 3         function measurement = measurementEquation(~, state, noise, camera)
 4             n_approx = 32;
 5             V_ = ell2polygon(state,n_approx);   
 6             n_cam = numel(camera);
 7             measurement = nan(2*n_cam,size(noise,2));
 8             for k = 1:n_cam    
 9             H = inv(camera(k).getExtrinsics); 
10             % Extrinsics der Kamera
11             V = nan(size(V_));
12             V(1:2:end,:) = V_(1:2:end,:).*H(1,1)+V_(2:2:end,:).*H(1,2) + H(1,3);
13             V(2:2:end,:) = V_(1:2:end,:).*H(2,1)+V_(2:2:end,:).*H(2,2) + H(2,3);
14 
15             % Projektion der Kamera
16             K = camera(k).getIntrinsics;
17             px = (V(1:2:end,:).* K(1,1) + V(2:2:end,:).* K(1,2))./V(2:2:end,:);
18             Constraint = clamp([min(px) 
19                                 max(px)],[1 camera(k).getResolution]);
20              
21             meas = noise(2*(k-1)+[1 2],:);
22             measurement(2*(k-1)+[1 2],:) = Constraint-meas;
23             end
24         end      
25     end
26 end
 1 classdef EllipseMeasModelPointCloud < GenerativeMeasurementModel
 2     methods
 3         function measurement = measurementEquation(~, state, noise, ~)
 4             n_approx = 32;
 5             V_ = ell2polygon2([zeros(1,size(state,2))
 6                               zeros(2,size(state,2))
 7                               state(4:5,:)],n_approx);   
 8             H = inv2(phit2H(state(1:3,:)));          
 9             measurement = cell2mat(cellfun(@(w_) polyDist2(V_,abs(h2c(mul2Hv(H,w_)))), ...
10                                    mat2cell(noise,2*ones(1,size(noise,1)/2),size(noise,2)),...
11                                    'UniformOutput',false));
12         end      
13     end
14 end
15 
16 function d = polyDist2(V,P)
17 n_V = size(V,1)/2 -1;
18 n_P = size(P,2);
19 
20 if size(V,2) == 1
21     A = repmat(V,1,n_P);
22 else
23     A = V;
24 end
25 
26 P = repmat(P,n_V,1);
27 
28 B = circshift(A,[2,0]);  
29 
30 B = B(3:end,:);
31 A = A(3:end,:);
32 
33 P_A = P-A;
34 B_A = B-A;
35 
36 
37 
38 lambda =  ( P_A(1:2:end,:).*B_A(1:2:end,:) + P_A(2:2:end,:).*B_A(2:2:end,:) ) ./ ...
39                 ( B_A(1:2:end,:).*B_A(1:2:end,:) + B_A(2:2:end,:).*B_A(2:2:end,:) );
40             
41 K = A + clamp(lambda(reshape([1:n_V;1:n_V],[],1),:), [0 1]).*B_A;
42 
43 d = P-K;
44 
45 % Signed Distance
46 [d,idx] = min(sqrt(d(1:2:end,:).^2 + d(2:2:end,:).^2),[],1);
47 idx = repmat(((1:n_P)*(n_V*2))-n_V*2,2,1) + [2*idx-1
48                                               2*idx];
49 % idxL = ((1:n_P)*(n_V))-n_V + idx;                                          
50 % 
51 s = B_A(idx(1,:)).*P_A(idx(2,:))-P_A(idx(1,:)).*B_A(idx(2,:))<=0 ; % ( lambda(idxL)<=0 | lambda(idxL)>=1 );
52 d(s) = -d(s);
53 
54 % d = B_A(idx(1,:)).*P_A(idx(2,:))-P_A(idx(1,:)).*B_A(idx(2,:));
55 end
56 
57 function x = clamp(x,I)
58 x(x<I(1)) = I(1);
59 x(x>I(2)) = I(2);
60 end
61 
62 function V = ell2polygon2(x,n)
63 % x = [phi cx cy a b]
64 % Anzahl zu konvertierenden Ellipsen
65 nx = size(x,2);
66 % Abtastwinkel
67 theta = (0:n-1)/(n-1)*pi/2;
68 % Punkte auf Einheitskreis
69 V = repmat(reshape([cos(theta)
70                     sin(theta)],[],1),1,nx);
71 % skalierte Punkte
72 % xRichtung
73 V(1:2:end,:) = V(1:2:end,:).*repmat(x(4,:),n,1);
74 % yRichtung
75 V(2:2:end,:) = V(2:2:end,:).*repmat(x(5,:),n,1);
76 
77 % Drehe Punkte
78 s_phi = sin(x(1,:));
79 c_phi = cos(x(1,:));
80 V_ = V;
81 V(1:2:end,:) = V_(1:2:end,:).*repmat(c_phi,n,1)-V_(2:2:end,:).*repmat(s_phi,n,1);
82 V(2:2:end,:) = V_(1:2:end,:).*repmat(s_phi,n,1)+V_(2:2:end,:).*repmat(c_phi,n,1);
83 
84 % Verschieben
85 V(1:2:end,:) = V(1:2:end,:) + repmat(x(2,:),n,1);
86 V(2:2:end,:) = V(2:2:end,:) + repmat(x(3,:),n,1); 
87  
88 V = reshape(V,[],nx); 
89 end
Download algorithm (4 files) as ZIP

Comments

Please login to post a comment.