|
| 1 | +%[text] %[text:anchor:F4CCA683] # Robotics, Vision & Control 3e: for MATLAB |
| 2 | +%[text] %[text:anchor:874DADDE] # Appendices |
| 3 | +%[text:tableOfContents]{"heading":"Table of Contents"} |
| 4 | +%[text] %[text:anchor:H_FD45E4DF] Copyright 2022\-2023 Peter Corke, Witold Jachimczyk, Remo Pillat |
| 5 | +%[text] %[text:anchor:F954A5D5] ## Appendix C: Geometry |
| 6 | +%[text] %[text:anchor:BAEBF2AC] ### C.1 Euclidean Geometry |
| 7 | +%[text] %[text:anchor:E4239FA6] #### C.1\.2 Lines |
| 8 | +%[text] %[text:anchor:E7791A79] #### C.1\.2\.2 Lines in 3D and Plücker Coordinates |
| 9 | +P = [2 3 4]; Q = [3 5 7]; |
| 10 | +L = Plucker(P,Q) |
| 11 | +L.v |
| 12 | +L.w |
| 13 | +L.skew |
| 14 | +axis([-5 5 -5 5 -5 5]); |
| 15 | +L.plot("b"); |
| 16 | +L.point([0 1 2]) |
| 17 | +[x,d] = L.closest([1 2 3]) %#ok<*ASGLU> |
| 18 | +L.intersect_plane([0 0 1 0]) |
| 19 | +%% |
| 20 | +%[text] %[text:anchor:B107E972] #### C.1\.4 Ellipses and Ellipsoids |
| 21 | +E = [1 1;1 2]; |
| 22 | +clf; plotellipse(E); hold on |
| 23 | +[x,e] = eig(E) |
| 24 | +r = 1./sqrt(diag(e)) |
| 25 | +p = x(:,1)*r(1); quiver(0,0,p(1),p(2),0,"r"); |
| 26 | +p = x(:,2)*r(2); quiver(0,0,p(1),p(2),0,"r"); |
| 27 | +atan2d(x(2,1),x(1,1)) |
| 28 | +%% |
| 29 | +%[text] %[text:anchor:81891655] #### C.1\.4\.1 Drawing an Ellipse |
| 30 | +E = [1 1; 1 2]; |
| 31 | +th = linspace(0,2*pi,50); |
| 32 | +y = [cos(th);sin(th)]; |
| 33 | +x = inv(sqrtm(E))*y; |
| 34 | +clf; plot(x(1,:),x(2,:)); |
| 35 | +clf; plotellipse(E) |
| 36 | +%% |
| 37 | +%[text] %[text:anchor:A199F180] #### C.1\.4\.2 Fitting an Ellipse to Data |
| 38 | +rng(0); % reset random number generator |
| 39 | +x = []; % empty point set |
| 40 | +while size(x,2) < 500 |
| 41 | + p = (rand(2,1)-0.5)*4; |
| 42 | + if norm(p'*E*p) <= 1 |
| 43 | + x = [x p]; |
| 44 | + end |
| 45 | +end |
| 46 | +plot(x(1,:),x(2,:),".") |
| 47 | +% compute the moments |
| 48 | +m00 = mpq_point(x,0,0); |
| 49 | +m10 = mpq_point(x,1,0); |
| 50 | +m01 = mpq_point(x,0,1); |
| 51 | +xc = m10/m00; yc = m01/m00; |
| 52 | +% compute second moments relative to centroid |
| 53 | +x0 = x - [xc; yc]; |
| 54 | +m20 = mpq_point(x0,2,0); |
| 55 | +m02 = mpq_point(x0,0,2); |
| 56 | +m11 = mpq_point(x0,1,1); |
| 57 | +% compute the moments and ellipse matrix |
| 58 | +J = [m20 m11;m11 m02]; |
| 59 | +E_est = m00*inv(J)/4; |
| 60 | +E_est |
| 61 | +plotellipse(E_est,"r") |
| 62 | +%% |
| 63 | +%[text] %[text:anchor:22C91717] ### C.2 Homogeneous Coordinates |
| 64 | +%[text] %[text:anchor:A2EF233A] #### C.2\.1 Two Dimensions |
| 65 | +%[text] %[text:anchor:25F2AFB3] #### C.2\.1\.1 Points and Lines |
| 66 | +l1 = [1 -1 0]; |
| 67 | +l2 = [1 -1 -1]; |
| 68 | +plothomline(l1,"b") |
| 69 | +plothomline(l2,"r") |
| 70 | +cross(l1, l2) |
| 71 | +%[text] %[text:anchor:82267EC7] ## |
| 72 | +%[text] %[text:anchor:29028678] ## Appendix E: Linearization, Jacobians, and Hessians |
| 73 | +%[text] %[text:anchor:05136303] ### E.4 Deriving Jacobians |
| 74 | +zrange = @(xi,xv,w) ... |
| 75 | + [sqrt((xi(1)-xv(1))^2 + (xi(2)-xv(2))^2) + w(1); |
| 76 | + atan((xi(2)-xv(2))/(xi(1)-xv(1)))-xv(3) + w(2)]; |
| 77 | +xv = [1 2 pi/3]; xi = [10 8]; w= [0,0]; |
| 78 | +h0 = zrange(xi,xv,w) |
| 79 | +d = 0.001; |
| 80 | +J = [zrange(xi,xv+[1 0 0]*d,w)-h0 ... |
| 81 | + zrange(xi,xv+[0 1 0]*d,w)-h0 ... |
| 82 | + zrange(xi,xv+[0 0 1]*d,w)-h0]/d |
| 83 | +syms xi yi xv yv thetav wr wb |
| 84 | +z = zrange([xi yi],[xv yv thetav],[wr wb]) |
| 85 | +J = jacobian(z,[xv yv thetav]) |
| 86 | +whos J |
| 87 | +Jf = matlabFunction(J); |
| 88 | +xv = [1 2 pi/3]; xi = [10 8]; w = [0 0]; |
| 89 | +Jf(xi(1),xv(1),xi(2),xv(2)) |
| 90 | +%% |
| 91 | +%[text] %[text:anchor:99DE1C25] ## |
| 92 | +%% |
| 93 | +%[text] %[text:anchor:980876A6] ## Appendix G: Gaussian Random Variables |
| 94 | +x = linspace(-6,6,500); |
| 95 | +plot(x,gaussfunc(0,1,x),"r") |
| 96 | +hold on |
| 97 | +plot(x,gaussfunc(0,2^2,x),"b--") |
| 98 | +sigma = 1; mu = 0; |
| 99 | +g = sigma*randn(100,1) + mu; |
| 100 | +[x,y] = meshgrid(-5:0.1:5,-5:0.1:5); |
| 101 | +P = diag([1 2]).^2; |
| 102 | +surfc(x,y,gaussfunc([0 0],P,x,y)) |
| 103 | +s = chi2inv(0.5,2) |
| 104 | +%% |
| 105 | +%[text] %[text:anchor:DD77BE42] ## Appendix H: Kalman Filter |
| 106 | +%[text] %[text:anchor:72ADDA8C] ### H.2 Nonlinear Systems \-\- Extended Kalman Filter |
| 107 | +x = 2*randn(1000000,1) + 5; |
| 108 | +y = (x+2).^2/4; |
| 109 | +clf; histogram(y, Normalization="pdf"); |
| 110 | +%% |
| 111 | +%[text] %[text:anchor:1A4CC05A] ## Appendix I: Graphs |
| 112 | +g = UGraph() |
| 113 | +rng(10) |
| 114 | +for i = 1:5 |
| 115 | + g.add_node(rand(2,1)); |
| 116 | +end |
| 117 | +g.add_edge(1,2); |
| 118 | +g.add_edge(1,3); |
| 119 | +g.add_edge(1,4); |
| 120 | +g.add_edge(2,3); |
| 121 | +g.add_edge(2,4); |
| 122 | +g.add_edge(4,5); |
| 123 | +g |
| 124 | +clf; g.plot(labels=true); |
| 125 | +g.neighbors(2) |
| 126 | +e = g.edges(2) |
| 127 | +g.cost(e) |
| 128 | +g.nodes(5)' % transpose for display |
| 129 | +[n,c] = g.neighbors(2) |
| 130 | +g.about(1) |
| 131 | +g.closest([0.5 0.5]) |
| 132 | +g.path_Astar(3, 5) |
| 133 | +%% |
| 134 | +%[text] %[text:anchor:8633A41B] ## Appendix J: Peak finding |
| 135 | +%[text] %[text:anchor:794CD871] ### J.1 1D Signal |
| 136 | +load peakfit1 |
| 137 | +clf; plot(y,"-o") |
| 138 | +[ypk,k] = max(y) |
| 139 | +[ypk,k] = findpeaks(y,SortStr="descend"); |
| 140 | +ypk' % transpose for display |
| 141 | +k' % transpose for display |
| 142 | +ypk(2)/ypk(1) |
| 143 | +range=k(1)-1:k(1)+1 |
| 144 | +p = polyfit(range,y(range),2) % fit second-order polynomial |
| 145 | +pd = polyder(p) % derivative of fitted polynomial |
| 146 | +roots(pd) % zero value of the derivative |
| 147 | +ypk = findpeaks(y,MinPeakDistance=5)' % transpose for display |
| 148 | +%% |
| 149 | +%[text] %[text:anchor:A82CA7F7] ### J.2 2D Signal |
| 150 | +z |
| 151 | +[zmax,i] = max(z(:)) |
| 152 | +[y,x] = ind2sub(size(z),i) |
| 153 | +LMaxFinder = vision.LocalMaximaFinder(MaximumNumLocalMaxima=3, ... |
| 154 | + NeighborhoodSize=[3 3],Threshold=0); |
| 155 | +LMaxFinder(z) |
| 156 | +%[text] |
| 157 | +%[text] Suppress syntax warnings in this file |
| 158 | +%#ok<*NASGU> |
| 159 | +%#ok<*AGROW> |
| 160 | +%#ok<*MINV> |
| 161 | + |
| 162 | +%[appendix] |
| 163 | +%--- |
| 164 | +%[metadata:view] |
| 165 | +% data: {"layout":"inline","rightPanelPercent":40} |
| 166 | +%--- |
0 commit comments