1 | //-*-c++-*- |
---|
2 | |
---|
3 | #ifndef _GENERALRECTIFYPLANAR_H |
---|
4 | #define _GENERALRECTIFYPLANAR_H |
---|
5 | |
---|
6 | #include <vector> |
---|
7 | #include <math.h> |
---|
8 | #include <utility> |
---|
9 | |
---|
10 | #include "Ops.h" |
---|
11 | |
---|
12 | namespace Rectify { |
---|
13 | |
---|
14 | /*! This class encapsulates the process of rectifying a pair of images using a general rectification method. |
---|
15 | * Order of use is: |
---|
16 | * 1.) Call constructor with image details and some point matches |
---|
17 | * 2.) If desired resample the images with resampleIms to get the actual rectified images |
---|
18 | * 3.) Use RectifyPoint & UnRectifyPoint to move points to and from the resampled images |
---|
19 | */ |
---|
20 | class GeneralPlanarRectify { |
---|
21 | private: |
---|
22 | |
---|
23 | // These variables are for the nonlinear rectification stage |
---|
24 | Geometry::Homg2DPoint<double> epipole; |
---|
25 | // Rectification tables for nonlinear scaling in y |
---|
26 | typedef std::vector<std::pair<double, double> > RectifyTabT; |
---|
27 | typedef std::vector<double> UnRectTabT; |
---|
28 | RectifyTabT RectTab; |
---|
29 | UnRectTabT UnRectTab; |
---|
30 | |
---|
31 | // General Variables |
---|
32 | // Image sizes |
---|
33 | unsigned int im1xs, im1ys, im2xs, im2ys; |
---|
34 | // Max and min angle in the image |
---|
35 | double minang, maxang; |
---|
36 | // X region occupied by rectified images |
---|
37 | double minxim1, maxxim1, minxim2, maxxim2; |
---|
38 | // This variable indicates the images should be flipped about the x axis |
---|
39 | bool flipx; |
---|
40 | |
---|
41 | // For the compatible homographies stuff |
---|
42 | // The fundamental matrix |
---|
43 | const MultiViewGeom::FMatrix<double> &FM; |
---|
44 | // Homography representing the zero disparity plane |
---|
45 | // This transfers points from the given image to the rectify image (whichever |
---|
46 | // that is). |
---|
47 | // For the rectify image these will be the identity matrix |
---|
48 | MatVec::Matrix<double> Plane0Im1, Plane0InvIm1; |
---|
49 | MatVec::Matrix<double> Plane0Im2, Plane0InvIm2; |
---|
50 | |
---|
51 | // Add two angles with appropriate wrap around at 360 degrees |
---|
52 | double addtoangle(double angle, const double addon) const; |
---|
53 | |
---|
54 | // -------------------------------------------- |
---|
55 | // Functions operating in the unrectified image |
---|
56 | // -------------------------------------------- |
---|
57 | |
---|
58 | // Get the distance of the epipole from the image centre (pass image x and y sizes) |
---|
59 | // Returns DBL_MAX if it is an infinite epipole |
---|
60 | double GetEpiDist(const Geometry::Homg2DPoint<double> &e, const unsigned int xs, const unsigned int ys) const; |
---|
61 | // Gets an oriented epipolar line from a point (+ve scale factor only). Works for infinite epipole |
---|
62 | Geometry::Line2D<double> GetOrientELine( |
---|
63 | const Geometry::Homg2DPoint<double> &Point, const Geometry::Homg2DPoint<double> &e) const; |
---|
64 | // Transfer points using a homography |
---|
65 | Geometry::Point2D<double> TransferPointUsingH(const MatVec::Matrix<double> &H, const double &px, const double &py) const; |
---|
66 | // Transfer a line using a homography. |
---|
67 | Geometry::Line2D<double> TransferLineUsingH(const MatVec::Matrix<double> &H, const Geometry::Line2D<double> &Line) const; |
---|
68 | |
---|
69 | // ------------------------------------------- |
---|
70 | // Functions operating in the nonlinear image. |
---|
71 | // ------------------------------------------- |
---|
72 | // Get the angle a particular point makes with the given epipole |
---|
73 | double GetPointAngle(const Geometry::Point2D<double> &Point) const; |
---|
74 | // Get the distance of a point from an epipole. |
---|
75 | double GetDist(const Geometry::Point2D<double> &Point) const; |
---|
76 | // Get an epipolar line from an angle and the epipole |
---|
77 | Geometry::Line2D<double> GetEpipolarLineFromAng(const double angle) const; |
---|
78 | // Get the point in the nonlinear image given a distance and angle related to the epipole |
---|
79 | Geometry::Point2D<double> GetPointFromDistAndAng(const double dist, const double angle) const; |
---|
80 | |
---|
81 | // Convert a point in the nonlinear image to rectified y line (y) via look up table |
---|
82 | // and distance from epipole (x) |
---|
83 | Geometry::Point2D<double> RectPointNLin(const Geometry::Point2D<double> &Point) const; |
---|
84 | // UnRectify a point in the nonlinear image. Converts from y line via look up table |
---|
85 | // and distance from epipole (x) to a point in the nonlinear image |
---|
86 | Geometry::Point2D<double> UnRectPointNLin(const Geometry::Point2D<double> &Point) const; |
---|
87 | |
---|
88 | // ------------------------------------------------ |
---|
89 | // Major functions that actualy control the process |
---|
90 | // ------------------------------------------------ |
---|
91 | |
---|
92 | // Sets up the rectify table bounds |
---|
93 | void SetUpTablesAndXBounds(void); |
---|
94 | |
---|
95 | // Gets the maximal epipolar lines for the image with the given epipole and image size. Transfers these to nonlinear image using the given homography H. H need not be oriented. Uses the epipole, epipole which must be valid before calling. |
---|
96 | void GetImAngleRange(const unsigned int xs, const unsigned int ys, double &minang, double &maxang, const MatVec::Matrix<double> &H) const; |
---|
97 | |
---|
98 | //! mask of bits to & with your data to get the red channel |
---|
99 | static const unsigned int redmask=255; |
---|
100 | //! mask of bits to & with your data to get the green channel (don't forget to shift right 8 bits afterwards) |
---|
101 | static const unsigned int greenmask=255<<8; |
---|
102 | //! mask of bits to & with your data to get the blue channel (don't forget to shift right 16 bits afterwards) |
---|
103 | static const unsigned int bluemask=255<<16; |
---|
104 | |
---|
105 | // Get a sub-pixel co-ordinate in an image, using bilinear interpolation to determine a value |
---|
106 | template <class ImageT, class FracT> |
---|
107 | typename ImageT::value_type subpix_Bilinear(const ImageT &image, FracT x, FracT y) const { |
---|
108 | |
---|
109 | typedef typename ImageT::value_type T; |
---|
110 | |
---|
111 | int ix = int(x); // integral parts |
---|
112 | int iy = int(y); |
---|
113 | FracT fx = x - ix; // fractional parts |
---|
114 | FracT fy = y - iy; |
---|
115 | T c00 = image(ix, iy); // 4 corners of rectangle |
---|
116 | T c01 = image(ix, iy+1); |
---|
117 | T c10 = image(ix+1, iy); |
---|
118 | T c11 = image(ix+1, iy+1); |
---|
119 | |
---|
120 | if (!image.isColour()) { |
---|
121 | // interpolate along y |
---|
122 | FracT c0 = c00 + fy * (c01 - c00); |
---|
123 | FracT c1 = c10 + fy * (c11 - c10); |
---|
124 | // interpolate along x |
---|
125 | return (T)(c0 + fx * (c1 - c0)); |
---|
126 | } |
---|
127 | else |
---|
128 | { |
---|
129 | // Do red, green and blue seperately |
---|
130 | FracT c00_, c01_, c11_, c10_, c0, c1; |
---|
131 | T rval; |
---|
132 | |
---|
133 | // Red |
---|
134 | c00_=(c00 & redmask); c01_=(c01 & redmask); |
---|
135 | c11_=(c11 & redmask); c10_=(c10 & redmask); |
---|
136 | c0 = c00_ + fy * (c01_ - c00_); |
---|
137 | c1 = c10_ + fy * (c11_ - c10_); |
---|
138 | rval=(T)(c0 + fx * (c1 - c0)); |
---|
139 | |
---|
140 | // Green |
---|
141 | c00_=(c00 & greenmask)>>8; c01_=(c01 & greenmask)>>8; |
---|
142 | c11_=(c11 & greenmask)>>8; c10_=(c10 & greenmask)>>8; |
---|
143 | c0 = c00_ + fy * (c01_ - c00_); |
---|
144 | c1 = c10_ + fy * (c11_ - c10_); |
---|
145 | rval=rval | ((T)(c0 + fx * (c1 - c0)))<<8; |
---|
146 | |
---|
147 | // Blue |
---|
148 | c00_=(c00 & bluemask)>>16; c01_=(c01 & bluemask)>>16; |
---|
149 | c11_=(c11 & bluemask)>>16; c10_=(c10 & bluemask)>>16; |
---|
150 | c0 = c00_ + fy * (c01_ - c00_); |
---|
151 | c1 = c10_ + fy * (c11_ - c10_); |
---|
152 | rval=rval | ((T)(c0 + fx * (c1 - c0)))<<16; |
---|
153 | |
---|
154 | return rval; |
---|
155 | } |
---|
156 | } |
---|
157 | |
---|
158 | /// Rectifies the image in ImIn to ImOut. minx, maxx give dimensions of rectified image. Uses rectify table to give height of the image. |
---|
159 | template <class ImageT1, class ImageT2> |
---|
160 | void ResampleAndRectify( |
---|
161 | const ImageT1 &InIm, ImageT2 &OutIm, |
---|
162 | const double minx, const double maxx, const bool flip, |
---|
163 | const MatVec::Matrix<double> &Trans, |
---|
164 | const typename ImageT2::value_type bound) { |
---|
165 | |
---|
166 | // Original unrectified image dimensions |
---|
167 | size_t inxs=InIm.xsize(), inys=InIm.ysize(); |
---|
168 | // Output image dimensions |
---|
169 | size_t outxs=(unsigned int)fabs(maxx-minx), outys=(unsigned int)UnRectTab.size(); |
---|
170 | |
---|
171 | OutIm.resize(outxs, outys, InIm.isColour()); |
---|
172 | |
---|
173 | Geometry::Point2D<double> start, end; |
---|
174 | double dx,dy,norm,inx,iny; |
---|
175 | unsigned int outx; |
---|
176 | Geometry::Point2D<double> TPoint; |
---|
177 | for (unsigned int outy=0; outy<outys; ++outy) |
---|
178 | { |
---|
179 | // Get start and end of this epipolar line in images |
---|
180 | start=GetPointFromDistAndAng(minx, UnRectTab[outy]); |
---|
181 | end=GetPointFromDistAndAng(maxx, UnRectTab[outy]); |
---|
182 | |
---|
183 | // If flipping the image swap start and end |
---|
184 | if (flip) { |
---|
185 | Geometry::Point2D<double> temp(start); |
---|
186 | start=end; |
---|
187 | end=temp; |
---|
188 | } |
---|
189 | |
---|
190 | // Now, work from start to end according to the gradient. |
---|
191 | // Normalise the gradient to have eucl. norm 1 - hence go 1 pixel at a time. |
---|
192 | dx=end.x()-start.x(); dy=end.y()-start.y(); |
---|
193 | norm=1.0/hypot(dx,dy); |
---|
194 | dx=dx*norm; dy=dy*norm; |
---|
195 | inx=start.x(); iny=start.y(); |
---|
196 | for (outx=0;outx<outxs;++outx, inx+=dx, iny+=dy) |
---|
197 | { |
---|
198 | TPoint.x()=inx; TPoint.y()=iny; |
---|
199 | TPoint=TransferPointUsingH(Trans, TPoint.x(), TPoint.y()); |
---|
200 | // -2 because of interpolation |
---|
201 | const double tx=TPoint.x(), ty=TPoint.y(); |
---|
202 | if (tx>=0 && tx<=inxs-2 && ty>=0 && ty<=inys-2) |
---|
203 | OutIm(outx, outy)=subpix_Bilinear(InIm, tx, ty); |
---|
204 | else |
---|
205 | OutIm(outx, outy)=bound; |
---|
206 | } |
---|
207 | } |
---|
208 | } |
---|
209 | |
---|
210 | public: |
---|
211 | |
---|
212 | //------------------------------------- |
---|
213 | // Public functions for use by the user |
---|
214 | //------------------------------------- |
---|
215 | /** Rectify any pair of images sharing common features using a general planar technique. To actually rectify a pair of images you'll want to call resampleIms afterwards. There is a seperation so you can recreate this object when you just want to rectify or unrectify a few points. |
---|
216 | |
---|
217 | {\bf On Entry:} |
---|
218 | \begin{itemize} |
---|
219 | \item{\em im1xs, im1ys, im2xs, im2ys: } The dimensions of the two images to rectify |
---|
220 | \item{\em FM: } The fundamental matrix for images 1,2. |
---|
221 | \item{\em Matches: } The point matches used to calcuate the fundamental matrix. In fact any set of point matches that are nicely distributed, outlier free and fit the epipolar geometry will do - but who gives a shit. These will be hartley sturm corrected so don't feel the need to do that |
---|
222 | \item{\em NLinY: } A bool, which if set to true indicates to use nonlinear scaling on the y axis of the rectified images. This nonlinear scaling is such that there will be no pixel compression and the rectified image will be as small as possible. If there are infinite epipoles this method will not be used regardless (isn't necessary for infinite epipoles anyway). |
---|
223 | \end{itemize} |
---|
224 | |
---|
225 | {\bf On Exit:} |
---|
226 | \begin{itemize} |
---|
227 | \item{\em OutIm1, OutIm2: } 2 Output images. Can not be the same as Im1, Im2. There is no need to set them up to any size or bits per pixel. |
---|
228 | \end{itemize} |
---|
229 | |
---|
230 | {\bf Errors:} |
---|
231 | \begin{itemize} |
---|
232 | \item{\em 1:} Occurs if you try to rectify two non-overlapping images. Although they can theoreticaly be rectified - there is no overlapping section, so what is the point. |
---|
233 | \end{itemize} |
---|
234 | */ |
---|
235 | GeneralPlanarRectify(const unsigned int im1xs, const unsigned int im1ys, |
---|
236 | const unsigned int im2xs, const unsigned int im2ys, |
---|
237 | const MultiViewGeom::FMatrix<double> &FM, |
---|
238 | const std::vector<std::pair<Geometry::Point2D<double>, Geometry::Point2D<double> > > &Matches); |
---|
239 | |
---|
240 | ~GeneralPlanarRectify(); |
---|
241 | |
---|
242 | // Call to resample the images |
---|
243 | // Supply input and output images as well as the value used to indicate there is no valid image (i.e. for the boundary |
---|
244 | // outside non-square images). I like to use int images and -1 - but use whatever you like. |
---|
245 | template <class ImageT, class ImageT2> |
---|
246 | void resampleIms(const ImageT &Im1, const ImageT &Im2, |
---|
247 | ImageT2 &OutIm1, ImageT2 &OutIm2, |
---|
248 | const typename ImageT2::value_type bound) { |
---|
249 | // Output images |
---|
250 | ResampleAndRectify(Im1, OutIm1, minxim1, maxxim1, flipx, Plane0InvIm1, bound); |
---|
251 | ResampleAndRectify(Im2, OutIm2, minxim2, maxxim2, flipx, Plane0InvIm2, bound); |
---|
252 | } |
---|
253 | |
---|
254 | // Rectify the supplied point (x,y). Returns the point in x,y |
---|
255 | void RectifyPointIm1(double &x, double &y) const; |
---|
256 | void RectifyPointIm2(double &x, double &y) const; |
---|
257 | void UnRectifyPointIm1(double &x, double &y) const; |
---|
258 | void UnRectifyPointIm2(double &x, double &y) const; |
---|
259 | |
---|
260 | // Can be used to unrectify a set of points all on the same epipolar line. |
---|
261 | // The epipolar line is given by the y co-ordinate of the rectified line, and the |
---|
262 | // input iterator dereferences to give numbers representing x co-ordinates. |
---|
263 | // RectPoints is added to (not erased). |
---|
264 | // Sorry no proper use of iterators - couldn't stand cluttering up the .h file |
---|
265 | void UnRectifyPointsIm1(std::vector<double> x, const double y, std::vector<Geometry::Point2D<double> > &RectPoints) const; |
---|
266 | void UnRectifyPointsIm2(std::vector<double> x, const double y, std::vector<Geometry::Point2D<double> > &RectPoints) const; |
---|
267 | |
---|
268 | }; |
---|
269 | |
---|
270 | }; |
---|
271 | |
---|
272 | |
---|
273 | #endif |
---|