Fully implement undistort

src/main/java/org/photonvision/mrcal/MrCalJNI.java

@@ -25,7 +25,6 @@
 import org.opencv.core.MatOfPoint2f;
 
 public class MrCalJNI {
-
     public static class MrCalResult {
         public boolean success;
         public double[] intrinsics;
@@ -63,12 +62,12 @@ public static native MrCalResult mrcal_calibrate_camera(
             int boardWidth, int boardHeight, double boardSpacing,
             int imageWidth, int imageHeight, double focalLen);
 
+    public static native boolean undistort_mrcal(long srcMat, long dstMat, long cameraMat, long distCoeffsMat, int lensModelOrdinal, int order, int Nx, int Ny, int fov_x_deg);
 
     public static MrCalResult calibrateCamera(
             List<MatOfPoint2f> board_corners,
             int boardWidth, int boardHeight, double boardSpacing,
             int imageWidth, int imageHeight, double focalLen) {
-
         double[] observations = new double[boardWidth * boardHeight * 3 * board_corners.size()];
 
         int i = 0;

src/mrcal_jni.cpp

@@ -194,3 +194,23 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera
 
   return ret;
 }
+
+/*
+ * Class:     org_photonvision_mrcal_MrCalJNI_undistort
+ * Method:    1mrcal
+ * Signature: (JJJJIIIII)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_org_photonvision_mrcal_MrCalJNI_undistort_1mrcal
+  (JNIEnv *, jclass, jlong srcMat, jlong dstMat, jlong camMat, jlong distCoeffs,
+   jint lensModelOrdinal, jint order, jint Nx, jint Ny, jint fov_x_deg)
+{
+
+  return undistort_mrcal(
+      reinterpret_cast<cv::Mat *>(srcMat), reinterpret_cast<cv::Mat *>(dstMat),
+      reinterpret_cast<cv::Mat *>(camMat),
+      reinterpret_cast<cv::Mat *>(distCoeffs),
+      static_cast<CameraLensModel>(lensModelOrdinal),
+      static_cast<uint16_t>(order), static_cast<uint16_t>(Nx),
+      static_cast<uint16_t>(Ny), static_cast<uint16_t>(fov_x_deg));
+}

src/mrcal_jni.h

@@ -20,8 +20,8 @@
 
 /* Header for class MrCalJNI */
 
-#ifndef MRCAL_JAVA_MRCAL_JNI_H_
-#define MRCAL_JAVA_MRCAL_JNI_H_
+#ifndef MRCAL_JAVA_SRC_MRCAL_JNI_H_
+#define MRCAL_JAVA_SRC_MRCAL_JNI_H_
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -34,7 +34,17 @@ JNIEXPORT jobject JNICALL
 Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera(
     JNIEnv *, jclass, jdoubleArray, jint, jint, jdouble, jint, jint, jdouble);
 
+/*
+ * Class:     org_photonvision_mrcal_MrCalJNI
+ * Method:    undistort_mrcal
+ * Signature: (JJJJI)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_org_photonvision_mrcal_MrCalJNI_undistort_1mrcal(JNIEnv *, jclass, jlong,
+                                                      jlong, jlong, jlong, jint,
+                                                      jint, jint, jint, jint);
+
 #ifdef __cplusplus
 } // extern "C"
 #endif
-#endif // MRCAL_JAVA_MRCAL_JNI_H_
+#endif // MRCAL_JAVA_SRC_MRCAL_JNI_H_

src/mrcal_wrapper.cpp

@@ -231,7 +231,7 @@ std::unique_ptr<mrcal_result> mrcal_main(
       Nframes, Npoints, Npoints_fixed, c_observations_board,
       c_observations_point, problem_selections, &mrcal_lensmodel);
 
-  cholmod_sparse* Jt = cholmod_l_allocate_sparse(
+  cholmod_sparse *Jt = cholmod_l_allocate_sparse(
       static_cast<size_t>(Nstate), static_cast<size_t>(Nmeasurements),
       static_cast<size_t>(N_j_nonzero), 1, 1, 0, CHOLMOD_REAL, cctx.cc);
 
@@ -354,66 +354,66 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
 
 mrcal_result::~mrcal_result() {
   cholmod_l_free_sparse(&Jt, cctx.cc);
-  // free(Jt.p);
-  // free(Jt.i);
-  // free(Jt.x);
+  // std::free(Jt.p);
+  // std::free(Jt.i);
+  // std::free(Jt.x);
   return;
 }
 
-bool unproject_mrcal(cv::Mat& src, cv::Mat& dst, cv::Mat& cameraMat, cv::Mat& distCoeffs, CameraLensModel lensModel,
-  // Extra stuff for splined stereographic models
-  uint16_t order,
-  uint16_t Nx,
-  uint16_t Ny,
-  uint16_t fov_x_deg
-) {
-
+bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat,
+                     const cv::Mat *distCoeffs, CameraLensModel lensModel,
+                     // Extra stuff for splined stereographic models
+                     uint16_t order, uint16_t Nx, uint16_t Ny,
+                     uint16_t fov_x_deg) {
+
   mrcal_lensmodel_t mrcal_lensmodel;
   switch (lensModel) {
-    case CameraLensModel::LENSMODEL_OPENCV5:
-      mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV5;
-      break;
-    case CameraLensModel::LENSMODEL_OPENCV8:
-      mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
-      break;
-    case CameraLensModel::LENSMODEL_STEREOGRAPHIC:
-      mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
-      break;
-    case CameraLensModel::LENSMODEL_SPLINED_STEREOGRAPHIC:
-      mrcal_lensmodel.type = MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC;
-
-      /* Maximum degree of each 1D polynomial. This is almost certainly 2 */          
-      mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg = fov_x_deg;
-      /* The horizontal field of view. Not including fov_y. It's proportional with */ 
-      /* Ny and Nx */                                                                 
-      mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order;
-      /* We have a Nx by Ny grid of control points */                                 
-      mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx = Nx;
-      mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny = Ny;
-      break;
-    default:
-      std::cerr << "Unknown lensmodel\n";
-      return false;
+  case CameraLensModel::LENSMODEL_OPENCV5:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV5;
+    break;
+  case CameraLensModel::LENSMODEL_OPENCV8:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
+    break;
+  case CameraLensModel::LENSMODEL_STEREOGRAPHIC:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
+    break;
+  case CameraLensModel::LENSMODEL_SPLINED_STEREOGRAPHIC:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC;
+
+    /* Maximum degree of each 1D polynomial. This is almost certainly 2 */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg =
+        fov_x_deg;
+    /* The horizontal field of view. Not including fov_y. It's proportional with
+     */
+    /* Ny and Nx */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order;
+    /* We have a Nx by Ny grid of control points */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx = Nx;
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny = Ny;
+    break;
+  default:
+    std::cerr << "Unknown lensmodel\n";
+    return false;
   }
 
-  if (!(dst.cols == 2 && src.cols == 2 && dst.rows == src.rows)) {
-      std::cerr << "Bad input array size\n";
-      return false;
+  if (!(dst->cols == 2 && dst->cols == 2 && dst->rows == dst->rows)) {
+    std::cerr << "Bad input array size\n";
+    return false;
   }
-  if (!(dst.type() == CV_64FC2 && src.type() == CV_64FC2)) {
-      std::cerr << "Bad input type -- need CV_64F\n";
-      return false;
+  if (!(dst->type() == CV_64FC2 && dst->type() == CV_64FC2)) {
+    std::cerr << "Bad input type -- need CV_64F\n";
+    return false;
   }
-  if (!(dst.isContinuous() && src.isContinuous())) {
-      std::cerr << "Bad input array -- need continuous\n";
-      return false;
+  if (!(dst->isContinuous() && dst->isContinuous())) {
+    std::cerr << "Bad input array -- need continuous\n";
+    return false;
   }
 
   // extract intrinsics core from opencv camera matrix
-  double fx = cameraMat.at<double>(0, 0);
-  double fy = cameraMat.at<double>(1, 1);
-  double cx = cameraMat.at<double>(0, 2);
-  double cy = cameraMat.at<double>(1, 2);
+  double fx = cameraMat->at<double>(0, 0);
+  double fy = cameraMat->at<double>(1, 1);
+  double cx = cameraMat->at<double>(0, 2);
+  double cy = cameraMat->at<double>(1, 2);
 
   // Core, distortion coefficients concatenated
   int NlensParams = mrcal_lensmodel_num_params(&mrcal_lensmodel);
@@ -422,37 +422,30 @@ bool unproject_mrcal(cv::Mat& src, cv::Mat& dst, cv::Mat& cameraMat, cv::Mat& di
   intrinsics[1] = fy;
   intrinsics[2] = cx;
   intrinsics[3] = cy;
-  for (size_t i = 0; i < distCoeffs.cols; i++) {
-    intrinsics[i + 4] = distCoeffs.at<double>(i);
+  for (size_t i = 0; i < distCoeffs->cols; i++) {
+    intrinsics[i + 4] = distCoeffs->at<double>(i);
   }
 
   // input points in the distorted image pixel coordinates
-  mrcal_point2_t* in = reinterpret_cast<mrcal_point2_t*>(src.data);
+  mrcal_point2_t *in = reinterpret_cast<mrcal_point2_t *>(dst->data);
   // vec3 observation vectors defined up-to-length
-  std::vector<mrcal_point3_t> out(src.rows);
-
+  std::vector<mrcal_point3_t> out(dst->rows);
 
-
-  mrcal_unproject(
-    out.data(), in,
-    src.rows,
-    &mrcal_lensmodel,
-    intrinsics.data()
-  );
+  mrcal_unproject(out.data(), in, dst->rows, &mrcal_lensmodel,
+                  intrinsics.data());
 
   // The output is defined "up-to-length"
   // Let's project through pinhole again
 
   // Output points in pinhole pixel coordinates
-  mrcal_point2_t* pinhole_pts = reinterpret_cast<mrcal_point2_t*>(dst.data);
-
+  mrcal_point2_t *pinhole_pts = reinterpret_cast<mrcal_point2_t *>(dst->data);
 
-  size_t bound = src.rows;
+  size_t bound = dst->rows;
   for (size_t i = 0; i < bound; i++) {
     // from mrcal-project-internal/pinhole model
-    mrcal_point3_t& p = out[i];
+    mrcal_point3_t &p = out[i];
 
-    double z_recip = 1./p.z;
+    double z_recip = 1. / p.z;
     double x = p.x * z_recip;
     double y = p.y * z_recip;
 

src/mrcal_wrapper.h

@@ -29,26 +29,26 @@ extern "C" {
 #include <suitesparse/cholmod_core.h>
 } // extern "C"
 
+#include <memory>
 #include <opencv2/opencv.hpp>
 #include <span>
-#include <vector>
 #include <utility>
-#include <memory>
+#include <vector>
 
 struct mrcal_result {
   bool success;
   std::vector<double> intrinsics;
   double rms_error;
   std::vector<double> residuals;
-  cholmod_sparse* Jt;
+  cholmod_sparse *Jt;
   mrcal_calobject_warp_t calobject_warp;
   int Noutliers_board;
   // TODO standard devs
 
   mrcal_result() = default;
   mrcal_result(bool success_, std::vector<double> intrinsics_,
                double rms_error_, std::vector<double> residuals_,
-               cholmod_sparse* Jt_, mrcal_calobject_warp_t calobject_warp_,
+               cholmod_sparse *Jt_, mrcal_calobject_warp_t calobject_warp_,
                int Noutliers_board_)
       : success{success_}, intrinsics{std::move(intrinsics_)},
         rms_error{rms_error_}, residuals{std::move(residuals_)}, Jt{Jt_},
@@ -79,10 +79,8 @@ enum class CameraLensModel {
   LENSMODEL_SPLINED_STEREOGRAPHIC
 };
 
-bool unproject_mrcal(cv::Mat& src, cv::Mat& dst, cv::Mat& cameraMat, cv::Mat& distCoeffs, CameraLensModel lensModel,
-  // Extra stuff for splined stereographic models
-  uint16_t order,
-  uint16_t Nx,
-  uint16_t Ny,
-  uint16_t fov_x_deg
-);
+bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat,
+                     const cv::Mat *distCoeffs, CameraLensModel lensModel,
+                     // Extra stuff for splined stereographic models
+                     uint16_t order, uint16_t Nx, uint16_t Ny,
+                     uint16_t fov_x_deg);