经典手眼标定算法matlab代码

%Once the image is processed using the gridextractor method, this function %tries to fit the grid on the extraced points in order to establish the %correspondences (use this for severely distorted images, otherwise %(faster) use fitgrid % %Input: stats The processed blobs of this image % id0 The index of the main bar % id1 The index of the second main bar % searchPoints The potential grid points % doshow 1/0 to display / not display the results % %Output: success The flag indicating whether it worked or not % x_ the 2D points of the grid points % X_ the corresponding 3D points of the grid points % searchPoints The points % %Christian Wengert %Computer Vision Laboratory %ETH Zurich %Sternwartstrasse 7 %CH-8092 Zurich %www.vision.ee.ethz.ch/cwengert %wengert@vision.ee.ethz.ch function [success, x_, X_, searchPoints] = fitgridDistorted(stats, searchPoints, id0, id1, dx, dy, doshow) if(doshow), hold on, end %Init values x_ = []; X_ = []; success = 1; %Important sizes ndx = 1; ndy = 0; MAXANGLE = 25; %[deg] MAXDIST2LINE = 13; DISTFACTORBIG = 1.36; DISTFACTOR = 1.3; %Special values for special lines mainAxisX = -2; mainAxisY = -3; %Add the space for 3D coordinates searchPoints(4:5,:) = NaN*ones(2, length(searchPoints)); %number of searchPoints n = length(searchPoints); %Get main searchPoints first x0 = [stats(id0).Centroid(1); stats(id0).Centroid(2)]; x1 = [stats(id1).Centroid(1); stats(id1).Centroid(2)]; %Directions dir0 = (x1-x0)/(norm(x1-x0)); %Get a 90�angle dir90_ = [-dir0(2);dir0(1)]; %get the orientation of main bar orientation = stats(id0).Orientation; y = tand(orientation); dir90 = [-1;y]; dir90 = dir90/norm(dir90); %Compare direction of orientation with dir90_ and dir90 if(sign(dir90)==sign(dir90_)) else dir90 = -dir90; end %Show main bars + Directions if(doshow) line([x0(1), x0(1)+dir0(1)*100], [x0(2), x0(2)+dir0(2)*100], 'Color','r'); line([x0(1), x0(1)-dir0(1)*100], [x0(2), x0(2)-dir0(2)*100], 'Color','r'); line([x0(1), x0(1)+dir90(1)*100], [x0(2), x0(2)+dir90(2)*100]); line([x0(1), x0(1)-dir90(1)*100], [x0(2), x0(2)-dir90(2)*100]); end MAXDIST = stats(id0).MajorAxisLength/2; %Now first follow the better line and assign values searchPoints = getFirstLine(searchPoints, x0, -dir0, dx, 0, dx, 0, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); searchPoints = getFirstLine(searchPoints, x1, dir0, -3*dx, 0, -dx, 0, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); %Now the other direction, therefore we give some direction hint in %order to better get the same coordinate system always searchPoints = getOtherLine(searchPoints, x0, dir90, 0, 2*dy, 0, dy, MAXDIST, DISTFACTORBIG, MAXANGLE, MAXDIST2LINE, doshow); searchPoints = getOtherLine(searchPoints, x0, -dir90, 0, -2*dy, 0, -dy, MAXDIST, DISTFACTORBIG, MAXANGLE, MAXDIST2LINE, doshow); %Try to get all positive x-values ixpos = find(searchPoints(4,:) > 0); %these are the already found points that lie on x-axis in positive direction ixneg = find(searchPoints(4,:) < 0); %these are the already found points that lie on x-axis in negative direction %GO make new lines along x positive and y negative lastx = x0; %sort points to be closer to x0 [a,b] = sort(searchPoints(4,ixpos)); ixpos = ixpos(b); for i=ixpos x = searchPoints(2:3,i); %Also adapt the direction dir0 = (x-lastx)/(norm(x-lastx)); dir90 = [-dir0(2);dir0(1)]; if(doshow), plot(x(1) - dir90(1)*10,x(2) - dir90(2)*10,'yx'), end searchPoints = getOtherLine(searchPoints, x, -dir90, searchPoints(4,i), searchPoints(5,i)+dy, 0, dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); lastx = x; end %GO make new lines along x positive and y negative lastx = x0; for i=ixpos x = searchPoints(2:3,i); %Also adapt the direction dir0 = (x-lastx)/(norm(x-lastx)); dir90 = [-dir0(2);dir0(1)]; if(doshow), plot(x(1) + dir90(1)*10,x(2) + dir90(2)*10,'yx'), end searchPoints = getOtherLine(searchPoints, x, dir90, searchPoints(4,i), searchPoints(5,i)-dy, 0, -dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); lastx = x; end %GO make new lines along x negative and y negative [a,b] = sort(searchPoints(4,ixneg),'descend'); ixneg = ixneg(b); lastx = x1; for i=ixneg x = searchPoints(2:3,i); %Also adapt the direction dir0 = (x-lastx)/(norm(x-lastx)); dir90 = [-dir0(2);dir0(1)]; if(doshow),plot(x(1) + dir90(1)*10,x(2) + dir90(2)*10,'yx'), end searchPoints = getOtherLine(searchPoints, x, dir90, searchPoints(4,i), searchPoints(5,i)+dy, 0, dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); lastx = x; end %GO make new lines along x negative and y negative lastx = x1; for i=ixneg x = searchPoints(2:3,i); %Also adapt the direction dir0 = (x-lastx)/(norm(x-lastx)); dir90 = [-dir0(2);dir0(1)]; if(doshow),plot(x(1) - dir90(1)*10,x(2) - dir90(2)*10,'yx'),end searchPoints = getOtherLine(searchPoints, x, -dir90, searchPoints(4,i), searchPoints(5,i)-dy, 0, -dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); lastx = x; end % Now the two rows that are hidden by the small bar %Also adapt the direction dir0 = (x1-x0)/(norm(x1-x0)); dir90_ = [-dir0(2);dir0(1)]; orientation = stats(id1).Orientation; y = tand(orientation); dir0 = [-1;y]; dir0 = dir0/norm(dir0); dir90 = [-dir0(2);dir0(1)]; %Compare direction of orientation with dir90_ and dir90 if(sign(dir90)==sign(dir90_)) else dir90 = -dir90; dir0 = -dir0; end x = x1+dir0*stats(id1).MajorAxisLength/3; if(doshow), plot(x(1), x(2), 'mx','MarkerSize',15), end %First searchPoints = getOtherLine(searchPoints, x, -dir90, -2*dx, -dy, 0, -dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); searchPoints = getOtherLine(searchPoints, x, dir90, -2*dx, dy, 0, dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); %Second x = x1-dir0*stats(id1).MajorAxisLength/3; if(doshow), plot(x(1), x(2), 'mx','MarkerSize',15), end searchPoints = getOtherLine(searchPoints, x, -dir90, -dx, -dy, 0, -dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); searchPoints = getOtherLine(searchPoints, x, dir90, -dx, dy, 0, dy, MAXDIST, DISTFACTOR, MAXANGLE, MAXDIST2LINE, doshow); %Make the points finalindex = find(searchPoints(1,:)==0); x_ = searchPoints(2:3,finalindex); X_ = [searchPoints(4:5,finalindex)]; X_ = [X_;zeros(1,length(X_))]; return function searchPoints = getOtherLine(searchPoints, x0, dir0, sX, sY, dx, dy, d0, factor, MAXANGLE, MAXDIST2LINE, doshow) x = x0(1:2); xstart = x + dir0*10; dir = dir0; d = d0; cnt = 0; while(x(1)>0) if(doshow), line([x(1), x(1)+dir(1)*50], [x(2), x(2)+dir(2)*50]);end if(cnt==0) [x, d, dir, searchPoints] = getNextPointstart(searchPoints,x, xstart, dir, sX, sY, factor*d, MAXANGLE, MAXDIST2LINE, doshow); d = norm(x-x0); dir = (x - x0)/d; else [x, d, dir, searchPoints] = getNextPoint(searchPoints, x, dir, sX, sY, factor*d, MAXANGLE, MAXDIST2LINE, doshow); end %Get the angle % theta = acos(dir'*dir0)/(norm(dir)*norm(dir0))/pi*180 % %Its