Skip to content

Commit

Permalink
Run lint
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcm001
mcm001 committed May 9, 2024
1 parent 741b1ad commit 23b7f8d
Show file tree
Hide file tree
Showing 2 changed files with 92 additions and 89 deletions.
176 changes: 90 additions & 86 deletions src/mrcal_jni.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,11 @@

#include <algorithm>
#include <cstdio>
#include <exception>
#include <span>
#include <stdexcept>
#include <vector>

#include <exception>

#include "mrcal_wrapper.h"

// JClass helper from wpilib
Expand Down Expand Up @@ -103,15 +102,23 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) {

} // extern "C"

static std::string what(const std::exception_ptr &eptr = std::current_exception())
{
if (!eptr) { throw std::bad_exception(); }
static std::string
what(const std::exception_ptr &eptr = std::current_exception()) {
if (!eptr) {
throw std::bad_exception();
}

try { std::rethrow_exception(eptr); }
catch (const std::exception &e) { return e.what() ; }
catch (const std::string &e) { return e ; }
catch (const char *e) { return e ; }
catch (...) { return "who knows"; }
try {
std::rethrow_exception(eptr);
} catch (const std::exception &e) {
return e.what();
} catch (const std::string &e) {
return e;
} catch (const char *e) {
return e;
} catch (...) {
return "who knows";
}
}

/*
Expand All @@ -126,99 +133,96 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera
jdouble focalLenGuessMM)
{
try {


// Pull out arrays. We rely on data being packed and aligned to make this
// work! Observations should be [x, y, level]
std::span<mrcal_point3_t> observations{
reinterpret_cast<mrcal_point3_t *>(
env->GetDoubleArrayElements(observations_board, 0)),
env->GetArrayLength(observations_board) / 3lu};

size_t points_in_board = boardWidth * boardHeight;
if (observations.size() % points_in_board != 0) {
jclass exception_class = env->FindClass("java/lang/Exception");
if (exception_class && env) {
(env)->ExceptionClear();
env->ThrowNew(exception_class,
"Observation list length does not match board size!");
return {};
} else {
// ????
std::cerr << "Observation list length does not match board size!\n";
// Pull out arrays. We rely on data being packed and aligned to make this
// work! Observations should be [x, y, level]
std::span<mrcal_point3_t> observations{
reinterpret_cast<mrcal_point3_t *>(
env->GetDoubleArrayElements(observations_board, 0)),
env->GetArrayLength(observations_board) / 3lu};

size_t points_in_board = boardWidth * boardHeight;
if (observations.size() % points_in_board != 0) {
jclass exception_class = env->FindClass("java/lang/Exception");
if (exception_class && env) {
(env)->ExceptionClear();
env->ThrowNew(exception_class,
"Observation list length does not match board size!");
return {};
} else {
// ????
std::cerr << "Observation list length does not match board size!\n";
}
}
}

size_t boards_observed = observations.size() / points_in_board;

const auto boardSize = cv::Size{boardWidth, boardHeight};
const auto imagerSize = cv::Size{imageWidth, imageHeight};
size_t boards_observed = observations.size() / points_in_board;

// down big list of observations/extrinsic guesses (one per board object)
std::vector<mrcal_pose_t> total_frames_rt_toref;
const auto boardSize = cv::Size{boardWidth, boardHeight};
const auto imagerSize = cv::Size{imageWidth, imageHeight};

for (size_t i = 0; i < boards_observed; i++) {
auto seed_pose =
getSeedPose(&(*observations.begin()) + (i * points_in_board), boardSize,
imagerSize, boardSpacing, focalLenGuessMM);
// std::printf("Seed pose %lu: r %f %f %f t %f %f %f\n", i, seed_pose.r.x,
// seed_pose.r.y, seed_pose.r.z, seed_pose.t.x, seed_pose.t.y,
// seed_pose.t.z);
// down big list of observations/extrinsic guesses (one per board object)
std::vector<mrcal_pose_t> total_frames_rt_toref;

// Add to seed poses
total_frames_rt_toref.push_back(seed_pose);
}
for (size_t i = 0; i < boards_observed; i++) {
auto seed_pose =
getSeedPose(&(*observations.begin()) + (i * points_in_board),
boardSize, imagerSize, boardSpacing, focalLenGuessMM);
// std::printf("Seed pose %lu: r %f %f %f t %f %f %f\n", i, seed_pose.r.x,
// seed_pose.r.y, seed_pose.r.z, seed_pose.t.x, seed_pose.t.y,
// seed_pose.t.z);

// Convert detection level to weights
for (auto &o : observations) {
double &level = o.z;
if (level < 0) {
o.z = -1;
} else {
o.z = std::pow(0.5, level);
// Add to seed poses
total_frames_rt_toref.push_back(seed_pose);
}
}

auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize,
static_cast<double>(boardSpacing), imagerSize,
focalLenGuessMM);
if (!statsptr) {
return nullptr;
}
mrcal_result &stats = *statsptr;

// Find the constructor. Reference:
// https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html
static jmethodID constructor =
env->GetMethodID(detectionClass, "<init>", "(Z[DD[DDDI)V");
if (!constructor) {
return nullptr;
}
// Convert detection level to weights
for (auto &o : observations) {
double &level = o.z;
if (level < 0) {
o.z = -1;
} else {
o.z = std::pow(0.5, level);
}
}

size_t Nintrinsics = stats.intrinsics.size();
size_t Nresid = stats.residuals.size();
auto statsptr = mrcal_main(observations, total_frames_rt_toref, boardSize,
static_cast<double>(boardSpacing), imagerSize,
focalLenGuessMM);
if (!statsptr) {
return nullptr;
}
mrcal_result &stats = *statsptr;

// Find the constructor. Reference:
// https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html
static jmethodID constructor =
env->GetMethodID(detectionClass, "<init>", "(Z[DD[DDDI)V");
if (!constructor) {
return nullptr;
}

jdoubleArray intrinsics =
MakeJDoubleArray(env, stats.intrinsics.data(), Nintrinsics);
jdoubleArray residuals =
MakeJDoubleArray(env, stats.residuals.data(), Nresid);
jboolean success = stats.success;
jdouble rms_err = stats.rms_error;
jdouble warp_x = stats.calobject_warp.x2;
jdouble warp_y = stats.calobject_warp.y2;
jint Noutliers = stats.Noutliers_board;
size_t Nintrinsics = stats.intrinsics.size();
size_t Nresid = stats.residuals.size();

// Actually call the constructor (TODO)
auto ret = env->NewObject(detectionClass, constructor, success, intrinsics,
rms_err, residuals, warp_x, warp_y, Noutliers);
jdoubleArray intrinsics =
MakeJDoubleArray(env, stats.intrinsics.data(), Nintrinsics);
jdoubleArray residuals =
MakeJDoubleArray(env, stats.residuals.data(), Nresid);
jboolean success = stats.success;
jdouble rms_err = stats.rms_error;
jdouble warp_x = stats.calobject_warp.x2;
jdouble warp_y = stats.calobject_warp.y2;
jint Noutliers = stats.Noutliers_board;

return ret;
// Actually call the constructor (TODO)
auto ret = env->NewObject(detectionClass, constructor, success, intrinsics,
rms_err, residuals, warp_x, warp_y, Noutliers);

return ret;
} catch (...) {
std::cerr << "Calibration exception: " << what() << std::endl;

static char buff[512];
strcpy(buff, what().c_str());
std::strcpy(buff, what().c_str());
env->ThrowNew(env->FindClass("java/lang/Exception"), buff);
}
}
Expand Down
5 changes: 2 additions & 3 deletions src/mrcal_wrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -334,9 +334,8 @@ mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
&model, intrinsics);

std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(),
imagePoints.begin(), [](const auto &pt) {
return Point2d{pt.x, pt.y};
});
imagePoints.begin(),
[](const auto &pt) { return Point2d{pt.x, pt.y}; });
}

// Initial guess at intrinsics
Expand Down

0 comments on commit 23b7f8d

Please sign in to comment.