Code cleanup: silence some -Wnarrowing warnings from C++11
[blender.git] / extern / libmv / libmv / simple_pipeline / camera_intrinsics.cc
1 // Copyright (c) 2011 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include "libmv/simple_pipeline/camera_intrinsics.h"
22 #include "libmv/numeric/levenberg_marquardt.h"
23
24 namespace libmv {
25
26 struct Offset {
27   short ix, iy;
28   unsigned char fx,fy;
29 };
30
31 struct Grid {
32   struct Offset *offset;
33   int width, height;
34   double overscan;
35 };
36
37 static struct Grid *copyGrid(struct Grid *from)
38 {
39   struct Grid *to = NULL;
40
41   if (from) {
42     to = new Grid;
43
44     to->width = from->width;
45     to->height = from->height;
46     to->overscan = from->overscan;
47
48     to->offset = new Offset[to->width*to->height];
49     memcpy(to->offset, from->offset, sizeof(struct Offset)*to->width*to->height);
50   }
51
52   return to;
53 }
54
55 CameraIntrinsics::CameraIntrinsics()
56     : K_(Mat3::Identity()),
57       image_width_(0),
58       image_height_(0),
59       k1_(0),
60       k2_(0),
61       k3_(0),
62       p1_(0),
63       p2_(0),
64       distort_(0),
65       undistort_(0) {}
66
67 CameraIntrinsics::CameraIntrinsics(const CameraIntrinsics &from)
68     : K_(from.K_),
69       image_width_(from.image_width_),
70       image_height_(from.image_height_),
71       k1_(from.k1_),
72       k2_(from.k2_),
73       k3_(from.k3_),
74       p1_(from.p1_),
75       p2_(from.p2_)
76 {
77   distort_ = copyGrid(from.distort_);
78   undistort_ = copyGrid(from.undistort_);
79 }
80
81 CameraIntrinsics::~CameraIntrinsics() {
82   FreeLookupGrid();
83 }
84
85 /// Set the entire calibration matrix at once.
86 void CameraIntrinsics::SetK(const Mat3 new_k) {
87   K_ = new_k;
88   FreeLookupGrid();
89 }
90
91 /// Set both x and y focal length in pixels.
92 void CameraIntrinsics::SetFocalLength(double focal_x, double focal_y) {
93   K_(0, 0) = focal_x;
94   K_(1, 1) = focal_y;
95   FreeLookupGrid();
96 }
97
98 void CameraIntrinsics::SetPrincipalPoint(double cx, double cy) {
99   K_(0, 2) = cx;
100   K_(1, 2) = cy;
101   FreeLookupGrid();
102 }
103
104 void CameraIntrinsics::SetImageSize(int width, int height) {
105   image_width_ = width;
106   image_height_ = height;
107   FreeLookupGrid();
108 }
109
110 void CameraIntrinsics::SetRadialDistortion(double k1, double k2, double k3) {
111   k1_ = k1;
112   k2_ = k2;
113   k3_ = k3;
114   FreeLookupGrid();
115 }
116
117 void CameraIntrinsics::SetTangentialDistortion(double p1, double p2) {
118   p1_ = p1;
119   p2_ = p2;
120   FreeLookupGrid();
121 }
122
123 void CameraIntrinsics::ApplyIntrinsics(double normalized_x,
124                                        double normalized_y,
125                                        double *image_x,
126                                        double *image_y) const {
127   double x = normalized_x;
128   double y = normalized_y;
129
130   // Apply distortion to the normalized points to get (xd, yd).
131   double r2 = x*x + y*y;
132   double r4 = r2 * r2;
133   double r6 = r4 * r2;
134   double r_coeff = (1 + k1_*r2 + k2_*r4 + k3_*r6);
135   double xd = x * r_coeff + 2*p1_*x*y + p2_*(r2 + 2*x*x);
136   double yd = y * r_coeff + 2*p2_*x*y + p1_*(r2 + 2*y*y);
137
138   // Apply focal length and principal point to get the final image coordinates.
139   *image_x = focal_length_x() * xd + principal_point_x();
140   *image_y = focal_length_y() * yd + principal_point_y();
141 }
142
143 struct InvertIntrinsicsCostFunction {
144  public:
145   typedef Vec2 FMatrixType;
146   typedef Vec2 XMatrixType;
147
148   InvertIntrinsicsCostFunction(const CameraIntrinsics &intrinsics,
149                                double image_x, double image_y)
150     : intrinsics(intrinsics), x(image_x), y(image_y) {}
151
152   Vec2 operator()(const Vec2 &u) const {
153     double xx, yy;
154     intrinsics.ApplyIntrinsics(u(0), u(1), &xx, &yy);
155     Vec2 fx;
156     fx << (xx - x), (yy - y);
157     return fx;
158   }
159   const CameraIntrinsics &intrinsics;
160   double x, y;
161 };
162
163 void CameraIntrinsics::InvertIntrinsics(double image_x,
164                                         double image_y,
165                                         double *normalized_x,
166                                         double *normalized_y) const {
167   // Compute the initial guess. For a camera with no distortion, this will also
168   // be the final answer; the LM iteration will terminate immediately.
169   Vec2 normalized;
170   normalized(0) = (image_x - principal_point_x()) / focal_length_x();
171   normalized(1) = (image_y - principal_point_y()) / focal_length_y();
172
173   typedef LevenbergMarquardt<InvertIntrinsicsCostFunction> Solver;
174
175   InvertIntrinsicsCostFunction intrinsics_cost(*this, image_x, image_y);
176   Solver::SolverParameters params;
177   Solver solver(intrinsics_cost);
178
179   /*Solver::Results results =*/ solver.minimize(params, &normalized);
180
181   // TODO(keir): Better error handling.
182
183   *normalized_x = normalized(0);
184   *normalized_y = normalized(1);
185 }
186
187 // TODO(MatthiasF): downsample lookup
188 template<typename WarpFunction>
189 void CameraIntrinsics::ComputeLookupGrid(Grid* grid, int width, int height, double overscan) {
190   double w = (double)width / (1 + overscan);
191   double h = (double)height / (1 + overscan);
192   double aspx = (double)w / image_width_;
193   double aspy = (double)h / image_height_;
194
195   for (int y = 0; y < height; y++) {
196     for (int x = 0; x < width; x++) {
197       double src_x = (x - 0.5 * overscan * w) / aspx, src_y = (y - 0.5 * overscan * h) / aspy;
198       double warp_x, warp_y;
199       WarpFunction(this,src_x,src_y,&warp_x,&warp_y);
200       warp_x = warp_x*aspx + 0.5 * overscan * w;
201       warp_y = warp_y*aspy + 0.5 * overscan * h;
202       int ix = int(warp_x), iy = int(warp_y);
203       int fx = round((warp_x-ix)*256), fy = round((warp_y-iy)*256);
204       if(fx == 256) { fx=0; ix++; }
205       if(fy == 256) { fy=0; iy++; }
206       // Use nearest border pixel
207       if( ix < 0 ) { ix = 0, fx = 0; }
208       if( iy < 0 ) { iy = 0, fy = 0; }
209       if( ix >= width-2 ) ix = width-2;
210       if( iy >= height-2 ) iy = height-2;
211
212       Offset offset = { (short)(ix-x), (short)(iy-y), (unsigned char)fx, (unsigned char)fy };
213       grid->offset[y*width+x] = offset;
214     }
215   }
216 }
217
218 // TODO(MatthiasF): cubic B-Spline image sampling, bilinear lookup
219 template<typename T,int N>
220 static void Warp(const Grid* grid, const T* src, T* dst,
221                  int width, int height) {
222   for (int y = 0; y < height; y++) {
223     for (int x = 0; x < width; x++) {
224       Offset offset = grid->offset[y*width+x];
225       const T* s = &src[((y+offset.iy)*width+(x+offset.ix))*N];
226       for (int i = 0; i < N; i++) {
227         dst[(y*width+x)*N+i] = ((s[        i] * (256-offset.fx) + s[        N+i] * offset.fx) * (256-offset.fy)
228                                +(s[width*N+i] * (256-offset.fx) + s[width*N+N+i] * offset.fx) * offset.fy) / (256*256);
229       }
230     }
231   }
232 }
233
234 void CameraIntrinsics::FreeLookupGrid() {
235   if(distort_) {
236     delete distort_->offset;
237     delete distort_;
238     distort_ = NULL;
239   }
240
241   if(undistort_) {
242     delete undistort_->offset;
243     delete undistort_;
244     undistort_ = NULL;
245   }
246 }
247
248 // FIXME: C++ templates limitations makes thing complicated, but maybe there is a simpler method.
249 struct ApplyIntrinsicsFunction {
250   ApplyIntrinsicsFunction(CameraIntrinsics* intrinsics, double x, double y,
251                            double *warp_x, double *warp_y) {
252     intrinsics->ApplyIntrinsics(
253           (x-intrinsics->principal_point_x())/intrinsics->focal_length_x(),
254           (y-intrinsics->principal_point_y())/intrinsics->focal_length_y(),
255           warp_x, warp_y);
256   }
257 };
258 struct InvertIntrinsicsFunction {
259   InvertIntrinsicsFunction(CameraIntrinsics* intrinsics, double x, double y,
260                            double *warp_x, double *warp_y) {
261     intrinsics->InvertIntrinsics(x,y,warp_x,warp_y);
262     *warp_x = *warp_x*intrinsics->focal_length_x()+intrinsics->principal_point_x();
263     *warp_y = *warp_y*intrinsics->focal_length_y()+intrinsics->principal_point_y();
264   }
265 };
266
267 void CameraIntrinsics::CheckDistortLookupGrid(int width, int height, double overscan)
268 {
269   if(distort_) {
270     if(distort_->width != width || distort_->height != height || distort_->overscan != overscan) {
271       delete [] distort_->offset;
272       distort_->offset = NULL;
273     }
274   } else {
275     distort_ = new Grid;
276     distort_->offset = NULL;
277   }
278
279   if(!distort_->offset) {
280       distort_->offset = new Offset[width*height];
281       ComputeLookupGrid<InvertIntrinsicsFunction>(distort_,width,height,overscan);
282   }
283
284   distort_->width = width;
285   distort_->height = height;
286   distort_->overscan = overscan;
287 }
288
289 void CameraIntrinsics::CheckUndistortLookupGrid(int width, int height, double overscan)
290 {
291   if(undistort_) {
292     if(undistort_->width != width || undistort_->height != height || undistort_->overscan != overscan) {
293       delete [] undistort_->offset;
294       undistort_->offset = NULL;
295     }
296   } else {
297     undistort_ = new Grid;
298     undistort_->offset = NULL;
299   }
300
301   if(!undistort_->offset) {
302       undistort_->offset = new Offset[width*height];
303       ComputeLookupGrid<ApplyIntrinsicsFunction>(undistort_,width,height,overscan);
304   }
305
306   undistort_->width = width;
307   undistort_->height = height;
308   undistort_->overscan = overscan;
309 }
310
311 void CameraIntrinsics::Distort(const float* src, float* dst, int width, int height, double overscan, int channels) {
312   CheckDistortLookupGrid(width, height, overscan);
313        if(channels==1) Warp<float,1>(distort_,src,dst,width,height);
314   else if(channels==2) Warp<float,2>(distort_,src,dst,width,height);
315   else if(channels==3) Warp<float,3>(distort_,src,dst,width,height);
316   else if(channels==4) Warp<float,4>(distort_,src,dst,width,height);
317   //else assert("channels must be between 1 and 4");
318 }
319
320 void CameraIntrinsics::Distort(const unsigned char* src, unsigned char* dst, int width, int height, double overscan, int channels) {
321   CheckDistortLookupGrid(width, height, overscan);
322        if(channels==1) Warp<unsigned char,1>(distort_,src,dst,width,height);
323   else if(channels==2) Warp<unsigned char,2>(distort_,src,dst,width,height);
324   else if(channels==3) Warp<unsigned char,3>(distort_,src,dst,width,height);
325   else if(channels==4) Warp<unsigned char,4>(distort_,src,dst,width,height);
326   //else assert("channels must be between 1 and 4");
327 }
328
329 void CameraIntrinsics::Undistort(const float* src, float* dst, int width, int height, double overscan, int channels) {
330   CheckUndistortLookupGrid(width, height, overscan);
331        if(channels==1) Warp<float,1>(undistort_,src,dst,width,height);
332   else if(channels==2) Warp<float,2>(undistort_,src,dst,width,height);
333   else if(channels==3) Warp<float,3>(undistort_,src,dst,width,height);
334   else if(channels==4) Warp<float,4>(undistort_,src,dst,width,height);
335   //else assert("channels must be between 1 and 4");
336 }
337
338 void CameraIntrinsics::Undistort(const unsigned char* src, unsigned char* dst, int width, int height, double overscan, int channels) {
339   CheckUndistortLookupGrid(width, height, overscan);
340        if(channels==1) Warp<unsigned char,1>(undistort_,src,dst,width,height);
341   else if(channels==2) Warp<unsigned char,2>(undistort_,src,dst,width,height);
342   else if(channels==3) Warp<unsigned char,3>(undistort_,src,dst,width,height);
343   else if(channels==4) Warp<unsigned char,4>(undistort_,src,dst,width,height);
344   //else assert("channels must be between 1 and 4");
345 }
346
347 std::ostream& operator <<(std::ostream &os,
348                           const CameraIntrinsics &intrinsics) {
349   if (intrinsics.focal_length_x() == intrinsics.focal_length_x()) {
350     os << "f=" << intrinsics.focal_length();
351   } else {
352     os <<  "fx=" << intrinsics.focal_length_x()
353        << " fy=" << intrinsics.focal_length_y();
354   }
355   os << " cx=" << intrinsics.principal_point_x()
356      << " cy=" << intrinsics.principal_point_y()
357      << " w=" << intrinsics.image_width()
358      << " h=" << intrinsics.image_height();
359
360   if (intrinsics.k1() != 0.0) { os << " k1=" << intrinsics.k1(); }
361   if (intrinsics.k2() != 0.0) { os << " k2=" << intrinsics.k2(); }
362   if (intrinsics.k3() != 0.0) { os << " k3=" << intrinsics.k3(); }
363   if (intrinsics.p1() != 0.0) { os << " p1=" << intrinsics.p1(); }
364   if (intrinsics.p2() != 0.0) { os << " p2=" << intrinsics.p2(); }
365
366   return os;
367 }
368
369 }  // namespace libmv