Merge pull request #1 from PhotonVision/unproject

Undistort using mrcal

.gitignore

@@ -34,3 +34,4 @@
 build/
 cmake_build/*
 .gradle/*
+bin/

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,22 @@ 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_test.cpp

@@ -136,7 +136,6 @@ int homography_test() {
       *std::max_element(stats.residuals.begin(), stats.residuals.end());
 
   if (0) {
-
     std::printf("\n===============================\n\n");
     std::printf("RMS Reprojection Error: %.2f pixels\n", stats.rms_error);
     std::printf("Worst residual (by measurement): %.1f pixels\n", max_error);
@@ -152,8 +151,22 @@ int homography_test() {
     std::printf("\n");
   }
 
+  double fx = 100, fy = 100, cx = 50, cy = 20;
+  cv::Mat1d camMat = (Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
+  cv::Mat1d distCoeffs = (Mat_<double>(1,5) << 0.17802570252202954,-1.461379065131586,0.001019661566461145,0.0003215220840230439,2.7249642067580533);
+
+  undistort_mrcal(&inputs, &outputs, &camMat, &distCoeffs, CameraLensModel::LENSMODEL_OPENCV5, 0, 0, 0, 0);
+
+  cv::Mat2d outputs_opencv = cv::Mat2d::zeros(inputs.size());
+  cv::undistortImagePoints(inputs, outputs_opencv, camMat, distCoeffs,
+                          TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 50,
+                                                      1e-4));
 
-  return true;
+  std::cout << "cam mat\n" << camMat << std::endl;
+  std::cout << "dist\n" << distCoeffs << std::endl;
+  std::cout << "Inputs\n" << inputs << std::endl;
+  std::cout << "Outputs (mrcal)\n" << outputs << std::endl;
+  std::cout << "Outputs (opencv)\n" << outputs_opencv << std::endl;
 }
 
 int main() {

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,8 +354,103 @@ 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 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 */
+    /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order;
+    /* The horizontal field of view. Not including fov_y. It's proportional with
+     * Ny and Nx */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg =
+        fov_x_deg;
+    /* 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 && dst->cols == 2 && dst->rows == dst->rows)) {
+    std::cerr << "Bad input array size\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() && 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);
+
+  // Core, distortion coefficients concatenated
+  int NlensParams = mrcal_lensmodel_num_params(&mrcal_lensmodel);
+  std::vector<double> intrinsics(NlensParams);
+  intrinsics[0] = fx;
+  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);
+  }
+
+  // input points in the distorted image pixel coordinates
+  mrcal_point2_t *in = reinterpret_cast<mrcal_point2_t *>(dst->data);
+  // vec3 observation vectors defined up-to-length
+  std::vector<mrcal_point3_t> out(dst->rows);
+
+  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);
+
+  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];
+
+    double z_recip = 1. / p.z;
+    double x = p.x * z_recip;
+    double y = p.y * z_recip;
+
+    pinhole_pts[i].x = x * fx + cx;
+    pinhole_pts[i].y = y * fy + cy;
+  }
+
+  return true;
+}

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_},
@@ -71,3 +71,16 @@ std::unique_ptr<mrcal_result> mrcal_main(
     cv::Size calobjectSize, double boardSpacing,
     // res, pixels
     cv::Size cameraRes);
+
+enum class CameraLensModel {
+  LENSMODEL_OPENCV5 = 0,
+  LENSMODEL_OPENCV8,
+  LENSMODEL_STEREOGRAPHIC,
+  LENSMODEL_SPLINED_STEREOGRAPHIC
+};
+
+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);