[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [54930] trunk/blender/extern/libmv/libmv/ simple_pipeline: Move radial distortion code to own templated function

[Sergey Sharybin]

Revision: 54930
          http://projects.blender.org/scm/viewvc.php?view=rev&root=bf-blender&revision=54930
Author:   nazgul
Date:     2013-02-28 14:24:17 +0000 (Thu, 28 Feb 2013)
Log Message:
-----------
Move radial distortion code to own templated function

This shall not lead to any functional changes, just
avoids radial distortion code duplicated in camera
intrinsics and bundling code.

For fancier bundle adjustment supprting different
distortion models this is not actually enough and
would need to make some bigger changes, but this
changes makes code a bit easier to maintain already.

Modified Paths:
--------------
    trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc
    trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
    trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h

Modified: trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc
===================================================================
--- trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc	2013-02-28 11:39:13 UTC (rev 54929)
+++ trunk/blender/extern/libmv/libmv/simple_pipeline/bundle.cc	2013-02-28 14:24:17 UTC (rev 54930)
@@ -94,17 +94,16 @@
       // Apply distortion to the normalized points to get (xd, yd).
       // TODO(keir): Do early bailouts for zero distortion; these are expensive
       // jet operations.
-      T r2 = xn*xn + yn*yn;
-      T r4 = r2 * r2;
-      T r6 = r4 * r2;
-      T r_coeff = T(1) + k1*r2 + k2*r4 + k3*r6;
-      T xd = xn * r_coeff + T(2)*p1*xn*yn + p2*(r2 + T(2)*xn*xn);
-      T yd = yn * r_coeff + T(2)*p2*xn*yn + p1*(r2 + T(2)*yn*yn);
 
-      // Apply focal length and principal point to get the final
-      // image coordinates.
-      predicted_x = focal_length * xd + principal_point_x;
-      predicted_y = focal_length * yd + principal_point_y;
+      ApplyRadialDistortionCameraIntrinsics(focal_length,
+                                            focal_length,
+                                            principal_point_x,
+                                            principal_point_y,
+                                            k1, k2, k3,
+                                            p1, p2,
+                                            xn, yn,
+                                            &predicted_x,
+                                            &predicted_y);
     } else {
       predicted_x = xn;
       predicted_y = yn;

Modified: trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
===================================================================
--- trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc	2013-02-28 11:39:13 UTC (rev 54929)
+++ trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc	2013-02-28 14:24:17 UTC (rev 54930)
@@ -124,20 +124,16 @@
                                        double normalized_y,
                                        double *image_x,
                                        double *image_y) const {
-  double x = normalized_x;
-  double y = normalized_y;
-
-  // Apply distortion to the normalized points to get (xd, yd).
-  double r2 = x*x + y*y;
-  double r4 = r2 * r2;
-  double r6 = r4 * r2;
-  double r_coeff = (1 + k1_*r2 + k2_*r4 + k3_*r6);
-  double xd = x * r_coeff + 2*p1_*x*y + p2_*(r2 + 2*x*x);
-  double yd = y * r_coeff + 2*p2_*x*y + p1_*(r2 + 2*y*y);
-
-  // Apply focal length and principal point to get the final image coordinates.
-  *image_x = focal_length_x() * xd + principal_point_x();
-  *image_y = focal_length_y() * yd + principal_point_y();
+  ApplyRadialDistortionCameraIntrinsics(focal_length_x(),
+                                        focal_length_y(),
+                                        principal_point_x(),
+                                        principal_point_y(),
+                                        k1(), k2(), k3(),
+                                        p1(), p2(),
+                                        normalized_x,
+                                        normalized_y,
+                                        image_x,
+                                        image_y);
 }
 
 struct InvertIntrinsicsCostFunction {

Modified: trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h
===================================================================
--- trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h	2013-02-28 11:39:13 UTC (rev 54929)
+++ trunk/blender/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h	2013-02-28 14:24:17 UTC (rev 54930)
@@ -159,6 +159,37 @@
 std::ostream& operator <<(std::ostream &os,
                           const CameraIntrinsics &intrinsics);
 
+// Apply camera intrinsics to the normalized point to get image coordinates.
+// This applies the radial lens distortion to a point which is in normalized
+// camera coordinates (i.e. the principal point is at (0, 0)) to get image
+// coordinates in pixels. Templated for use with autodifferentiation.
+template <typename T>
+inline void ApplyRadialDistortionCameraIntrinsics(T focal_length_x,
+                                                  T focal_length_y,
+                                                  T principal_point_x,
+                                                  T principal_point_y,
+                                                  T k1, T k2, T k3,
+                                                  T p1, T p2,
+                                                  T normalized_x,
+                                                  T normalized_y,
+                                                  T *image_x,
+                                                  T *image_y) {
+  T x = normalized_x;
+  T y = normalized_y;
+
+  // Apply distortion to the normalized points to get (xd, yd).
+  T r2 = x*x + y*y;
+  T r4 = r2 * r2;
+  T r6 = r4 * r2;
+  T r_coeff = (T(1) + k1*r2 + k2*r4 + k3*r6);
+  T xd = x * r_coeff + T(2)*p1*x*y + p2*(r2 + T(2)*x*x);
+  T yd = y * r_coeff + T(2)*p2*x*y + p1*(r2 + T(2)*y*y);
+
+  // Apply focal length and principal point to get the final image coordinates.
+  *image_x = focal_length_x * xd + principal_point_x;
+  *image_y = focal_length_y * yd + principal_point_y;
+}
+
 }  // namespace libmv
 
 #endif  // LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_