From fc5e5fbcf3065e9fd5ff6c1df051a3ea781bacc3 Mon Sep 17 00:00:00 2001 From: Matt M Date: Sun, 3 Mar 2024 10:15:44 -0500 Subject: [PATCH 01/12] Start incremental solve --- src/mrcal_jni.cpp | 5 +- src/mrcal_test.cpp | 18 ++-- src/mrcal_wrapper.cpp | 235 +++++++++++++++++++++++++++++++++--------- 3 files changed, 199 insertions(+), 59 deletions(-) diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index 83581ef..875c7bd 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -153,7 +153,7 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera total_frames_rt_toref.push_back(seed_pose); } - // Convert detection level to weights (we do a little memory manipulation) + // Convert detection level to weights for (auto &o : observations) { double &level = o.z; if (level < 0) { @@ -165,6 +165,9 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize, static_cast(boardSpacing), imagerSize); + if (!statsptr) { + return nullptr; + } mrcal_result &stats = *statsptr; // Find the constructor. Reference: diff --git a/src/mrcal_test.cpp b/src/mrcal_test.cpp index 7caed18..48bcdc5 100644 --- a/src/mrcal_test.cpp +++ b/src/mrcal_test.cpp @@ -152,15 +152,18 @@ int homography_test() { } double fx = 100, fy = 100, cx = 50, cy = 20; - cv::Mat1d camMat = (Mat_(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); - cv::Mat1d distCoeffs = (Mat_(1,5) << 0.17802570252202954,-1.461379065131586,0.001019661566461145,0.0003215220840230439,2.7249642067580533); + cv::Mat1d camMat = (Mat_(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); + cv::Mat1d distCoeffs = + (Mat_(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); + 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)); + cv::undistortImagePoints( + inputs, outputs_opencv, camMat, distCoeffs, + TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 50, 1e-4)); std::cout << "cam mat\n" << camMat << std::endl; std::cout << "dist\n" << distCoeffs << std::endl; @@ -171,7 +174,6 @@ int homography_test() { int main() { // for (int i = 0; i < 1e6; i++) { - homography_test(); + homography_test(); // } - } diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index 6e35954..c1f0a90 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -61,7 +62,7 @@ mrcal_pose_t extrinsics_rt_fromref[0]; // Always zero for single camera, it seems? mrcal_point3_t points[0]; // Seems to always to be None for single camera? -std::unique_ptr mrcal_main( +static std::unique_ptr mrcal_calibrate( // List, depth is ordered array observation[N frames, object_height, // object_width] = [x,y, weight] weight<0 means ignored) std::span observations_board, @@ -70,7 +71,11 @@ std::unique_ptr mrcal_main( // Chessboard size, in corners (not squares) Size calobjectSize, double calibration_object_spacing, // res, pixels - Size cameraRes) { + Size cameraRes, + // solver options + mrcal_problem_selections_t problem_selections, + // seed intrinsics/cameramodel to optimize for + mrcal_lensmodel_t mrcal_lensmodel, std::vector intrinsics) { // Number of board observations we've got. List of boards. in python, it's // (number of chessboard pictures) x (rows) x (cos) x (3) // hard-coded to 8, since that's what I've got below @@ -95,23 +100,6 @@ std::unique_ptr mrcal_main( int Npoints = 0; // seems like this is also unused? whack int Npoints_fixed = 0; // seems like this is also unused? whack - int do_optimize_intrinsics_core = - 1; // basically just non-splined should always be true - int do_optimize_intrinsics_distortions = 1; // can skip intrinsics if we want - int do_optimize_extrinsics = 1; // can skip extrinsics if we want - int do_optimize_frames = 1; - int do_optimize_calobject_warp = 1; - int do_apply_regularization = 1; - int do_apply_outlier_rejection = 1; // can also skip - - mrcal_lensmodel_t mrcal_lensmodel; - mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; // TODO expose other models - - // pure pinhole guess for initial solve - double cx = (cameraRes.width / 2.0) - 0.5; - double cy = (cameraRes.height / 2.0) - 0.5; - std::vector intrinsics = {1200, 1200, cx, cy, 0, 0, 0, 0, 0, 0, 0, 0}; - // Number of cameras to solve for intrinsics int Ncameras_intrinsics = 1; @@ -174,12 +162,6 @@ std::unique_ptr mrcal_main( return ret; } - mrcal_problem_selections_t problem_selections = construct_problem_selections( - do_optimize_intrinsics_core, do_optimize_intrinsics_distortions, - do_optimize_extrinsics, do_optimize_frames, do_optimize_calobject_warp, - do_apply_regularization, do_apply_outlier_rejection, Ncameras_intrinsics, - Ncameras_extrinsics, Nframes, Nobservations_board); - int Nstate = mrcal_num_states( Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, Nobservations_board, problem_selections, &mrcal_lensmodel); @@ -256,38 +238,59 @@ std::unique_ptr mrcal_main( calobject_warp, stats.Noutliers); } -// lifted from mrcal-pywrap.c -static mrcal_problem_selections_t construct_problem_selections( - int do_optimize_intrinsics_core, int do_optimize_intrinsics_distortions, - int do_optimize_extrinsics, int do_optimize_frames, - int do_optimize_calobject_warp, int do_apply_regularization, - int do_apply_outlier_rejection, +struct MrcalSolveOptions { + // If true, we solve for the intrinsics core. Applies only to those models + // that HAVE a core (fx,fy,cx,cy) + int do_optimize_intrinsics_core; + + // If true, solve for the non-core lens parameters + int do_optimize_intrinsics_distortions; + + // If true, solve for the geometry of the cameras + int do_optimize_extrinsics; + + // If true, solve for the poses of the calibration object + int do_optimize_frames; - int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, - int Nobservations_board) { + // If true, optimize the shape of the calibration object + int do_optimize_calobject_warp; + + // If true, apply the regularization terms in the solver + int do_apply_regularization; + + // Whether to try to find NEW outliers. The outliers given on + // input are respected regardless + int do_apply_outlier_rejection; +}; + +// lifted from mrcal-pywrap.c. Restrict a given selection to only valid options +static mrcal_problem_selections_t +construct_problem_selections(MrcalSolveOptions s, int Ncameras_intrinsics, + int Ncameras_extrinsics, int Nframes, + int Nobservations_board) { // By default we optimize everything we can - if (do_optimize_intrinsics_core < 0) - do_optimize_intrinsics_core = Ncameras_intrinsics > 0; - if (do_optimize_intrinsics_distortions < 0) - do_optimize_intrinsics_core = Ncameras_intrinsics > 0; - if (do_optimize_extrinsics < 0) - do_optimize_extrinsics = Ncameras_extrinsics > 0; - if (do_optimize_frames < 0) - do_optimize_frames = Nframes > 0; - if (do_optimize_calobject_warp < 0) - do_optimize_calobject_warp = Nobservations_board > 0; + if (s.do_optimize_intrinsics_core < 0) + s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0; + if (s.do_optimize_intrinsics_distortions < 0) + s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0; + if (s.do_optimize_extrinsics < 0) + s.do_optimize_extrinsics = Ncameras_extrinsics > 0; + if (s.do_optimize_frames < 0) + s.do_optimize_frames = Nframes > 0; + if (s.do_optimize_calobject_warp < 0) + s.do_optimize_calobject_warp = Nobservations_board > 0; return { .do_optimize_intrinsics_core = - static_cast(do_optimize_intrinsics_core), + static_cast(s.do_optimize_intrinsics_core), .do_optimize_intrinsics_distortions = - static_cast(do_optimize_intrinsics_distortions), - .do_optimize_extrinsics = static_cast(do_optimize_extrinsics), - .do_optimize_frames = static_cast(do_optimize_frames), + static_cast(s.do_optimize_intrinsics_distortions), + .do_optimize_extrinsics = static_cast(s.do_optimize_extrinsics), + .do_optimize_frames = static_cast(s.do_optimize_frames), .do_optimize_calobject_warp = - static_cast(do_optimize_calobject_warp), - .do_apply_regularization = static_cast(do_apply_regularization), + static_cast(s.do_optimize_calobject_warp), + .do_apply_regularization = static_cast(s.do_apply_regularization), .do_apply_outlier_rejection = - static_cast(do_apply_outlier_rejection), + static_cast(s.do_apply_outlier_rejection), // .do_apply_regularization_unity_cam01 = false }; } @@ -360,6 +363,138 @@ mrcal_result::~mrcal_result() { return; } +std::unique_ptr mrcal_main( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + Size calobjectSize, double calibration_object_spacing, + // res, pixels + Size cameraRes, double focal_length_guess) { + + std::unique_ptr result; + { + // stereographic initial guess for intrinsics + double cx = (cameraRes.width / 2.0) - 0.5; + double cy = (cameraRes.height / 2.0) - 0.5; + std::vector intrinsics = {focal_length_guess, focal_length_guess, + cx, cy}; + + std::cout << "Initial solve (Geometry only)" << std::endl; + + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = false, + .do_optimize_intrinsics_distortions = false, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = false, + .do_apply_outlier_rejection = false}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + + // And run calibration. This should mutate frames_rt_toref in place + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, intrinsics); + } + + { + std::cout + << "Initial solve (Geometry and LENSMODEL_STEREOGRAPHIC core only)" + << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = false, + .do_apply_outlier_rejection = false}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, result->intrinsics); + } + + { + // Now we've got a seed, expand intrinsics to our target model + // see + // https://github.com/dkogan/mrcal/blob/33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2/mrcal-calibrate-cameras#L533 + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + int nparams = mrcal_lensmodel_num_params(&mrcal_lensmodel); + std::vector intrinsics(nparams); + + // copy core in + std::copy(result->intrinsics.begin(), result->intrinsics.end(), + intrinsics.begin()); + + // and generate random distortion + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-0.5, 0.5); + + std::vector seedDistortions(nparams); + + for (int j = 0; j < nparams; ++j) { + if (j < 5) + seedDistortions[j] = dis(gen) * 2.0 * 1e-6; + else + seedDistortions[j] = dis(gen) * 2.0 * 1e-9; + } + + // copy distortion into our big intrinsics array + std::copy(seedDistortions.begin(), seedDistortions.end(), + intrinsics.begin() + result->intrinsics.size()); + + std::cout << "Optimizing everything from seeded intrinsics" << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = true, + .do_apply_outlier_rejection = true}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, intrinsics); + } + + { + std::cout << "Final, full solve" << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = true, + .do_apply_regularization = true, + .do_apply_outlier_rejection = true}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, result->intrinsics); + } + + return result; +} + 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 From ef8a5ed351a9918be9806bde7c09b7c15a1ac435 Mon Sep 17 00:00:00 2001 From: Matt M Date: Sun, 3 Mar 2024 11:51:30 -0500 Subject: [PATCH 02/12] Fix segfault --- .gitmodules | 3 ++ CMakeLists.txt | 12 +++---- mrcal | 2 +- src/mrcal_jni.cpp | 2 +- src/mrcal_test.cpp | 49 ++++++++++++++--------------- src/mrcal_wrapper.cpp | 73 +++++++++++++++++++++++-------------------- src/mrcal_wrapper.h | 5 ++- vnlog | 1 + 8 files changed, 77 insertions(+), 70 deletions(-) create mode 160000 vnlog diff --git a/.gitmodules b/.gitmodules index 8aba98c..886bf7b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "vcpkg"] path = vcpkg url = https://github.com/microsoft/vcpkg +[submodule "vnlog"] + path = vnlog + url = https://github.com/dkogan/vnlog diff --git a/CMakeLists.txt b/CMakeLists.txt index 974708f..4b7bf25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,17 +8,13 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set (WITH_ASAN OFF) +set (WITH_ASAN ON) if (WITH_ASAN) add_compile_options(-fsanitize=address -g -Wall -fsanitize=undefined) endif () -# add_library(libvnlog SHARED IMPORTED) -# set_target_properties(libvnlog PROPERTIES IMPORTED_LOCATION ${SOURCE_DIR}/libvnlog.so) - - find_package(JNI) if (JNI_FOUND) # Fixes odd AWT dependency @@ -71,7 +67,7 @@ set( src/mrcal_wrapper.cpp src/mrcal_jni.cpp libdogleg/dogleg.c - mrcal/mrcal.c mrcal/cahvore.cc mrcal/poseutils-opencv.c mrcal/poseutils-uses-autodiff.cc mrcal/poseutils.c mrcal/mrcal-opencv.c + mrcal/mrcal.c mrcal/cahvore.cc mrcal/poseutils-opencv.c mrcal/poseutils-uses-autodiff.cc mrcal/poseutils.c mrcal/opencv.c mrcal/triangulation.cc ) @@ -109,11 +105,11 @@ ENDIF() # Test script for checking our linker add_executable(mrcal_jni_test src/mrcal_test.cpp) target_link_libraries(mrcal_jni_test PUBLIC mrcal_jni) -# target_include_directories(mrcal_jni_test PRIVATE ${VNLOG_INCLUDE_DIR}) +target_include_directories(mrcal_jni_test PRIVATE ${PROJECT_SOURCE_DIR}/vnlog) # add_dependencies(mrcal_jni_test ) target_link_libraries(mrcal_jni_test PRIVATE ${OpenCV_LIBS} - # libvnlog + ${PROJECT_SOURCE_DIR}/vnlog/libvnlog.so ) if (WITH_ASAN) diff --git a/mrcal b/mrcal index e667bba..33c3c50 160000 --- a/mrcal +++ b/mrcal @@ -1 +1 @@ -Subproject commit e667bba8cae5f7e2b2688cfa69436cb046348ea6 +Subproject commit 33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2 diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index 875c7bd..289fd72 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -164,7 +164,7 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera } auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize, - static_cast(boardSpacing), imagerSize); + static_cast(boardSpacing), imagerSize, focalLenGuessMM); if (!statsptr) { return nullptr; } diff --git a/src/mrcal_test.cpp b/src/mrcal_test.cpp index 48bcdc5..fa7f4c4 100644 --- a/src/mrcal_test.cpp +++ b/src/mrcal_test.cpp @@ -31,7 +31,7 @@ using namespace cv; extern "C" { -#include +#include "vnlog-parser.h" } // extern "C" struct cmpByFilename { @@ -44,15 +44,14 @@ struct cmpByFilename { }; int homography_test() { - Size boardSize = {7, 7}; - Size imagerSize = {640, 480}; + Size boardSize = {18, 13}; + Size imagerSize = {1600, 1200}; // std::FILE *fp = // std::fopen("/home/matt/github/photon_640_480/corners.vnl", "r"); // Size boardSize = {10, 10}; // Size imagerSize = {1600, 896}; std::FILE *fp = - std::fopen("/home/matt/Documents/GitHub/photonvision/test-resources/" - "calibrationSquaresImg/piCam/640_480_1/corners.vnl", + std::fopen("/home/matt/mrcal_debug_tmp/output_will/corners.vnl", "r"); if (fp == NULL) @@ -125,7 +124,7 @@ int homography_test() { } auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize, - 0.0254, imagerSize); + 0.0254, imagerSize, 1500); auto dt = std::chrono::steady_clock::now() - start; int dt_ms = dt.count(); @@ -151,25 +150,25 @@ int homography_test() { std::printf("\n"); } - double fx = 100, fy = 100, cx = 50, cy = 20; - cv::Mat1d camMat = (Mat_(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); - cv::Mat1d distCoeffs = - (Mat_(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)); - - 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; + // double fx = 100, fy = 100, cx = 50, cy = 20; + // cv::Mat1d camMat = (Mat_(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); + // cv::Mat1d distCoeffs = + // (Mat_(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)); + + // 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() { diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index c1f0a90..b6325f8 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -153,8 +153,8 @@ static std::unique_ptr mrcal_calibrate( int Ncameras_extrinsics = 0; // Seems to always be zero for single camera int Nframes = frames_rt_toref.size(); // Number of pictures of the object we've got - // mrcal_observation_point_triangulated_t *observations_point_triangulated = - // NULL; + mrcal_observation_point_triangulated_t *observations_point_triangulated = + NULL; if (!lensmodel_one_validate_args(&mrcal_lensmodel, intrinsics, false)) { auto ret = std::make_unique(); @@ -168,8 +168,8 @@ static std::unique_ptr mrcal_calibrate( int Nmeasurements = mrcal_num_measurements( Nobservations_board, Nobservations_point, - // observations_point_triangulated, - // 0, // hard-coded to 0 + observations_point_triangulated, + 0, // hard-coded to 0 calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, problem_selections, &mrcal_lensmodel); @@ -200,42 +200,44 @@ static std::unique_ptr mrcal_calibrate( c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, c_observations_board, c_observations_point, Nobservations_board, - Nobservations_point, c_observations_board_pool, &mrcal_lensmodel, + Nobservations_point, + observations_point_triangulated, -1, + c_observations_board_pool, NULL, &mrcal_lensmodel, c_imagersizes, problem_selections, &problem_constants, calibration_object_spacing, calibration_object_width_n, calibration_object_height_n, verbose, false); - // and for fun, evaluate the jacobian - // cholmod_sparse* Jt = NULL; - int N_j_nonzero = _mrcal_num_j_nonzero( - Nobservations_board, Nobservations_point, calibration_object_width_n, - calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, - Nframes, Npoints, Npoints_fixed, c_observations_board, - c_observations_point, problem_selections, &mrcal_lensmodel); - - cholmod_sparse *Jt = cholmod_l_allocate_sparse( - static_cast(Nstate), static_cast(Nmeasurements), - static_cast(N_j_nonzero), 1, 1, 0, CHOLMOD_REAL, cctx.cc); - - // std::printf("Getting jacobian\n"); - if (!mrcal_optimizer_callback( - c_b_packed_final, Nstate * sizeof(double), c_x_final, - Nmeasurements * sizeof(double), Jt, c_intrinsics, c_extrinsics, - c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, - Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, - c_observations_board, c_observations_point, Nobservations_board, - Nobservations_point, c_observations_board_pool, &mrcal_lensmodel, - c_imagersizes, problem_selections, &problem_constants, - calibration_object_spacing, calibration_object_width_n, - calibration_object_height_n, verbose)) { - std::cerr << "callback failed!\n"; - } - // std::cout << "Jacobian! " << std::endl; + // // and for fun, evaluate the jacobian + cholmod_sparse* Jt = NULL; + // int N_j_nonzero = _mrcal_num_j_nonzero( + // Nobservations_board, Nobservations_point, calibration_object_width_n, + // calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, + // Nframes, Npoints, Npoints_fixed, c_observations_board, + // c_observations_point, problem_selections, &mrcal_lensmodel); + + // cholmod_sparse *Jt = cholmod_l_allocate_sparse( + // static_cast(Nstate), static_cast(Nmeasurements), + // static_cast(N_j_nonzero), 1, 1, 0, CHOLMOD_REAL, cctx.cc); + + // // std::printf("Getting jacobian\n"); + // if (!mrcal_optimizer_callback( + // c_b_packed_final, Nstate * sizeof(double), c_x_final, + // Nmeasurements * sizeof(double), Jt, c_intrinsics, c_extrinsics, + // c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, + // Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + // c_observations_board, c_observations_point, Nobservations_board, + // Nobservations_point, c_observations_board_pool, &mrcal_lensmodel, + // c_imagersizes, problem_selections, &problem_constants, + // calibration_object_spacing, calibration_object_width_n, + // calibration_object_height_n, verbose)) { + // std::cerr << "callback failed!\n"; + // } + // // std::cout << "Jacobian! " << std::endl; std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; return std::make_unique( true, intrinsics, stats.rms_reproj_error__pixels, residuals, Jt, - calobject_warp, stats.Noutliers); + calobject_warp, stats.Noutliers_board); } struct MrcalSolveOptions { @@ -375,6 +377,7 @@ std::unique_ptr mrcal_main( Size cameraRes, double focal_length_guess) { std::unique_ptr result; + { // stereographic initial guess for intrinsics double cx = (cameraRes.width / 2.0) - 0.5; @@ -431,6 +434,7 @@ std::unique_ptr mrcal_main( // https://github.com/dkogan/mrcal/blob/33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2/mrcal-calibrate-cameras#L533 mrcal_lensmodel_t mrcal_lensmodel; mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + // num distortion params int nparams = mrcal_lensmodel_num_params(&mrcal_lensmodel); std::vector intrinsics(nparams); @@ -443,9 +447,10 @@ std::unique_ptr mrcal_main( std::mt19937 gen(rd()); std::uniform_real_distribution<> dis(-0.5, 0.5); - std::vector seedDistortions(nparams); + int nDistortion = nparams - 4; + std::vector seedDistortions(nDistortion); - for (int j = 0; j < nparams; ++j) { + for (int j = 0; j < seedDistortions.size(); j++) { if (j < 5) seedDistortions[j] = dis(gen) * 2.0 * 1e-6; else diff --git a/src/mrcal_wrapper.h b/src/mrcal_wrapper.h index 304ee5f..d685783 100644 --- a/src/mrcal_wrapper.h +++ b/src/mrcal_wrapper.h @@ -70,7 +70,10 @@ std::unique_ptr mrcal_main( // Chessboard size, in corners (not squares) cv::Size calobjectSize, double boardSpacing, // res, pixels - cv::Size cameraRes); + cv::Size cameraRes, + // focal length, in pixels + double focal_len_guess + ); enum class CameraLensModel { LENSMODEL_OPENCV5 = 0, diff --git a/vnlog b/vnlog new file mode 160000 index 0000000..be94ea1 --- /dev/null +++ b/vnlog @@ -0,0 +1 @@ +Subproject commit be94ea1348c1c050c38dcaf6ab9b9e054d6d904f From 03eccafc30f3c10a35e5a4c25b6971c0e7bef3b1 Mon Sep 17 00:00:00 2001 From: Matt M Date: Mon, 4 Mar 2024 21:49:01 -0500 Subject: [PATCH 03/12] boop --- CMakeLists.txt | 6 +++--- src/mrcal_test.cpp | 14 ++++++++------ src/mrcal_wrapper.cpp | 21 ++++++++++++++------- 3 files changed, 25 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b7bf25..1428b6f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ option(BUILD_SHARED_LIBS "Build shared libraries" ON) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set (WITH_ASAN ON) +set (WITH_ASAN OFF) if (WITH_ASAN) add_compile_options(-fsanitize=address -g -Wall -fsanitize=undefined) @@ -90,7 +90,7 @@ set( CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib ) # Create shared library add_library(mrcal_jni SHARED ${INCLUDE_HPP} ${SRC_HPP} ${SRC_CPP}) -target_include_directories(mrcal_jni PUBLIC ${JNI_INCLUDE_DIRS} ${OPENCV_INCLUDE_PATH} mrcal libdogleg) +target_include_directories(mrcal_jni SYSTEM PUBLIC ${JNI_INCLUDE_DIRS} ${OPENCV_INCLUDE_PATH} mrcal libdogleg) add_dependencies(mrcal_jni generate_minimath) IF (WIN32) @@ -105,7 +105,7 @@ ENDIF() # Test script for checking our linker add_executable(mrcal_jni_test src/mrcal_test.cpp) target_link_libraries(mrcal_jni_test PUBLIC mrcal_jni) -target_include_directories(mrcal_jni_test PRIVATE ${PROJECT_SOURCE_DIR}/vnlog) +target_include_directories(mrcal_jni_test SYSTEM PRIVATE ${PROJECT_SOURCE_DIR}/vnlog) # add_dependencies(mrcal_jni_test ) target_link_libraries(mrcal_jni_test PRIVATE ${OpenCV_LIBS} diff --git a/src/mrcal_test.cpp b/src/mrcal_test.cpp index fa7f4c4..8b8d23b 100644 --- a/src/mrcal_test.cpp +++ b/src/mrcal_test.cpp @@ -51,7 +51,7 @@ int homography_test() { // Size boardSize = {10, 10}; // Size imagerSize = {1600, 896}; std::FILE *fp = - std::fopen("/home/matt/mrcal_debug_tmp/output_will/corners.vnl", + std::fopen("/home/matt/mrcal_debug_tmp/output_will/images-trimmed/corners.vnl", "r"); if (fp == NULL) @@ -107,9 +107,9 @@ int homography_test() { for (const auto &[key, value] : points) { if (value.size()) { - auto ret = getSeedPose(value.data(), boardSize, imagerSize, 0.0254, 800); + auto ret = getSeedPose(value.data(), boardSize, imagerSize, 0.03, 1200); - if (0) + if (1) std::printf("Seed pose %s: r %f %f %f t %f %f %f\n", key.c_str(), ret.r.x, ret.r.y, ret.r.z, ret.t.x, ret.t.y, ret.t.z); @@ -124,7 +124,7 @@ int homography_test() { } auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize, - 0.0254, imagerSize, 1500); + 0.030, imagerSize, 1200); auto dt = std::chrono::steady_clock::now() - start; int dt_ms = dt.count(); @@ -134,7 +134,7 @@ int homography_test() { double max_error = *std::max_element(stats.residuals.begin(), stats.residuals.end()); - if (0) { + if (1) { 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); @@ -144,7 +144,7 @@ int homography_test() { std::printf("calobject_warp: [%f, %f]\n", stats.calobject_warp.x2, stats.calobject_warp.y2); std::printf("dt, seeding + solve: %f ms\n", dt_ms / 1e6); - std::printf("Intrinsics [%lu]:\n", stats.intrinsics.size()); + std::printf("Intrinsics [%lu]: ", stats.intrinsics.size()); for (auto i : stats.intrinsics) std::printf("%f ", i); std::printf("\n"); @@ -169,6 +169,8 @@ int homography_test() { // std::cout << "Inputs\n" << inputs << std::endl; // std::cout << "Outputs (mrcal)\n" << outputs << std::endl; // std::cout << "Outputs (opencv)\n" << outputs_opencv << std::endl; + + return 0; } int main() { diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index b6325f8..21e3f69 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -350,8 +350,15 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, vector objectPoints3; for (auto a : objectPoints) objectPoints3.push_back(Point3f(a.x, a.y, 0)); + + for (size_t i = 0; i < imagePoints.size(); i++) { + auto o = objectPoints3[i]; + auto im = imagePoints[i]; + std::cout << o << " | " << im << std::endl; + } + solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, - false, SOLVEPNP_EPNP); + false); return mrcal_pose_t{.r = {.x = rvec(0), .y = rvec(1), .z = rvec(2)}, .t = {.x = tvec(0), .y = tvec(1), .z = tvec(2)}}; @@ -385,12 +392,12 @@ std::unique_ptr mrcal_main( std::vector intrinsics = {focal_length_guess, focal_length_guess, cx, cy}; - std::cout << "Initial solve (Geometry only)" << std::endl; + std::cout << "Initial solve (geometry only)" << std::endl; mrcal_problem_selections_t options = construct_problem_selections( {.do_optimize_intrinsics_core = false, .do_optimize_intrinsics_distortions = false, - .do_optimize_extrinsics = true, + .do_optimize_extrinsics = false, .do_optimize_frames = true, .do_optimize_calobject_warp = false, .do_apply_regularization = false, @@ -408,12 +415,12 @@ std::unique_ptr mrcal_main( { std::cout - << "Initial solve (Geometry and LENSMODEL_STEREOGRAPHIC core only)" + << "Initial solve (geometry and LENSMODEL_STEREOGRAPHIC core only)" << std::endl; mrcal_problem_selections_t options = construct_problem_selections( {.do_optimize_intrinsics_core = true, .do_optimize_intrinsics_distortions = true, - .do_optimize_extrinsics = true, + .do_optimize_extrinsics = false, .do_optimize_frames = true, .do_optimize_calobject_warp = false, .do_apply_regularization = false, @@ -461,7 +468,7 @@ std::unique_ptr mrcal_main( std::copy(seedDistortions.begin(), seedDistortions.end(), intrinsics.begin() + result->intrinsics.size()); - std::cout << "Optimizing everything from seeded intrinsics" << std::endl; + std::cout << "Optimizing everything except board warp from seeded intrinsics" << std::endl; mrcal_problem_selections_t options = construct_problem_selections( {.do_optimize_intrinsics_core = true, .do_optimize_intrinsics_distortions = true, @@ -535,7 +542,7 @@ bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat, return false; } - if (!(dst->cols == 2 && dst->cols == 2 && dst->rows == dst->rows)) { + if (!(dst->cols == 2 && dst->cols == 2)) { std::cerr << "Bad input array size\n"; return false; } From a5e559f1dbbab3b75556293412c8d1f74542c04f Mon Sep 17 00:00:00 2001 From: Matt M Date: Mon, 4 Mar 2024 21:56:26 -0500 Subject: [PATCH 04/12] update launch --- .vscode/launch.json | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 1eee601..b097c07 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -5,17 +5,23 @@ "version": "0.2.0", "configurations": [ { - "type": "java", - "name": "MrCalJNI", + "name": "Python debug calibrate cameras", + "type": "debugpy", "request": "launch", - "mainClass": "org.photonvision.mrcal.MrCalJNI", - "projectName": "mrcal-java_a3a3fa78" + "program": "/usr/bin/mrcal-calibrate-cameras", + "console": "integratedTerminal", + "cwd": "/home/matt/mrcal_debug_tmp/output_will/images-trimmed", + "args": [ + "--corners-cache","corners.vnl","--lensmodel","LENSMODEL_OPENCV8","--focal","1200", + "--object-spacing","0.03","--object-width-n","18","--object-height-n","13","*.png" + ], + "justMyCode": false }, { "name": "(gdb) Launch mrcal jni test", "type": "cppdbg", "request": "launch", - "program": "${workspaceFolder}/build/mrcal_jni_test", + "program": "${workspaceFolder}/cmake_build/bin/mrcal_jni_test", "args": [], "stopAtEntry": false, "cwd": "${workspaceFolder}", @@ -57,4 +63,4 @@ ] } ] -} +} \ No newline at end of file From 56a6902642ea7f209ca0b592788b79bef21f4831 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Mon, 4 Mar 2024 22:51:59 -0500 Subject: [PATCH 05/12] Fix row/col mixup --- src/mrcal_jni.cpp | 3 ++- src/mrcal_test.cpp | 23 ++++++++++++----- src/mrcal_wrapper.cpp | 59 ++++++++++++++++++++++++++++++------------- src/mrcal_wrapper.h | 5 ++-- 4 files changed, 61 insertions(+), 29 deletions(-) diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index 289fd72..43e19e5 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -164,7 +164,8 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera } auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize, - static_cast(boardSpacing), imagerSize, focalLenGuessMM); + static_cast(boardSpacing), imagerSize, + focalLenGuessMM); if (!statsptr) { return nullptr; } diff --git a/src/mrcal_test.cpp b/src/mrcal_test.cpp index 8b8d23b..0c70686 100644 --- a/src/mrcal_test.cpp +++ b/src/mrcal_test.cpp @@ -51,8 +51,7 @@ int homography_test() { // Size boardSize = {10, 10}; // Size imagerSize = {1600, 896}; std::FILE *fp = - std::fopen("/home/matt/mrcal_debug_tmp/output_will/images-trimmed/corners.vnl", - "r"); + std::fopen("/home/matt/mrcal_will_debug/imgs/corners.vnl", "r"); if (fp == NULL) return -1; @@ -105,17 +104,20 @@ int homography_test() { observations_board.reserve(total_points); frames_rt_toref.reserve(points.size()); + std::chrono::steady_clock::time_point begin = + std::chrono::steady_clock::now(); + for (const auto &[key, value] : points) { if (value.size()) { auto ret = getSeedPose(value.data(), boardSize, imagerSize, 0.03, 1200); if (1) - std::printf("Seed pose %s: r %f %f %f t %f %f %f\n", key.c_str(), - ret.r.x, ret.r.y, ret.r.z, ret.t.x, ret.t.y, ret.t.z); + // std::printf("Seed pose %s: r %f %f %f t %f %f %f\n", key.c_str(), + // ret.r.x, ret.r.y, ret.r.z, ret.t.x, ret.t.y, ret.t.z); - // Append to the Big List of board corners/levels - observations_board.insert(observations_board.end(), value.begin(), - value.end()); + // Append to the Big List of board corners/levels + observations_board.insert(observations_board.end(), value.begin(), + value.end()); // And list of pose seeds frames_rt_toref.push_back(ret); } else { @@ -123,6 +125,13 @@ int homography_test() { } } + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + std::cout << "Seed took: " + << std::chrono::duration_cast(end - + begin) + .count() + << "[ms]" << std::endl; + auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize, 0.030, imagerSize, 1200); diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index 21e3f69..6f8246f 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -167,8 +167,7 @@ static std::unique_ptr mrcal_calibrate( Nobservations_board, problem_selections, &mrcal_lensmodel); int Nmeasurements = mrcal_num_measurements( - Nobservations_board, Nobservations_point, - observations_point_triangulated, + Nobservations_board, Nobservations_point, observations_point_triangulated, 0, // hard-coded to 0 calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, @@ -200,15 +199,13 @@ static std::unique_ptr mrcal_calibrate( c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, c_observations_board, c_observations_point, Nobservations_board, - Nobservations_point, - observations_point_triangulated, -1, - c_observations_board_pool, NULL, &mrcal_lensmodel, - c_imagersizes, problem_selections, &problem_constants, - calibration_object_spacing, calibration_object_width_n, - calibration_object_height_n, verbose, false); + Nobservations_point, observations_point_triangulated, -1, + c_observations_board_pool, NULL, &mrcal_lensmodel, c_imagersizes, + problem_selections, &problem_constants, calibration_object_spacing, + calibration_object_width_n, calibration_object_height_n, verbose, false); // // and for fun, evaluate the jacobian - cholmod_sparse* Jt = NULL; + cholmod_sparse *Jt = NULL; // int N_j_nonzero = _mrcal_num_j_nonzero( // Nobservations_board, Nobservations_point, calibration_object_width_n, // calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, @@ -326,12 +323,12 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, double cy = (imagerSize.height / 2.0) - 0.5; vector objectPoints; - vector imagePoints; + vector imagePoints; // Fill in object/image points for (int i = 0; i < boardSize.height; i++) { for (int j = 0; j < boardSize.width; j++) { - auto &corner = c_observations_board_pool[i * boardSize.height + j]; + auto &corner = c_observations_board_pool[i * boardSize.width + j]; // weight<0 means ignored -- filter these out if (corner.z >= 0) { imagePoints.emplace_back(corner.x, corner.y); @@ -342,6 +339,31 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, } } + { + // convert from stereographic to pinhole to match python + std::vector mrcal_imagepts(imagePoints.size()); + std::transform( + imagePoints.begin(), imagePoints.end(), mrcal_imagepts.begin(), + [](const auto &pt) { return mrcal_point2_t{.x = pt.x, .y = pt.y}; }); + + mrcal_lensmodel_t model{.type = MRCAL_LENSMODEL_STEREOGRAPHIC}; + std::vector out(imagePoints.size()); + const double intrinsics[] = {fx, fy, cx, cy}; + bool ret = mrcal_unproject(out.data(), mrcal_imagepts.data(), + mrcal_imagepts.size(), &model, intrinsics); + if (!ret) { + std::cerr << "couldn't unproject!" << std::endl; + } + model = {.type = MRCAL_LENSMODEL_PINHOLE}; + mrcal_project(mrcal_imagepts.data(), NULL, NULL, out.data(), out.size(), + &model, intrinsics); + + std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(), + imagePoints.begin(), [](const auto &pt) { + return Point2d{pt.x, pt.y}; + }); + } + // Initial guess at intrinsics Mat cameraMatrix = (Mat_(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); Mat distCoeffs = Mat(4, 1, CV_64FC1, Scalar(0)); @@ -351,14 +373,13 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, for (auto a : objectPoints) objectPoints3.push_back(Point3f(a.x, a.y, 0)); - for (size_t i = 0; i < imagePoints.size(); i++) { - auto o = objectPoints3[i]; - auto im = imagePoints[i]; - std::cout << o << " | " << im << std::endl; - } + // for (auto& o : objectPoints) std::cout << o << std::endl; + // for (auto& i : imagePoints) std::cout << i << std::endl; + // std::cout << "cam mat\n" << cameraMatrix << std::endl; + // std::cout << "distortion: " << distCoeffs << std::endl; solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, - false); + false, SOLVEPNP_ITERATIVE); return mrcal_pose_t{.r = {.x = rvec(0), .y = rvec(1), .z = rvec(2)}, .t = {.x = tvec(0), .y = tvec(1), .z = tvec(2)}}; @@ -468,7 +489,9 @@ std::unique_ptr mrcal_main( std::copy(seedDistortions.begin(), seedDistortions.end(), intrinsics.begin() + result->intrinsics.size()); - std::cout << "Optimizing everything except board warp from seeded intrinsics" << std::endl; + std::cout + << "Optimizing everything except board warp from seeded intrinsics" + << std::endl; mrcal_problem_selections_t options = construct_problem_selections( {.do_optimize_intrinsics_core = true, .do_optimize_intrinsics_distortions = true, diff --git a/src/mrcal_wrapper.h b/src/mrcal_wrapper.h index d685783..e32ba62 100644 --- a/src/mrcal_wrapper.h +++ b/src/mrcal_wrapper.h @@ -70,10 +70,9 @@ std::unique_ptr mrcal_main( // Chessboard size, in corners (not squares) cv::Size calobjectSize, double boardSpacing, // res, pixels - cv::Size cameraRes, + cv::Size cameraRes, // focal length, in pixels - double focal_len_guess - ); + double focal_len_guess); enum class CameraLensModel { LENSMODEL_OPENCV5 = 0, From f03ad580cca7062665077f7c7d0d94f7242858e5 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Mon, 4 Mar 2024 22:57:51 -0500 Subject: [PATCH 06/12] Downgrade mrcal to v2.4.1 --- CMakeLists.txt | 2 +- mrcal | 2 +- src/mrcal_wrapper.cpp | 16 +++++++++------- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1428b6f..b8cd135 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,7 +67,7 @@ set( src/mrcal_wrapper.cpp src/mrcal_jni.cpp libdogleg/dogleg.c - mrcal/mrcal.c mrcal/cahvore.cc mrcal/poseutils-opencv.c mrcal/poseutils-uses-autodiff.cc mrcal/poseutils.c mrcal/opencv.c mrcal/triangulation.cc + mrcal/mrcal.c mrcal/cahvore.cc mrcal/poseutils-opencv.c mrcal/poseutils-uses-autodiff.cc mrcal/poseutils.c mrcal/poseutils-opencv.c mrcal/mrcal-opencv.c mrcal/triangulation.cc ) diff --git a/mrcal b/mrcal index 33c3c50..0d5426b 160000 --- a/mrcal +++ b/mrcal @@ -1 +1 @@ -Subproject commit 33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2 +Subproject commit 0d5426b5851be80dd8e51470a0784a73565a3006 diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index 6f8246f..ce86ad4 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -153,8 +153,8 @@ static std::unique_ptr mrcal_calibrate( int Ncameras_extrinsics = 0; // Seems to always be zero for single camera int Nframes = frames_rt_toref.size(); // Number of pictures of the object we've got - mrcal_observation_point_triangulated_t *observations_point_triangulated = - NULL; + // mrcal_observation_point_triangulated_t *observations_point_triangulated = + // NULL; if (!lensmodel_one_validate_args(&mrcal_lensmodel, intrinsics, false)) { auto ret = std::make_unique(); @@ -167,8 +167,9 @@ static std::unique_ptr mrcal_calibrate( Nobservations_board, problem_selections, &mrcal_lensmodel); int Nmeasurements = mrcal_num_measurements( - Nobservations_board, Nobservations_point, observations_point_triangulated, - 0, // hard-coded to 0 + Nobservations_board, Nobservations_point, + // observations_point_triangulated, + // 0, // hard-coded to 0 calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, problem_selections, &mrcal_lensmodel); @@ -199,8 +200,9 @@ static std::unique_ptr mrcal_calibrate( c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, c_observations_board, c_observations_point, Nobservations_board, - Nobservations_point, observations_point_triangulated, -1, - c_observations_board_pool, NULL, &mrcal_lensmodel, c_imagersizes, + Nobservations_point, + // observations_point_triangulated, -1, + c_observations_board_pool, &mrcal_lensmodel, c_imagersizes, problem_selections, &problem_constants, calibration_object_spacing, calibration_object_width_n, calibration_object_height_n, verbose, false); @@ -234,7 +236,7 @@ static std::unique_ptr mrcal_calibrate( std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; return std::make_unique( true, intrinsics, stats.rms_reproj_error__pixels, residuals, Jt, - calobject_warp, stats.Noutliers_board); + calobject_warp, stats.Noutliers); } struct MrcalSolveOptions { From fee5e6e8a9823d3dd2fbf35a44127984ffcf4819 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Mon, 4 Mar 2024 23:02:50 -0500 Subject: [PATCH 07/12] remove dead code --- src/mrcal_wrapper.cpp | 38 ++++---------------------------------- src/mrcal_wrapper.h | 6 ++---- 2 files changed, 6 insertions(+), 38 deletions(-) diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index ce86ad4..5badd83 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -167,7 +167,7 @@ static std::unique_ptr mrcal_calibrate( Nobservations_board, problem_selections, &mrcal_lensmodel); int Nmeasurements = mrcal_num_measurements( - Nobservations_board, Nobservations_point, + Nobservations_board, Nobservations_point, // observations_point_triangulated, // 0, // hard-coded to 0 calibration_object_width_n, calibration_object_height_n, @@ -200,42 +200,15 @@ static std::unique_ptr mrcal_calibrate( c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, c_observations_board, c_observations_point, Nobservations_board, - Nobservations_point, + Nobservations_point, // observations_point_triangulated, -1, c_observations_board_pool, &mrcal_lensmodel, c_imagersizes, problem_selections, &problem_constants, calibration_object_spacing, calibration_object_width_n, calibration_object_height_n, verbose, false); - // // and for fun, evaluate the jacobian - cholmod_sparse *Jt = NULL; - // int N_j_nonzero = _mrcal_num_j_nonzero( - // Nobservations_board, Nobservations_point, calibration_object_width_n, - // calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, - // Nframes, Npoints, Npoints_fixed, c_observations_board, - // c_observations_point, problem_selections, &mrcal_lensmodel); - - // cholmod_sparse *Jt = cholmod_l_allocate_sparse( - // static_cast(Nstate), static_cast(Nmeasurements), - // static_cast(N_j_nonzero), 1, 1, 0, CHOLMOD_REAL, cctx.cc); - - // // std::printf("Getting jacobian\n"); - // if (!mrcal_optimizer_callback( - // c_b_packed_final, Nstate * sizeof(double), c_x_final, - // Nmeasurements * sizeof(double), Jt, c_intrinsics, c_extrinsics, - // c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, - // Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, - // c_observations_board, c_observations_point, Nobservations_board, - // Nobservations_point, c_observations_board_pool, &mrcal_lensmodel, - // c_imagersizes, problem_selections, &problem_constants, - // calibration_object_spacing, calibration_object_width_n, - // calibration_object_height_n, verbose)) { - // std::cerr << "callback failed!\n"; - // } - // // std::cout << "Jacobian! " << std::endl; - std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; return std::make_unique( - true, intrinsics, stats.rms_reproj_error__pixels, residuals, Jt, + true, intrinsics, stats.rms_reproj_error__pixels, residuals, calobject_warp, stats.Noutliers); } @@ -388,10 +361,7 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, } mrcal_result::~mrcal_result() { - cholmod_l_free_sparse(&Jt, cctx.cc); - // std::free(Jt.p); - // std::free(Jt.i); - // std::free(Jt.x); + // cholmod_l_free_sparse(&Jt, cctx.cc); return; } diff --git a/src/mrcal_wrapper.h b/src/mrcal_wrapper.h index e32ba62..4927d25 100644 --- a/src/mrcal_wrapper.h +++ b/src/mrcal_wrapper.h @@ -40,7 +40,6 @@ struct mrcal_result { std::vector intrinsics; double rms_error; std::vector residuals; - cholmod_sparse *Jt; mrcal_calobject_warp_t calobject_warp; int Noutliers_board; // TODO standard devs @@ -48,10 +47,9 @@ struct mrcal_result { mrcal_result() = default; mrcal_result(bool success_, std::vector intrinsics_, double rms_error_, std::vector residuals_, - cholmod_sparse *Jt_, mrcal_calobject_warp_t calobject_warp_, - int Noutliers_board_) + 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_}, + rms_error{rms_error_}, residuals{std::move(residuals_)}, calobject_warp{calobject_warp_}, Noutliers_board{Noutliers_board_} {} mrcal_result(mrcal_result &&) = delete; ~mrcal_result(); From 49be69f4752a56cd5eb6a5a092a8da79ff28cae1 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Tue, 5 Mar 2024 14:46:22 -0500 Subject: [PATCH 08/12] pass back up rt-toref --- .../java/org/photonvision/mrcal/MrCalJNI.java | 4 +- src/mrcal_jni.cpp | 38 +++++++++++++++---- src/mrcal_wrapper.cpp | 5 --- 3 files changed, 32 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index cb3659c..e99ef1e 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -17,8 +17,6 @@ package org.photonvision.mrcal; -import java.io.IOException; -import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -38,7 +36,7 @@ public MrCalResult(boolean success) { this.success = success; } public MrCalResult( - boolean success, double[] intrinsics, double rms_error, double[] residuals, double warp_x, + boolean success, double[] intrinsics, double[] optimized_rt_rtoref, double rms_error, double[] residuals, double warp_x, double warp_y, int Noutliers) { this.success = success; this.intrinsics = intrinsics; diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index 43e19e5..81ff080 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include "mrcal_wrapper.h" @@ -45,6 +46,21 @@ WPI_JNI_MAKEJARRAY(jdouble, Double) #undef WPI_JNI_MAKEJARRAY +#define JNI_BOOL "Z" +#define JNI_VOID "V" +#define JNI_INT "I" +#define JNI_DOUBLE "D" +#define JNI_DOUBLEARR "[D" + +#define JNI_STRINGIFY(x) #x + +template +std::string jni_make_method_sig(A retval, Ts&&... args) { + std::ostringstream oss; + (oss << "(" << ... << std::forward(args)) << ")" << retval; + return oss.str(); +} + /** * Finds a class and keeps it as a global reference. * @@ -80,7 +96,8 @@ class JClass { }; // Cache MrCalResult class -JClass detectionClass; +static JClass detectionClass; +static jmethodID constructor; extern "C" { JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) { @@ -96,6 +113,11 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) { return JNI_ERR; } + // Find the constructor. Reference: + // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html + constructor = + env->GetMethodID(detectionClass, "", jni_make_method_sig(JNI_VOID, JNI_BOOL, JNI_DOUBLEARR, JNI_DOUBLEARR, JNI_DOUBLE, JNI_DOUBLEARR, JNI_DOUBLE, JNI_DOUBLE, JNI_INT).c_str()); + return JNI_VERSION_1_6; } @@ -171,10 +193,6 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera } mrcal_result &stats = *statsptr; - // Find the constructor. Reference: - // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html - static jmethodID constructor = - env->GetMethodID(detectionClass, "", "(Z[DD[DDDI)V"); if (!constructor) { return nullptr; } @@ -192,8 +210,14 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera jdouble warp_y = stats.calobject_warp.y2; jint Noutliers = stats.Noutliers_board; - // Actually call the constructor (TODO) - auto ret = env->NewObject(detectionClass, constructor, success, intrinsics, + jdoubleArray optimized_rt_toref = MakeJDoubleArray( + env, (double *)total_frames_rt_toref.data(), + total_frames_rt_toref.size() * sizeof(mrcal_pose_t) / sizeof(double)); + + env->SetDoubleArrayRegion(observations_board, 0, observations.size()*3, (double*)observations.data()); + + // Actually call the constructor + auto ret = env->NewObject(detectionClass, constructor, success, intrinsics, optimized_rt_toref, rms_err, residuals, warp_x, warp_y, Noutliers); return ret; diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp index 5badd83..2d88d09 100644 --- a/src/mrcal_wrapper.cpp +++ b/src/mrcal_wrapper.cpp @@ -348,11 +348,6 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, for (auto a : objectPoints) objectPoints3.push_back(Point3f(a.x, a.y, 0)); - // for (auto& o : objectPoints) std::cout << o << std::endl; - // for (auto& i : imagePoints) std::cout << i << std::endl; - // std::cout << "cam mat\n" << cameraMatrix << std::endl; - // std::cout << "distortion: " << distCoeffs << std::endl; - solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, SOLVEPNP_ITERATIVE); From c11e2cb173c0d9a636a31ae9e639e8579e1061ea Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Tue, 5 Mar 2024 14:49:35 -0500 Subject: [PATCH 09/12] add wpilib things --- build.gradle | 2 ++ src/main/java/org/photonvision/mrcal/MrCalJNI.java | 2 ++ 2 files changed, 4 insertions(+) diff --git a/build.gradle b/build.gradle index 3560db4..b1f7fee 100644 --- a/build.gradle +++ b/build.gradle @@ -35,6 +35,8 @@ dependencies { testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") testImplementation("org.junit.jupiter:junit-jupiter-params:5.8.2") testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2") + + implementation wpilibTools.deps.wpilibJava("wpimath") } tasks.withType(JavaCompile) { diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index e99ef1e..a2b6069 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -21,6 +21,7 @@ import java.util.List; import org.opencv.core.MatOfPoint2f; +import edu.wpi.first.math.geometry.Pose3d; public class MrCalJNI { public static class MrCalResult { @@ -31,6 +32,7 @@ public static class MrCalResult { public double warp_x; public double warp_y; public int Noutliers; + public Pose3d[] optimizedPoses; public MrCalResult(boolean success) { this.success = success; From cb0abfb9122572e91685ab5d1404d433226795f3 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Tue, 5 Mar 2024 14:54:25 -0500 Subject: [PATCH 10/12] Add pose to retval --- .../java/org/photonvision/mrcal/MrCalJNI.java | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index a2b6069..2e5a4bf 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -21,7 +21,11 @@ import java.util.List; import org.opencv.core.MatOfPoint2f; + +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; public class MrCalJNI { public static class MrCalResult { @@ -32,7 +36,7 @@ public static class MrCalResult { public double warp_x; public double warp_y; public int Noutliers; - public Pose3d[] optimizedPoses; + public List optimizedPoses; public MrCalResult(boolean success) { this.success = success; @@ -47,6 +51,20 @@ public MrCalResult( this.warp_x = warp_x; this.warp_y = warp_y; this.Noutliers = Noutliers; + + for (int i = 0; i < optimized_rt_rtoref.length; i+=6) { + var rot = new Rotation3d(VecBuilder.fill( + optimized_rt_rtoref[i+0], + optimized_rt_rtoref[i+1], + optimized_rt_rtoref[i+2] + )); + var t = new Translation3d( + optimized_rt_rtoref[i+3], + optimized_rt_rtoref[i+4], + optimized_rt_rtoref[i+5]); + + optimizedPoses.add(new Pose3d(t, rot)); + } } @Override From df28f558f85843caf0be1e08b4125bef41766ae6 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Tue, 5 Mar 2024 23:23:22 -0500 Subject: [PATCH 11/12] Pass corners used back up --- build.gradle | 2 ++ .../java/org/photonvision/mrcal/MrCalJNI.java | 12 +++++++-- src/mrcal_jni.cpp | 26 +++++++++++++------ 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/build.gradle b/build.gradle index b1f7fee..76c9bc3 100644 --- a/build.gradle +++ b/build.gradle @@ -37,6 +37,8 @@ dependencies { testRuntimeOnly("org.junit.jupiter:junit-jupiter-engine:5.8.2") implementation wpilibTools.deps.wpilibJava("wpimath") + implementation wpilibTools.deps.wpilibJava("wpiunits") + implementation "com.fasterxml.jackson.core:jackson-annotations:2.15.2" } tasks.withType(JavaCompile) { diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index 2e5a4bf..522a1a7 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -17,6 +17,7 @@ package org.photonvision.mrcal; +import java.util.ArrayList; import java.util.Arrays; import java.util.List; @@ -37,13 +38,14 @@ public static class MrCalResult { public double warp_y; public int Noutliers; public List optimizedPoses; + public List cornersUsed; public MrCalResult(boolean success) { this.success = success; } public MrCalResult( - boolean success, double[] intrinsics, double[] optimized_rt_rtoref, double rms_error, double[] residuals, double warp_x, - double warp_y, int Noutliers) { + boolean success, int width, int height, double[] intrinsics, double[] optimized_rt_rtoref, double rms_error, double[] residuals, double warp_x, + double warp_y, int Noutliers, boolean[] cornerUseMask) { this.success = success; this.intrinsics = intrinsics; this.rms_error = rms_error; @@ -65,6 +67,12 @@ public MrCalResult( optimizedPoses.add(new Pose3d(t, rot)); } + + var cornersPerBoard = width*height; + cornersUsed=new ArrayList<>(); + for (int cornerIdx = 0; cornerIdx < cornerUseMask.length; cornerIdx+=cornersPerBoard) { + cornersUsed.add(Arrays.copyOfRange(cornerUseMask, cornerIdx, cornerIdx+cornersPerBoard)); + } } @Override diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index 81ff080..c9531eb 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -20,9 +20,9 @@ #include #include #include +#include #include #include -#include #include "mrcal_wrapper.h" @@ -51,11 +51,12 @@ WPI_JNI_MAKEJARRAY(jdouble, Double) #define JNI_INT "I" #define JNI_DOUBLE "D" #define JNI_DOUBLEARR "[D" +#define JNI_BOOLARR "[Z" #define JNI_STRINGIFY(x) #x template -std::string jni_make_method_sig(A retval, Ts&&... args) { +std::string jni_make_method_sig(A retval, Ts &&...args) { std::ostringstream oss; (oss << "(" << ... << std::forward(args)) << ")" << retval; return oss.str(); @@ -115,8 +116,12 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) { // Find the constructor. Reference: // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html - constructor = - env->GetMethodID(detectionClass, "", jni_make_method_sig(JNI_VOID, JNI_BOOL, JNI_DOUBLEARR, JNI_DOUBLEARR, JNI_DOUBLE, JNI_DOUBLEARR, JNI_DOUBLE, JNI_DOUBLE, JNI_INT).c_str()); + constructor = env->GetMethodID( + detectionClass, "", + jni_make_method_sig(JNI_VOID, JNI_BOOL, JNI_INT, JNI_INT, JNI_DOUBLEARR, + JNI_DOUBLEARR, JNI_DOUBLE, JNI_DOUBLEARR, JNI_DOUBLE, + JNI_DOUBLE, JNI_INT, JNI_BOOLARR) + .c_str()); return JNI_VERSION_1_6; } @@ -211,14 +216,19 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera jint Noutliers = stats.Noutliers_board; jdoubleArray optimized_rt_toref = MakeJDoubleArray( - env, (double *)total_frames_rt_toref.data(), + env, reinterpret_cast(total_frames_rt_toref.data()), total_frames_rt_toref.size() * sizeof(mrcal_pose_t) / sizeof(double)); - env->SetDoubleArrayRegion(observations_board, 0, observations.size()*3, (double*)observations.data()); + std::vector cornersUsedMask; + std::transform(observations.begin(), observations.end(), + cornersUsedMask.begin(), + [](const auto &pt) { return pt.z > 0; }); // Actually call the constructor - auto ret = env->NewObject(detectionClass, constructor, success, intrinsics, optimized_rt_toref, - rms_err, residuals, warp_x, warp_y, Noutliers); + auto ret = + env->NewObject(detectionClass, constructor, success, boardWidth, + boardHeight, intrinsics, optimized_rt_toref, rms_err, + residuals, warp_x, warp_y, Noutliers, cornersUsedMask); return ret; } From 15368579bcdd1c58a9314ae4077a2440ca43cb0d Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Tue, 5 Mar 2024 23:29:17 -0500 Subject: [PATCH 12/12] oops oops 2 --- .../java/org/photonvision/mrcal/MrCalJNI.java | 34 +++++++++++-------- src/mrcal_jni.cpp | 7 ++-- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/photonvision/mrcal/MrCalJNI.java b/src/main/java/org/photonvision/mrcal/MrCalJNI.java index 522a1a7..e9f3eaa 100644 --- a/src/main/java/org/photonvision/mrcal/MrCalJNI.java +++ b/src/main/java/org/photonvision/mrcal/MrCalJNI.java @@ -43,8 +43,10 @@ public static class MrCalResult { public MrCalResult(boolean success) { this.success = success; } + public MrCalResult( - boolean success, int width, int height, double[] intrinsics, double[] optimized_rt_rtoref, double rms_error, double[] residuals, double warp_x, + boolean success, int width, int height, double[] intrinsics, double[] optimized_rt_rtoref, + double rms_error, double[] residuals, double warp_x, double warp_y, int Noutliers, boolean[] cornerUseMask) { this.success = success; this.intrinsics = intrinsics; @@ -54,24 +56,24 @@ public MrCalResult( this.warp_y = warp_y; this.Noutliers = Noutliers; - for (int i = 0; i < optimized_rt_rtoref.length; i+=6) { + optimizedPoses = new ArrayList<>(); + for (int i = 0; i < optimized_rt_rtoref.length; i += 6) { var rot = new Rotation3d(VecBuilder.fill( - optimized_rt_rtoref[i+0], - optimized_rt_rtoref[i+1], - optimized_rt_rtoref[i+2] - )); + optimized_rt_rtoref[i + 0], + optimized_rt_rtoref[i + 1], + optimized_rt_rtoref[i + 2])); var t = new Translation3d( - optimized_rt_rtoref[i+3], - optimized_rt_rtoref[i+4], - optimized_rt_rtoref[i+5]); + optimized_rt_rtoref[i + 3], + optimized_rt_rtoref[i + 4], + optimized_rt_rtoref[i + 5]); optimizedPoses.add(new Pose3d(t, rot)); } - var cornersPerBoard = width*height; - cornersUsed=new ArrayList<>(); - for (int cornerIdx = 0; cornerIdx < cornerUseMask.length; cornerIdx+=cornersPerBoard) { - cornersUsed.add(Arrays.copyOfRange(cornerUseMask, cornerIdx, cornerIdx+cornersPerBoard)); + var cornersPerBoard = width * height; + cornersUsed = new ArrayList<>(); + for (int cornerIdx = 0; cornerIdx < cornerUseMask.length; cornerIdx += cornersPerBoard) { + cornersUsed.add(Arrays.copyOfRange(cornerUseMask, cornerIdx, cornerIdx + cornersPerBoard)); } } @@ -88,7 +90,8 @@ 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 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 board_corners, @@ -115,6 +118,7 @@ public static MrCalResult calibrateCamera( return new MrCalResult(false); } - return mrcal_calibrate_camera(observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight, focalLen); + return mrcal_calibrate_camera(observations, boardWidth, boardHeight, boardSpacing, imageWidth, imageHeight, + focalLen); } } diff --git a/src/mrcal_jni.cpp b/src/mrcal_jni.cpp index c9531eb..2e281ea 100644 --- a/src/mrcal_jni.cpp +++ b/src/mrcal_jni.cpp @@ -219,16 +219,17 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera env, reinterpret_cast(total_frames_rt_toref.data()), total_frames_rt_toref.size() * sizeof(mrcal_pose_t) / sizeof(double)); - std::vector cornersUsedMask; + std::vector cornersUsedMask(observations.size()); std::transform(observations.begin(), observations.end(), cornersUsedMask.begin(), - [](const auto &pt) { return pt.z > 0; }); + [](const auto &pt) { return (jboolean)(pt.z > 0); }); + auto cornersUsedJarr = MakeJBooleanArray(env, cornersUsedMask.data(), cornersUsedMask.size()); // Actually call the constructor auto ret = env->NewObject(detectionClass, constructor, success, boardWidth, boardHeight, intrinsics, optimized_rt_toref, rms_err, - residuals, warp_x, warp_y, Noutliers, cornersUsedMask); + residuals, warp_x, warp_y, Noutliers, cornersUsedJarr); return ret; }