1 | // needed under WIN32 to get M_PI defined because M_PI is clearly such an unusual constant. |
---|
2 | #define _USE_MATH_DEFINES |
---|
3 | #include <math.h> |
---|
4 | #include <assert.h> |
---|
5 | #include <float.h> |
---|
6 | #include <iostream> |
---|
7 | |
---|
8 | #include "General2Im.h" |
---|
9 | |
---|
10 | //#define DEBUG_GENERALRECTIFY |
---|
11 | |
---|
12 | using namespace MatVec; |
---|
13 | using namespace MultiViewGeom; |
---|
14 | using namespace Geometry; |
---|
15 | using namespace std; |
---|
16 | |
---|
17 | namespace Rectify { |
---|
18 | |
---|
19 | void GetEpipoles(const FMatrix<double> &FM, Homg2DPoint<double> &e1, Homg2DPoint<double> &e2) { |
---|
20 | double EigenVals[3]; |
---|
21 | EigenVals[0]=0.0; // Just to stop compiler warnings - it IS used. |
---|
22 | Matrix<double> EigenVecs(3,3), FT(3,3); |
---|
23 | |
---|
24 | // Do FM*FM^{T} |
---|
25 | // TTranspose |
---|
26 | double sum; |
---|
27 | for (unsigned int i=0;i<3;++i) { |
---|
28 | for (unsigned int k=0;k<=i;++k) |
---|
29 | { |
---|
30 | sum=FM(0,i)*FM(0,k); |
---|
31 | for (unsigned int j=1;j<3;++j) |
---|
32 | sum+=FM(j,i)*FM(j,k); |
---|
33 | FT(i,k)=sum; |
---|
34 | FT(k,i)=sum; |
---|
35 | } |
---|
36 | } |
---|
37 | |
---|
38 | MatrixOps::EigenValVec_Symmetric(FT, EigenVals, EigenVecs); |
---|
39 | e1[0]=EigenVecs(0,0); |
---|
40 | e1[1]=EigenVecs(1,0); |
---|
41 | e1[2]=EigenVecs(2,0); |
---|
42 | |
---|
43 | // Do FM^{T}*FM |
---|
44 | // TransposeT() |
---|
45 | for (unsigned int i=0;i<3;i++) { |
---|
46 | for (unsigned int k=0;k<3;k++) |
---|
47 | { |
---|
48 | sum=FM(i,0)*FM(k,0); |
---|
49 | for (unsigned int j=1;j<3;j++) |
---|
50 | sum+=FM(i,j)*FM(k,j); |
---|
51 | |
---|
52 | FT(i,k)=sum; |
---|
53 | } |
---|
54 | } |
---|
55 | |
---|
56 | |
---|
57 | MatrixOps::EigenValVec_Symmetric(FT, EigenVals, EigenVecs); |
---|
58 | e2[0]=EigenVecs(0,0); |
---|
59 | e2[1]=EigenVecs(1,0); |
---|
60 | e2[2]=EigenVecs(2,0); |
---|
61 | } |
---|
62 | |
---|
63 | |
---|
64 | // ----------------------------------------- |
---|
65 | // Functions extracted from my geometry code |
---|
66 | // to make this file less dependent |
---|
67 | // These are a bit naff! |
---|
68 | // ----------------------------------------- |
---|
69 | template <class T> |
---|
70 | inline T MIN(T a, T b) { |
---|
71 | if (a<b) |
---|
72 | return a; |
---|
73 | return b; |
---|
74 | } |
---|
75 | |
---|
76 | template <class T> |
---|
77 | inline T MAX(T a, T b) { |
---|
78 | if (a>b) |
---|
79 | return a; |
---|
80 | return b; |
---|
81 | } |
---|
82 | |
---|
83 | template <class T> |
---|
84 | inline short SGN(T a) { |
---|
85 | if (a<0.0) |
---|
86 | return -1; |
---|
87 | else |
---|
88 | return 1; |
---|
89 | } |
---|
90 | |
---|
91 | template <class VectorT1, class VectorT2, class VectorT3> |
---|
92 | inline void CrossProduct(const VectorT1 &a, const VectorT2 &b, VectorT3 &result) { |
---|
93 | assert(b.size()==3 && a.size()==3 && result.size()==3); |
---|
94 | result[0]=a[1]*b[2]-a[2]*b[1]; |
---|
95 | result[1]=a[2]*b[0]-a[0]*b[2]; |
---|
96 | result[2]=a[0]*b[1]-a[1]*b[0]; |
---|
97 | } |
---|
98 | |
---|
99 | /// Get the distance between 2 2D points. |
---|
100 | template <class Point2DT, class Point2DT2> |
---|
101 | static inline typename Point2DT::value_type distance_points(const Point2DT &P1, const Point2DT2 &P2) { |
---|
102 | return hypot(P1[0]-P2[0], P1[1]-P2[1]); |
---|
103 | } |
---|
104 | |
---|
105 | /// Test if parallel with another line. |
---|
106 | template <class Line2DT1, class Line2DT2, class T> |
---|
107 | static bool Parallel(const Line2DT1 &Line1, const Line2DT2 &Line2, const T tol) { |
---|
108 | T d1x, d1y, d2x, d2y; |
---|
109 | getperpdirection(Line1,d1x,d1y); getperpdirection(Line2,d2x,d2y); |
---|
110 | if ((fabs(d1x-d2x)<tol) && (fabs(d1y-d2y)<tol)) |
---|
111 | return true; |
---|
112 | return false; |
---|
113 | } |
---|
114 | |
---|
115 | // Get the direction of the line. This is basicaly just the normalised line such that sqrt(a^{2}+b^{2})=1.0. Note: This will be the perpendicular direction. |
---|
116 | template <class Line2DT, class T> |
---|
117 | static void getperpdirection(const Line2DT &Line, T &dx, T &dy) { |
---|
118 | T norm; |
---|
119 | norm=hypot(Line[0], Line[1]); |
---|
120 | if (norm!=0.0) |
---|
121 | { dx=Line[0]/norm; dy=Line[1]/norm; } |
---|
122 | else |
---|
123 | { dx=0.0; dy=0.0; } |
---|
124 | return; |
---|
125 | } |
---|
126 | |
---|
127 | /// To intersect with another line. Returns false if they don't intersect, or true if they do intersect. If they do intersect, the supplied point will be alterd to contain the intersecion point. If they don't intersect it won't be touched. |
---|
128 | template <class Point2DT, class Line2DT1, class Line2DT2, class T> |
---|
129 | static bool Intersect(Point2DT &IPoint, const Line2DT1 &Line1, const Line2DT2 &Line2, const T tol) { |
---|
130 | if (Parallel(Line1, Line2, tol)) |
---|
131 | return false; |
---|
132 | IPoint[0]=(Line1[1]*Line2[2]-Line1[2]*Line2[1])/(Line2[1]*Line1[0]-Line1[1]*Line2[0]); |
---|
133 | IPoint[1]=(Line1[0]*Line2[2]-Line1[2]*Line2[0])/(Line1[1]*Line2[0]-Line1[0]*Line2[1]); |
---|
134 | return true; |
---|
135 | } |
---|
136 | |
---|
137 | /// Intersect a given line with a line segment specified by two points. Returns false if it doesn't intersect, otherwise IntersectP is returned containing the intersection point. |
---|
138 | template <class Line2DT, class Point2DT1, class Point2DT2, class Point2DT3> |
---|
139 | static bool IntersectWithLineSeg(const Point2DT1 &SegStart, const Point2DT2 &SegEnd, const Line2DT &Line, Point2DT3 &IntersectP) { |
---|
140 | // Get line between SegStart->SegEnd |
---|
141 | Geometry::Line2D<double> SegLine( |
---|
142 | SegStart.y()-SegEnd.y(), |
---|
143 | SegEnd.x()-SegStart.x(), |
---|
144 | (SegStart.x()-SegEnd.x())*SegStart.y()+(SegEnd.y()-SegStart.y())*SegStart.x()); |
---|
145 | |
---|
146 | Geometry::Point2D<double> IPoint; |
---|
147 | if (!Intersect(IPoint, SegLine, Line, 1e-12)) |
---|
148 | return false; |
---|
149 | |
---|
150 | // if (IPoint[0]>=MIN(SegStart[0], SegEnd[0]) && IPoint[0]<=MAX(SegStart[0], SegEnd[0]) && IPoint[1]>=MIN(SegStart[1], SegEnd[1]) && IPoint[1]<=MAX(SegStart[1], SegEnd[1])) |
---|
151 | if (IPoint[0]-MIN(SegStart[0], SegEnd[0])>-1e-6 && IPoint[0]-MAX(SegStart[0], SegEnd[0])<1e-6 && IPoint[1]-MIN(SegStart[1], SegEnd[1])>-1e-6 && IPoint[1]-MAX(SegStart[1], SegEnd[1])<1e-6) |
---|
152 | { |
---|
153 | IntersectP[0]=IPoint[0]; |
---|
154 | IntersectP[1]=IPoint[1]; |
---|
155 | return true; |
---|
156 | } |
---|
157 | return false; |
---|
158 | } |
---|
159 | |
---|
160 | // Intersect the given line (Line) with the box defined by the four points (TopLeft, TopRight, BottomRight, BottomLeft). Returns true if it does intersect and false if it doesn't. If it does intersect, the start and end points of the intersected line segment are returned in Start and End |
---|
161 | template <class Point2DT1, class Point2DT2, class Point2DT3, class Point2DT4, class Point2DT5, class Point2DT6, class Line2DT> |
---|
162 | static bool IntersectWithBox(const Point2DT1 &TopLeft, const Point2DT2 &TopRight, const Point2DT3 &BottomLeft, const Point2DT4 &BottomRight, const Line2DT &Line, Point2DT5 &Start, Point2DT6 &End) { |
---|
163 | // So, intersect with all the line segments |
---|
164 | Geometry::Point2D<double> IPoints[4]; |
---|
165 | unsigned int i=0; |
---|
166 | |
---|
167 | if (IntersectWithLineSeg(TopLeft, TopRight, Line, IPoints[i])) |
---|
168 | i++; |
---|
169 | if (IntersectWithLineSeg(TopRight, BottomRight, Line, IPoints[i])) |
---|
170 | i++; |
---|
171 | if (IntersectWithLineSeg(BottomRight, BottomLeft, Line, IPoints[i])) |
---|
172 | i++; |
---|
173 | if (IntersectWithLineSeg(BottomLeft, TopLeft, Line, IPoints[i])) |
---|
174 | i++; |
---|
175 | |
---|
176 | if (i==0) |
---|
177 | return false; |
---|
178 | |
---|
179 | // Presumably intersects almost exactly with a corner (hopefuly!) |
---|
180 | // return start and end as the same |
---|
181 | if (i==1) { |
---|
182 | Start=IPoints[0]; End=IPoints[0]; |
---|
183 | } |
---|
184 | |
---|
185 | if (i==2) { |
---|
186 | Start=IPoints[0]; End=IPoints[1]; |
---|
187 | } |
---|
188 | |
---|
189 | if (i>=3) { |
---|
190 | Start=IPoints[0]; |
---|
191 | double dist=0.0; |
---|
192 | for (unsigned int c=1;c<i;++c) |
---|
193 | if (distance_points(IPoints[c], Start)>dist) |
---|
194 | { End=IPoints[c]; dist=distance_points(IPoints[c], Start); } |
---|
195 | } |
---|
196 | |
---|
197 | return true; |
---|
198 | } |
---|
199 | |
---|
200 | |
---|
201 | |
---|
202 | |
---|
203 | |
---|
204 | |
---|
205 | |
---|
206 | |
---|
207 | |
---|
208 | |
---|
209 | |
---|
210 | // -------------------------------------------- |
---|
211 | // Functions operating in the unrectified image |
---|
212 | // -------------------------------------------- |
---|
213 | |
---|
214 | // This adds a value has been added to an angle representing a polar co-ordinate. This function checks to see if the angle has looped around from -180 to 180 or vice-versa and corrects it appropriately. |
---|
215 | double GeneralPlanarRectify::addtoangle(double angle, const double addon) const { |
---|
216 | assert(addon<=2*M_PI); |
---|
217 | angle+=addon; |
---|
218 | while (angle<-M_PI) |
---|
219 | angle+=2.0*M_PI; |
---|
220 | while (angle>M_PI) |
---|
221 | angle-=2.0*M_PI; |
---|
222 | return angle; |
---|
223 | } |
---|
224 | |
---|
225 | double GeneralPlanarRectify::GetEpiDist(const Homg2DPoint<double> &e, const unsigned int xs, const unsigned int ys) const |
---|
226 | { |
---|
227 | // Check for a numerically infinite epipole |
---|
228 | if (e.w()>DBL_EPSILON*(MIN(fabs(e.x()), fabs(e.y())))) |
---|
229 | return DBL_MAX; |
---|
230 | Point2D<double> en; |
---|
231 | en.x()=e.x()/e.w(); en.y()=e.y()/e.w(); |
---|
232 | return distance_points(Point2D<double>(xs/2.0, ys/2.0), en); |
---|
233 | } |
---|
234 | |
---|
235 | // Gets an oriented epipolar line from a point, that is to say restricted up to a positive scale factor. This function works fine even if the epipole is infinite. |
---|
236 | Line2D<double> GeneralPlanarRectify::GetOrientELine(const Homg2DPoint<double> &Point, const Homg2DPoint<double> &e) const { |
---|
237 | Line2D<double> Line; |
---|
238 | CrossProduct(Point, e, Line); |
---|
239 | // Now, give the line a sign based on quadrant the point is in in relation to the |
---|
240 | // epipole |
---|
241 | short int sign=1; |
---|
242 | if ((Point.x()*e.w()<e.x() && Line.b()>0.0) || |
---|
243 | (Point.x()*e.w()>e.x() && Line.b()<0.0)) |
---|
244 | sign=-1; |
---|
245 | if ((Point.y()*e.w()<e.y() && Line.a()<0.0) || |
---|
246 | (Point.y()*e.w()>e.y() && Line.a()>0.0)) |
---|
247 | sign=-1; |
---|
248 | Line.a()=Line.a()*sign; |
---|
249 | Line.b()=Line.b()*sign; |
---|
250 | Line.c()=Line.c()*sign; |
---|
251 | return Line; |
---|
252 | } |
---|
253 | |
---|
254 | Geometry::Point2D<double> GeneralPlanarRectify::TransferPointUsingH(const MatVec::Matrix<double> &H, const double &px, const double &py) const { |
---|
255 | double tz=H(2,0)*px+H(2,1)*py+H(2,2); |
---|
256 | return Geometry::Point2D<double>((H(0,0)*px+H(0,1)*py+H(0,2))/tz, |
---|
257 | (H(1,0)*px+H(1,1)*py+H(1,2))/tz); |
---|
258 | } |
---|
259 | |
---|
260 | // Transfer a line using a homography |
---|
261 | Geometry::Line2D<double> GeneralPlanarRectify::TransferLineUsingH(const MatVec::Matrix<double> &H, const Geometry::Line2D<double> &Line) const { |
---|
262 | return Geometry::Line2D<double>(Line[0]*H(0,0)+Line[1]*H(1,0)+Line[2]*H(2,0), Line[0]*H(0,1)+Line[1]*H(1,1)+Line[2]*H(2,1), Line[0]*H(0,2)+Line[1]*H(1,2)+Line[2]*H(2,2)); |
---|
263 | } |
---|
264 | |
---|
265 | |
---|
266 | |
---|
267 | |
---|
268 | |
---|
269 | |
---|
270 | |
---|
271 | |
---|
272 | |
---|
273 | |
---|
274 | |
---|
275 | |
---|
276 | |
---|
277 | |
---|
278 | |
---|
279 | |
---|
280 | |
---|
281 | |
---|
282 | // ------------------------------------------- |
---|
283 | // Functions operating in the nonlinear image. |
---|
284 | // ------------------------------------------- |
---|
285 | |
---|
286 | double GeneralPlanarRectify::GetPointAngle(const Point2D<double> &Point) const { |
---|
287 | // Get the relevant half epipolar line and do it from that instead |
---|
288 | // That way will still work even if epipole is infinite |
---|
289 | Line2D<double> Line=GetOrientELine(Homg2DPoint<double>(Point.x(), Point.y(), 1.0), epipole); |
---|
290 | return atan2(Line.a(), Line.b()); |
---|
291 | } |
---|
292 | |
---|
293 | double GeneralPlanarRectify::GetDist(const Point2D<double> &Point) const { |
---|
294 | return distance_points(Point, epipole); |
---|
295 | } |
---|
296 | |
---|
297 | Line2D<double> GeneralPlanarRectify::GetEpipolarLineFromAng(const double angle) const { |
---|
298 | return Line2D<double>(sin(angle)*epipole.w(), |
---|
299 | cos(angle)*epipole.w(), |
---|
300 | -epipole.x()*sin(angle)-epipole.y()*cos(angle)); |
---|
301 | } |
---|
302 | |
---|
303 | Point2D<double> GeneralPlanarRectify::GetPointFromDistAndAng(const double dist, const double angle) const { |
---|
304 | // Now, get point at distance dist from the epipole along the line Line |
---|
305 | const Line2D<double> Line=GetEpipolarLineFromAng(angle); |
---|
306 | const double norm=hypot(Line[1],Line[0]); |
---|
307 | // Get normalised line gradient into dx,dy |
---|
308 | const double dx=Line[1]/norm, dy=-Line[0]/norm; |
---|
309 | return Point2D<double>(epipole.x()+dx*dist, epipole.y()+dy*dist); |
---|
310 | } |
---|
311 | |
---|
312 | Point2D<double> GeneralPlanarRectify::RectPointNLin(const Point2D<double> &PointIn) const { |
---|
313 | // So, first need to find the relevant angle |
---|
314 | const double theta=GetPointAngle(PointIn); |
---|
315 | |
---|
316 | // Now, find the relevant pair of surrounding angles in the map |
---|
317 | RectifyTabT::const_iterator first=RectTab.begin(), second=first; ++second; |
---|
318 | // Need the final fabs(addtoanglestuff)>M_PI to prevent problems at the wrap around from M_PI to -M_PI. |
---|
319 | bool iterate=true; |
---|
320 | while (second!=RectTab.end() && iterate) { |
---|
321 | if ((SGN(addtoangle(second->first, -theta))==SGN(addtoangle(first->first, -theta)) || fabs(addtoangle(second->first, -theta)-addtoangle(first->first, -theta))>M_PI)) { |
---|
322 | ++second; ++first; |
---|
323 | } |
---|
324 | else |
---|
325 | iterate=false; |
---|
326 | } |
---|
327 | double angle1=MIN(first->first, second->first), |
---|
328 | angle2=MAX(first->first, second->first); |
---|
329 | double y1=first->second; |
---|
330 | |
---|
331 | double x=GetDist(PointIn); |
---|
332 | |
---|
333 | // use angle of line (theta) to interpolate an exact y value |
---|
334 | return Point2D<double>(x, ((theta-angle1)/(angle2-angle1))+y1); |
---|
335 | } |
---|
336 | |
---|
337 | Point2D<double> GeneralPlanarRectify::UnRectPointNLin(const Point2D<double> &PointIn) const { |
---|
338 | double dist=PointIn.x(), angle; |
---|
339 | |
---|
340 | // NonLinear Y scaling |
---|
341 | // So, use the y co-ordinate to look up the relevant epipolar line |
---|
342 | const double ryf=floor(PointIn.y()); |
---|
343 | const int ry=(int)ryf; |
---|
344 | |
---|
345 | // If y is effectively integer, just read out of the table |
---|
346 | if (fabs(PointIn.y()-ryf)<1e-6) |
---|
347 | angle=UnRectTab[ry]; |
---|
348 | else |
---|
349 | { |
---|
350 | // y is sub-pixel so need to interpolate |
---|
351 | double theta1=UnRectTab[ry], theta2=UnRectTab[ry+1]; |
---|
352 | // So, to get actual line, interpolate between angles |
---|
353 | angle=((theta2-theta1)*(PointIn.y()-floor(PointIn.y())))+theta1; |
---|
354 | } |
---|
355 | return GetPointFromDistAndAng(dist, angle); |
---|
356 | } |
---|
357 | |
---|
358 | |
---|
359 | |
---|
360 | |
---|
361 | |
---|
362 | |
---|
363 | |
---|
364 | |
---|
365 | |
---|
366 | |
---|
367 | |
---|
368 | |
---|
369 | |
---|
370 | |
---|
371 | |
---|
372 | |
---|
373 | // ------------------------------------------------ |
---|
374 | // Major functions that actualy control the process |
---|
375 | // ------------------------------------------------ |
---|
376 | #ifdef DEBUG_GENERALRECTIFY |
---|
377 | template <class Iterator> |
---|
378 | double TransferError(Matrix<double> &H, Iterator MatchesBegin, Iterator MatchesEnd) { |
---|
379 | double totalerr=0.0; |
---|
380 | double npoints=0.0; |
---|
381 | for (Iterator iter=MatchesBegin; iter!=MatchesEnd; ++iter) { |
---|
382 | const double px=iter->first[0], py=iter->first[1], pw=1.0; |
---|
383 | double tx=H(0,0)*px+H(0,1)*py+H(0,2)*pw; |
---|
384 | double ty=H(1,0)*px+H(1,1)*py+H(1,2)*pw; |
---|
385 | double tz=H(2,0)*px+H(2,1)*py+H(2,2)*pw; |
---|
386 | Point2D<double> NP(tx/tz,ty/tz); |
---|
387 | totalerr+=distance_points(NP, iter->second); |
---|
388 | ++npoints; |
---|
389 | } |
---|
390 | return totalerr/npoints; |
---|
391 | } |
---|
392 | #endif |
---|
393 | |
---|
394 | void GeneralPlanarRectify::SetUpTablesAndXBounds(void) { |
---|
395 | minxim1=DBL_MAX; maxxim1=-DBL_MAX; minxim2=DBL_MAX; maxxim2=-DBL_MAX; |
---|
396 | RectTab.clear(); UnRectTab.clear(); |
---|
397 | |
---|
398 | Line2D<double> Line; |
---|
399 | Point2D<double> start,end; |
---|
400 | |
---|
401 | Point2D<double> TopLeft1(0,0), TopRight1(im1xs,0); |
---|
402 | Point2D<double> BottomRight1(im1xs,im1ys), BottomLeft1(0,im1ys); |
---|
403 | Point2D<double> TopLeft2(0,0), TopRight2(im1xs,0); |
---|
404 | Point2D<double> BottomRight2(im1xs,im1ys), BottomLeft2(0,im1ys); |
---|
405 | |
---|
406 | TopLeft1=TransferPointUsingH(Plane0Im1, TopLeft1.x(), TopLeft1.y()); |
---|
407 | TopRight1=TransferPointUsingH(Plane0Im1, TopRight1.x(), TopRight1.y()); |
---|
408 | BottomLeft1=TransferPointUsingH(Plane0Im1, BottomLeft1.x(), BottomLeft1.y()); |
---|
409 | BottomRight1=TransferPointUsingH(Plane0Im1, BottomRight1.x(), BottomRight1.y()); |
---|
410 | TopLeft2=TransferPointUsingH(Plane0Im2, TopLeft2.x(), TopLeft2.y()); |
---|
411 | TopRight2=TransferPointUsingH(Plane0Im2, TopRight2.x(), TopRight2.y()); |
---|
412 | BottomLeft2=TransferPointUsingH(Plane0Im2, BottomLeft2.x(), BottomLeft2.y()); |
---|
413 | BottomRight2=TransferPointUsingH(Plane0Im2, BottomRight2.x(), BottomRight2.y()); |
---|
414 | |
---|
415 | double angle=minang, angle2=minang; |
---|
416 | double maxang2=maxang; |
---|
417 | if (maxang<minang) |
---|
418 | maxang2+=2.0*M_PI; |
---|
419 | double x,x2; |
---|
420 | double angstep; |
---|
421 | |
---|
422 | while (angle2<maxang2) { |
---|
423 | // Handle the line at the current angle |
---|
424 | // Add it into the rectification tables |
---|
425 | RectTab.push_back(make_pair(angle, UnRectTab.size())); |
---|
426 | UnRectTab.push_back(angle); |
---|
427 | |
---|
428 | // Update max and min x for both images |
---|
429 | // Do image 1 first |
---|
430 | Line=GetEpipolarLineFromAng(angle); |
---|
431 | // Update max and min x for line |
---|
432 | IntersectWithBox(TopLeft1, TopRight1, BottomLeft1, BottomRight1, |
---|
433 | Line, start, end); |
---|
434 | x=GetDist(start); x2=GetDist(end); |
---|
435 | minxim1=MIN(x, minxim1); maxxim1=MAX(x, maxxim1); |
---|
436 | minxim1=MIN(x2, minxim1); maxxim1=MAX(x2, maxxim1); |
---|
437 | // Get maximum angle step for image 1 that preserves worst case pixel size |
---|
438 | angstep=atan(1.0/MAX(x,x2)); |
---|
439 | |
---|
440 | // Now image 2 |
---|
441 | // Already have the epipolar line in image 1 |
---|
442 | // Update max and min x for line |
---|
443 | IntersectWithBox(TopLeft2, TopRight2, BottomLeft2, BottomRight2, |
---|
444 | Line, start, end); |
---|
445 | |
---|
446 | x=GetDist(start); x2=GetDist(end); |
---|
447 | minxim2=MIN(x, minxim2); maxxim2=MAX(x, maxxim2); |
---|
448 | minxim2=MIN(x2, minxim2); maxxim2=MAX(x2, maxxim2); |
---|
449 | |
---|
450 | // Make angstep minimal step |
---|
451 | angstep=MIN(angstep, atan(1.0/MAX(x,x2))); |
---|
452 | |
---|
453 | // Now, find the next epipolar line |
---|
454 | // Is smallest angle step of step in image 2 and image 1 |
---|
455 | angle=addtoangle(angle, angstep); |
---|
456 | angle2+=angstep; |
---|
457 | } |
---|
458 | |
---|
459 | // Make min and max x same for both images so both images are the same size |
---|
460 | minxim1=MIN(minxim1, minxim2); |
---|
461 | maxxim1=MAX(maxxim1, maxxim2); |
---|
462 | minxim2=minxim1; maxxim2=maxxim1; |
---|
463 | |
---|
464 | #ifdef DEBUG_GENERALRECTIFY |
---|
465 | cerr << "Image 1: minx=" << minxim1 << " maxx=" << maxxim1 << endl; |
---|
466 | cerr << "Image 2: minx=" << minxim2 << " maxx=" << maxxim2 << endl; |
---|
467 | cerr << "Output image y sizes = " << (unsigned int)UnRectTab.size() << endl; |
---|
468 | #endif |
---|
469 | } |
---|
470 | |
---|
471 | void GeneralPlanarRectify::GetImAngleRange(const unsigned int xs, const unsigned int ys, double &minangim, double &maxangim, const Matrix<double> &H) const { |
---|
472 | Point2D<double> TopLeft(0,0), TopRight(xs,0); |
---|
473 | Point2D<double> BottomRight(xs,ys), BottomLeft(0,ys); |
---|
474 | |
---|
475 | TopLeft=TransferPointUsingH(H, TopLeft.x(), TopLeft.y()); |
---|
476 | TopRight=TransferPointUsingH(H, TopRight.x(), TopRight.y()); |
---|
477 | BottomLeft=TransferPointUsingH(H, BottomLeft.x(), BottomLeft.y()); |
---|
478 | BottomRight=TransferPointUsingH(H, BottomRight.x(), BottomRight.y()); |
---|
479 | |
---|
480 | // Check where the epipole is. |
---|
481 | // So, check if epipole is within transfered image by intersecting |
---|
482 | // line with each corner with the image box. If all lines intersect the box |
---|
483 | // twice then is within the image |
---|
484 | Line2D<double> TopL, TopR, BotL, BotR; |
---|
485 | CrossProduct(epipole, Homg2DPoint<double>(TopLeft.x(), TopLeft.y(), 1.0), TopL); |
---|
486 | CrossProduct(epipole, Homg2DPoint<double>(TopRight.x(), TopRight.y(), 1.0), TopR); |
---|
487 | CrossProduct(epipole, Homg2DPoint<double>(BottomLeft.x(), BottomLeft.y(), 1.0), BotL); |
---|
488 | CrossProduct(epipole, Homg2DPoint<double>(BottomRight.x(), BottomRight.y(), 1.0), BotR); |
---|
489 | |
---|
490 | Point2D<double> start1,end1; |
---|
491 | Point2D<double> start2,end2; |
---|
492 | Point2D<double> start3,end3; |
---|
493 | Point2D<double> start4,end4; |
---|
494 | if (IntersectWithBox(TopLeft, TopRight, BottomLeft, BottomRight, |
---|
495 | TopL, start1, end1) && |
---|
496 | IntersectWithBox(TopLeft, TopRight, BottomLeft, BottomRight, |
---|
497 | TopR, start2, end2) && |
---|
498 | IntersectWithBox(TopLeft, TopRight, BottomLeft, BottomRight, |
---|
499 | BotL, start3, end3) && |
---|
500 | IntersectWithBox(TopLeft, TopRight, BottomLeft, BottomRight, |
---|
501 | BotR, start4, end4)) { |
---|
502 | // If all lines intersected with the box twice then the epipole must be within |
---|
503 | // the image. Need to be careful of rounding errors, so don't check exact |
---|
504 | // equivalence |
---|
505 | double THRESH=1e-4; |
---|
506 | if (distance_points(start1,end1)>THRESH && |
---|
507 | distance_points(start2,end2)>THRESH && |
---|
508 | distance_points(start3,end3)>THRESH && |
---|
509 | distance_points(start4,end4)>THRESH) { |
---|
510 | minangim=-M_PI; maxangim=M_PI; return; |
---|
511 | } |
---|
512 | } |
---|
513 | |
---|
514 | double addon=-GetPointAngle(TopLeft), angle; |
---|
515 | minangim=0.0; maxangim=0.0; |
---|
516 | angle=GetPointAngle(TopRight); angle=addtoangle(angle, addon); |
---|
517 | minangim=MIN(minangim, angle); maxangim=MAX(maxangim, angle); |
---|
518 | angle=GetPointAngle(BottomRight); angle=addtoangle(angle, addon); |
---|
519 | minangim=MIN(minangim, angle); maxangim=MAX(maxangim, angle); |
---|
520 | angle=GetPointAngle(BottomLeft); angle=addtoangle(angle, addon); |
---|
521 | minangim=MIN(minangim, angle); maxangim=MAX(maxangim, angle); |
---|
522 | |
---|
523 | minangim=addtoangle(minangim, -addon); |
---|
524 | maxangim=addtoangle(maxangim, -addon); |
---|
525 | } |
---|
526 | |
---|
527 | |
---|
528 | |
---|
529 | |
---|
530 | |
---|
531 | |
---|
532 | |
---|
533 | |
---|
534 | |
---|
535 | |
---|
536 | |
---|
537 | |
---|
538 | |
---|
539 | |
---|
540 | |
---|
541 | |
---|
542 | |
---|
543 | |
---|
544 | |
---|
545 | |
---|
546 | |
---|
547 | |
---|
548 | |
---|
549 | |
---|
550 | |
---|
551 | |
---|
552 | //------------------------------------- |
---|
553 | // Public functions for use by the user |
---|
554 | //------------------------------------- |
---|
555 | |
---|
556 | GeneralPlanarRectify::GeneralPlanarRectify(const unsigned int im1xs_, const unsigned int im1ys_, |
---|
557 | const unsigned int im2xs_, const unsigned int im2ys_, |
---|
558 | const FMatrix<double> &FM_, |
---|
559 | const vector<pair<Point2D<double>, Point2D<double> > > &Matches_) |
---|
560 | : im1xs(im1xs_), im1ys(im1ys_), |
---|
561 | im2xs(im2xs_), im2ys(im2ys_), FM(FM_) { |
---|
562 | |
---|
563 | Homg2DPoint<double> e1, e2; |
---|
564 | GetEpipoles(FM, e1, e2); |
---|
565 | |
---|
566 | #ifdef DEBUG_GENERALRECTIFY |
---|
567 | cerr << "Image sizes: " << im1xs << "x" << im1ys << endl; |
---|
568 | cerr << "Image 1 epipole: (" << e1.x()/e1.w() << "," << e1.y()/e1.w() << ")"; |
---|
569 | cerr << " Image 2 epipole: (" << e2.x()/e2.w() << "," << e2.y()/e2.w() << ")" << endl; |
---|
570 | #endif |
---|
571 | |
---|
572 | typedef vector<pair<Point2D<double>, Point2D<double> > > MatchVec2Im; |
---|
573 | // Make all the matches perfect |
---|
574 | MatchVec2Im Matches, OutlierFree; |
---|
575 | for (unsigned int i=0;i<Matches_.size();++i) |
---|
576 | // Matches.push_back(FM.get_nearest_perfect_match(Matches_[i], 1e-7)); |
---|
577 | Matches.push_back(Matches_[i]); |
---|
578 | |
---|
579 | // --------------------------------------------- |
---|
580 | // Now, get the consistant homography (robustly) |
---|
581 | // --------------------------------------------- |
---|
582 | Matrix<double> H(3,3), HInv(3,3); |
---|
583 | |
---|
584 | // Yeuch - what WAS I thinking when I wrote the calcplanarhomog routines. |
---|
585 | // I did them the wrong way around :o( |
---|
586 | HomogConsistF::RANSAC(Matches, OutlierFree, HInv, FM); |
---|
587 | MatrixOps::inverse(HInv, H); |
---|
588 | |
---|
589 | #ifdef DEBUG_GENERALRECTIFY |
---|
590 | cerr << "Got Full Homography, transfer error=" << TransferError(H, OutlierFree.begin(), OutlierFree.end()) << " outliers=" << (unsigned int)(Matches.size()-OutlierFree.size()) << endl; |
---|
591 | /* |
---|
592 | Geometry:: Point2D<double> e1n(e1.x()/e1.w(), e1.y()/e1.w()), e2n(e2.x()/e2.w(), e2.y()/e2.w()); |
---|
593 | MultiViewGeom::EuclMatch2Im ematch(e2n,e1n); |
---|
594 | cerr << TransferError(H, &ematch, &ematch+1) << endl;*/ |
---|
595 | #endif |
---|
596 | |
---|
597 | Matrix<double> id(3,3); |
---|
598 | for (size_t i=0;i<3;++i) |
---|
599 | for (size_t j=0;j<3;++j) { |
---|
600 | if (i==j) |
---|
601 | id(i,j)=1.0; |
---|
602 | else |
---|
603 | id(i,j)=0.0; |
---|
604 | } |
---|
605 | // Select image with epipole closest to the image as the nonlinear image |
---|
606 | if (GetEpiDist(e1, im1xs, im1ys)<GetEpiDist(e2, im2xs, im2ys)) { |
---|
607 | epipole=e1; |
---|
608 | Plane0Im1=id; Plane0InvIm1=Plane0Im1; |
---|
609 | Plane0Im2=H; Plane0InvIm2=HInv; |
---|
610 | #ifdef DEBUG_GENERALRECTIFY |
---|
611 | cerr << "Doing nonlinear transform in image 1" << endl; |
---|
612 | #endif |
---|
613 | } |
---|
614 | else |
---|
615 | { |
---|
616 | epipole=e2; |
---|
617 | Plane0Im1=HInv; Plane0InvIm1=H; |
---|
618 | Plane0Im2=id; Plane0InvIm2=Plane0Im2; |
---|
619 | #ifdef DEBUG_GENERALRECTIFY |
---|
620 | cerr << "Doing nonlinear transform in image 2" << endl; |
---|
621 | #endif |
---|
622 | } |
---|
623 | epipole.x()=epipole.x()/epipole.w(); |
---|
624 | epipole.y()=epipole.y()/epipole.w(); |
---|
625 | epipole.w()=1.0; |
---|
626 | |
---|
627 | // ------------------------------------------ |
---|
628 | // Get the common angle range for both images |
---|
629 | // ------------------------------------------ |
---|
630 | // First get extreme angles from both images |
---|
631 | double minangim1, maxangim1, minangim2, maxangim2; |
---|
632 | GetImAngleRange(im1xs, im1ys, minangim1, maxangim1, Plane0Im1); |
---|
633 | GetImAngleRange(im2xs, im2ys, minangim2, maxangim2, Plane0Im2); |
---|
634 | #ifdef DEBUG_GENERALRECTIFY |
---|
635 | cerr << " Image 1 min angle=" << minangim1 << " max angle=" << maxangim1 << endl; |
---|
636 | cerr << " Image 2 min angle=" << minangim2 << " max angle=" << maxangim2 << endl; |
---|
637 | #endif |
---|
638 | |
---|
639 | // If the epipole is in either image, then have anomalous case |
---|
640 | if (maxangim1-minangim1==2.0*M_PI || maxangim2-minangim2==2.0*M_PI) |
---|
641 | { |
---|
642 | // Set to range of image without epipole in it |
---|
643 | // If both within then will simply be full range anyway |
---|
644 | if (maxangim1-minangim1==2.0*M_PI) |
---|
645 | { minang=minangim2; maxang=maxangim2; } |
---|
646 | else |
---|
647 | { minang=minangim1; maxang=maxangim1; } |
---|
648 | } |
---|
649 | else |
---|
650 | { |
---|
651 | // Otherwise need to take second minimum and second maximum angle |
---|
652 | if (fabs(minangim1-minangim2)>M_PI) |
---|
653 | minang=MIN(minangim1, minangim2); |
---|
654 | else |
---|
655 | minang=MAX(minangim1, minangim2); |
---|
656 | |
---|
657 | if (fabs(maxangim1-maxangim2)>M_PI) |
---|
658 | maxang=MAX(maxangim1, maxangim2); |
---|
659 | else |
---|
660 | maxang=MIN(maxangim1, maxangim2); |
---|
661 | } |
---|
662 | #ifdef DEBUG_GENERALRECTIFY |
---|
663 | cerr << " Common range, min angle=" << minang << " max angle=" << maxang << endl; |
---|
664 | #endif |
---|
665 | |
---|
666 | SetUpTablesAndXBounds(); |
---|
667 | |
---|
668 | // Now, check to see if need to flip the images about y axis |
---|
669 | // Do flips by flipping the angle range (add 180 degrees), and |
---|
670 | // using -ve distances |
---|
671 | // Purely aesthetic and totaly unimportant |
---|
672 | flipx=false; |
---|
673 | if (SGN(e1.x())*SGN(e1.w())==1) |
---|
674 | flipx=true; |
---|
675 | #ifdef DEBUG_GENERALRECTIFY |
---|
676 | cerr << "flipx: "; |
---|
677 | if (flipx) |
---|
678 | cerr << "true" << endl; |
---|
679 | else |
---|
680 | cerr << "false" << endl; |
---|
681 | #endif |
---|
682 | |
---|
683 | } |
---|
684 | |
---|
685 | GeneralPlanarRectify::~GeneralPlanarRectify() { |
---|
686 | } |
---|
687 | |
---|
688 | void GeneralPlanarRectify::RectifyPointIm1(double &x, double &y) const { |
---|
689 | // First, convert the point to the nonlinear image |
---|
690 | Point2D<double> PointIn(TransferPointUsingH(Plane0Im1, x, y)); |
---|
691 | |
---|
692 | PointIn=RectPointNLin(PointIn); |
---|
693 | // And account for image bounds and flipping |
---|
694 | PointIn.x()-=minxim1; |
---|
695 | if (flipx) |
---|
696 | PointIn.x()=(maxxim1-minxim1)-PointIn.x(); |
---|
697 | x=PointIn.x(); y=PointIn.y(); |
---|
698 | } |
---|
699 | void GeneralPlanarRectify::RectifyPointIm2(double &x, double &y) const { |
---|
700 | // First, convert the point to the nonlinear image |
---|
701 | Point2D<double> PointIn(TransferPointUsingH(Plane0Im2, x, y)); |
---|
702 | |
---|
703 | PointIn=RectPointNLin(PointIn); |
---|
704 | // And account for image bounds and flipping |
---|
705 | PointIn.x()-=minxim2; |
---|
706 | if (flipx) |
---|
707 | PointIn.x()=(maxxim2-minxim2)-PointIn.x(); |
---|
708 | x=PointIn.x(); y=PointIn.y(); |
---|
709 | } |
---|
710 | |
---|
711 | void GeneralPlanarRectify::UnRectifyPointIm1(double &x, double &y) const { |
---|
712 | // First, remove any bounding and flipping effects |
---|
713 | if (flipx) |
---|
714 | x=maxxim1-x; |
---|
715 | else |
---|
716 | x+=minxim1; |
---|
717 | |
---|
718 | // Undo nonlinear rectification |
---|
719 | Point2D<double> PointIn(UnRectPointNLin(Point2D<double>(x,y))); |
---|
720 | |
---|
721 | // And transfer to image one |
---|
722 | PointIn=TransferPointUsingH(Plane0InvIm1, PointIn.x(), PointIn.y()); |
---|
723 | |
---|
724 | x=PointIn.x(); y=PointIn.y(); |
---|
725 | } |
---|
726 | void GeneralPlanarRectify::UnRectifyPointIm2(double &x, double &y) const { |
---|
727 | // First, remove any bounding and flipping effects |
---|
728 | if (flipx) |
---|
729 | x=maxxim2-x; |
---|
730 | else |
---|
731 | x+=minxim2; |
---|
732 | |
---|
733 | // Undo nonlinear rectification |
---|
734 | Point2D<double> PointIn(UnRectPointNLin(Point2D<double>(x,y))); |
---|
735 | |
---|
736 | // And transfer to image one |
---|
737 | PointIn=TransferPointUsingH(Plane0InvIm2, PointIn.x(), PointIn.y()); |
---|
738 | x=PointIn.x(); y=PointIn.y(); |
---|
739 | } |
---|
740 | |
---|
741 | // This function unrectifies a set of points in image 1 all on the same epipolar |
---|
742 | // line |
---|
743 | void GeneralPlanarRectify::UnRectifyPointsIm1(vector<double> xvals, |
---|
744 | const double y, |
---|
745 | vector<Point2D<double> > &RectPoints) const { |
---|
746 | |
---|
747 | double angle; |
---|
748 | // So, get the angle of the unrectified line first |
---|
749 | // NonLinear Y scaling |
---|
750 | // So, use the y co-ordinate to look up the relevant epipolar line |
---|
751 | const double ryf=floor(((double)y)); |
---|
752 | const int ry=(int)ryf; |
---|
753 | |
---|
754 | // If y is effectively integer, just read out of the table |
---|
755 | if (fabs(y-ryf)<1e-6) |
---|
756 | angle=UnRectTab[ry]; |
---|
757 | else |
---|
758 | { |
---|
759 | // y is sub-pixel so need to interpolate |
---|
760 | double theta1=UnRectTab[ry], theta2=UnRectTab[ry+1]; |
---|
761 | // So, to get actual line, interpolate between angles |
---|
762 | angle=((theta2-theta1)*(y-floor(y)))+theta1; |
---|
763 | } |
---|
764 | |
---|
765 | const Line2D<double> ELine(GetEpipolarLineFromAng(angle)); |
---|
766 | const double norm=hypot(ELine[1],ELine[0]); |
---|
767 | // Get normalised line gradient into dx,dy |
---|
768 | const double dx=ELine[1]/norm, dy=-ELine[0]/norm; |
---|
769 | |
---|
770 | double dist; |
---|
771 | vector<double>::iterator BeginX=xvals.begin(), EndX=xvals.end(); |
---|
772 | for (; BeginX!=EndX; ++BeginX) { |
---|
773 | // So, get dist for the given point |
---|
774 | if (flipx) |
---|
775 | dist=maxxim1-*BeginX; |
---|
776 | else |
---|
777 | dist=*BeginX+minxim1; |
---|
778 | // And find point at distance dist from the epipole on the given line |
---|
779 | Point2D<double> Im1P(epipole.x()+dx*dist, epipole.y()+dy*dist); |
---|
780 | RectPoints.push_back(TransferPointUsingH(Plane0InvIm1, Im1P.x(), Im1P.y())); |
---|
781 | } |
---|
782 | } |
---|
783 | |
---|
784 | // This function unrectifies a set of points in image 2 all on the same epipolar |
---|
785 | // line |
---|
786 | void GeneralPlanarRectify::UnRectifyPointsIm2(vector<double> xvals, |
---|
787 | const double y, |
---|
788 | vector<Point2D<double> > &RectPoints) const { |
---|
789 | double angle; |
---|
790 | // So, get the angle of the unrectified line first |
---|
791 | // NonLinear Y scaling |
---|
792 | // So, use the y co-ordinate to look up the relevant epipolar line |
---|
793 | const double ryf=floor(y); |
---|
794 | const int ry=(int)ryf; |
---|
795 | |
---|
796 | // If y is effectively integer, just read out of the table |
---|
797 | if (fabs(y-ryf)<1e-6) |
---|
798 | angle=UnRectTab[ry]; |
---|
799 | else |
---|
800 | { |
---|
801 | // y is sub-pixel so need to interpolate |
---|
802 | double theta1=UnRectTab[ry], theta2=UnRectTab[ry+1]; |
---|
803 | // So, to get actual line, interpolate between angles |
---|
804 | angle=((theta2-theta1)*(y-floor(y)))+theta1; |
---|
805 | } |
---|
806 | |
---|
807 | const Line2D<double> ELine(GetEpipolarLineFromAng(angle)); |
---|
808 | const double norm=hypot(ELine[1],ELine[0]); |
---|
809 | // Get normalised line gradient into dx,dy |
---|
810 | const double dx=ELine[1]/norm, dy=-ELine[0]/norm; |
---|
811 | |
---|
812 | vector<double>::iterator BeginX=xvals.begin(), EndX=xvals.end(); |
---|
813 | for (; BeginX!=EndX; ++BeginX) { |
---|
814 | // So, get dist for the given point |
---|
815 | double dist; |
---|
816 | if (flipx) |
---|
817 | dist=maxxim2-*BeginX; |
---|
818 | else |
---|
819 | dist=*BeginX+minxim2; |
---|
820 | // And find point at distance dist from the epipole on the given line |
---|
821 | Point2D<double> Im2P(epipole.x()+dx*dist, epipole.y()+dy*dist); |
---|
822 | RectPoints.push_back(TransferPointUsingH(Plane0InvIm2, Im2P.x(), Im2P.y())); |
---|
823 | } |
---|
824 | } |
---|
825 | |
---|
826 | /* |
---|
827 | std::ostream& GeneralPlanarRectify::SaveToStream(std::ostream &s) const { |
---|
828 | s << "General Planar Rectification:" << endl; |
---|
829 | s << epipole << FM << Plane0Im1 << Plane0InvIm1 << Plane0Im2 << Plane0InvIm2; |
---|
830 | s << im1xs << " " << im1ys << " " << im2xs << " " << im2ys << " "; |
---|
831 | s << minang << " " << maxang << " "; |
---|
832 | s << minxim1 << " " << maxxim1 << " " << minxim2 << " " << maxxim2 << " " << flipx << endl; |
---|
833 | return s; |
---|
834 | } |
---|
835 | |
---|
836 | std::istream& GeneralPlanarRectify::LoadFromStream(std::istream &s) { |
---|
837 | string LookFor("General Planar Rectification:"); |
---|
838 | std::string::size_type LForSize=LookFor.size(); |
---|
839 | char Temp[LForSize]; |
---|
840 | int off=s.tellg(); |
---|
841 | s.read(Temp, LForSize); |
---|
842 | if (s.eof()) |
---|
843 | s.seekg(off, std::ios::beg); |
---|
844 | |
---|
845 | if (LookFor.compare(Temp)!=0) { |
---|
846 | s.seekg(-((int)LForSize), std::ios::cur); |
---|
847 | s.setstate(ios_base::badbit); |
---|
848 | return s; |
---|
849 | } |
---|
850 | s >> epipole >> FM >> Plane0Im1 >> Plane0InvIm1 >> Plane0Im2 >> Plane0InvIm2 >> im1xs >> im1ys >> im2xs >> im2ys >> minang >> maxang >> minxim1 >> maxxim1 >> minxim2 >> maxxim2 >> flipx; s.ignore(1); |
---|
851 | SetUpTablesAndXBounds(); |
---|
852 | return s; |
---|
853 | } |
---|
854 | */ |
---|
855 | |
---|
856 | } // namespace Rectify |
---|