19
19
20
20
#include < algorithm>
21
21
#include < cstdio>
22
+ #include < exception>
22
23
#include < span>
23
24
#include < stdexcept>
24
25
#include < vector>
25
26
26
- #include < exception>
27
-
28
27
#include " mrcal_wrapper.h"
29
28
30
29
// JClass helper from wpilib
@@ -103,15 +102,23 @@ JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM *vm, void *reserved) {
103
102
104
103
} // extern "C"
105
104
106
- static std::string what (const std::exception_ptr &eptr = std::current_exception())
107
- {
108
- if (!eptr) { throw std::bad_exception (); }
105
+ static std::string
106
+ what (const std::exception_ptr &eptr = std::current_exception()) {
107
+ if (!eptr) {
108
+ throw std::bad_exception ();
109
+ }
109
110
110
- try { std::rethrow_exception (eptr); }
111
- catch (const std::exception &e) { return e.what () ; }
112
- catch (const std::string &e) { return e ; }
113
- catch (const char *e) { return e ; }
114
- catch (...) { return " who knows" ; }
111
+ try {
112
+ std::rethrow_exception (eptr);
113
+ } catch (const std::exception &e) {
114
+ return e.what ();
115
+ } catch (const std::string &e) {
116
+ return e;
117
+ } catch (const char *e) {
118
+ return e;
119
+ } catch (...) {
120
+ return " who knows" ;
121
+ }
115
122
}
116
123
117
124
/*
@@ -126,99 +133,96 @@ Java_org_photonvision_mrcal_MrCalJNI_mrcal_1calibrate_1camera
126
133
jdouble focalLenGuessMM)
127
134
{
128
135
try {
129
-
130
-
131
- // Pull out arrays. We rely on data being packed and aligned to make this
132
- // work! Observations should be [x, y, level]
133
- std::span<mrcal_point3_t > observations{
134
- reinterpret_cast <mrcal_point3_t *>(
135
- env->GetDoubleArrayElements (observations_board, 0 )),
136
- env->GetArrayLength (observations_board) / 3lu};
137
-
138
- size_t points_in_board = boardWidth * boardHeight;
139
- if (observations.size () % points_in_board != 0 ) {
140
- jclass exception_class = env->FindClass (" java/lang/Exception" );
141
- if (exception_class && env) {
142
- (env)->ExceptionClear ();
143
- env->ThrowNew (exception_class,
144
- " Observation list length does not match board size!" );
145
- return {};
146
- } else {
147
- // ????
148
- std::cerr << " Observation list length does not match board size!\n " ;
136
+ // Pull out arrays. We rely on data being packed and aligned to make this
137
+ // work! Observations should be [x, y, level]
138
+ std::span<mrcal_point3_t > observations{
139
+ reinterpret_cast <mrcal_point3_t *>(
140
+ env->GetDoubleArrayElements (observations_board, 0 )),
141
+ env->GetArrayLength (observations_board) / 3lu};
142
+
143
+ size_t points_in_board = boardWidth * boardHeight;
144
+ if (observations.size () % points_in_board != 0 ) {
145
+ jclass exception_class = env->FindClass (" java/lang/Exception" );
146
+ if (exception_class && env) {
147
+ (env)->ExceptionClear ();
148
+ env->ThrowNew (exception_class,
149
+ " Observation list length does not match board size!" );
150
+ return {};
151
+ } else {
152
+ // ????
153
+ std::cerr << " Observation list length does not match board size!\n " ;
154
+ }
149
155
}
150
- }
151
-
152
- size_t boards_observed = observations.size () / points_in_board;
153
156
154
- const auto boardSize = cv::Size {boardWidth, boardHeight};
155
- const auto imagerSize = cv::Size {imageWidth, imageHeight};
157
+ size_t boards_observed = observations.size () / points_in_board;
156
158
157
- // down big list of observations/extrinsic guesses (one per board object)
158
- std::vector< mrcal_pose_t > total_frames_rt_toref ;
159
+ const auto boardSize = cv:: Size {boardWidth, boardHeight};
160
+ const auto imagerSize = cv:: Size {imageWidth, imageHeight} ;
159
161
160
- for (size_t i = 0 ; i < boards_observed; i++) {
161
- auto seed_pose =
162
- getSeedPose (&(*observations.begin ()) + (i * points_in_board), boardSize,
163
- imagerSize, boardSpacing, focalLenGuessMM);
164
- // std::printf("Seed pose %lu: r %f %f %f t %f %f %f\n", i, seed_pose.r.x,
165
- // seed_pose.r.y, seed_pose.r.z, seed_pose.t.x, seed_pose.t.y,
166
- // seed_pose.t.z);
162
+ // down big list of observations/extrinsic guesses (one per board object)
163
+ std::vector<mrcal_pose_t > total_frames_rt_toref;
167
164
168
- // Add to seed poses
169
- total_frames_rt_toref.push_back (seed_pose);
170
- }
165
+ for (size_t i = 0 ; i < boards_observed; i++) {
166
+ auto seed_pose =
167
+ getSeedPose (&(*observations.begin ()) + (i * points_in_board),
168
+ boardSize, imagerSize, boardSpacing, focalLenGuessMM);
169
+ // std::printf("Seed pose %lu: r %f %f %f t %f %f %f\n", i, seed_pose.r.x,
170
+ // seed_pose.r.y, seed_pose.r.z, seed_pose.t.x, seed_pose.t.y,
171
+ // seed_pose.t.z);
171
172
172
- // Convert detection level to weights
173
- for (auto &o : observations) {
174
- double &level = o.z ;
175
- if (level < 0 ) {
176
- o.z = -1 ;
177
- } else {
178
- o.z = std::pow (0.5 , level);
173
+ // Add to seed poses
174
+ total_frames_rt_toref.push_back (seed_pose);
179
175
}
180
- }
181
176
182
- auto statsptr = mrcal_main (observations, total_frames_rt_toref, boardSize,
183
- static_cast <double >(boardSpacing), imagerSize,
184
- focalLenGuessMM);
185
- if (!statsptr) {
186
- return nullptr ;
187
- }
188
- mrcal_result &stats = *statsptr;
189
-
190
- // Find the constructor. Reference:
191
- // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html
192
- static jmethodID constructor =
193
- env->GetMethodID (detectionClass, " <init>" , " (Z[DD[DDDI)V" );
194
- if (!constructor) {
195
- return nullptr ;
196
- }
177
+ // Convert detection level to weights
178
+ for (auto &o : observations) {
179
+ double &level = o.z ;
180
+ if (level < 0 ) {
181
+ o.z = -1 ;
182
+ } else {
183
+ o.z = std::pow (0.5 , level);
184
+ }
185
+ }
197
186
198
- size_t Nintrinsics = stats.intrinsics .size ();
199
- size_t Nresid = stats.residuals .size ();
187
+ auto statsptr = mrcal_main (observations, total_frames_rt_toref, boardSize,
188
+ static_cast <double >(boardSpacing), imagerSize,
189
+ focalLenGuessMM);
190
+ if (!statsptr) {
191
+ return nullptr ;
192
+ }
193
+ mrcal_result &stats = *statsptr;
194
+
195
+ // Find the constructor. Reference:
196
+ // https://www.microfocus.com/documentation/extend-acucobol/925/BKITITJAVAS027.html
197
+ static jmethodID constructor =
198
+ env->GetMethodID (detectionClass, " <init>" , " (Z[DD[DDDI)V" );
199
+ if (!constructor) {
200
+ return nullptr ;
201
+ }
200
202
201
- jdoubleArray intrinsics =
202
- MakeJDoubleArray (env, stats.intrinsics .data (), Nintrinsics);
203
- jdoubleArray residuals =
204
- MakeJDoubleArray (env, stats.residuals .data (), Nresid);
205
- jboolean success = stats.success ;
206
- jdouble rms_err = stats.rms_error ;
207
- jdouble warp_x = stats.calobject_warp .x2 ;
208
- jdouble warp_y = stats.calobject_warp .y2 ;
209
- jint Noutliers = stats.Noutliers_board ;
203
+ size_t Nintrinsics = stats.intrinsics .size ();
204
+ size_t Nresid = stats.residuals .size ();
210
205
211
- // Actually call the constructor (TODO)
212
- auto ret = env->NewObject (detectionClass, constructor, success, intrinsics,
213
- rms_err, residuals, warp_x, warp_y, Noutliers);
206
+ jdoubleArray intrinsics =
207
+ MakeJDoubleArray (env, stats.intrinsics .data (), Nintrinsics);
208
+ jdoubleArray residuals =
209
+ MakeJDoubleArray (env, stats.residuals .data (), Nresid);
210
+ jboolean success = stats.success ;
211
+ jdouble rms_err = stats.rms_error ;
212
+ jdouble warp_x = stats.calobject_warp .x2 ;
213
+ jdouble warp_y = stats.calobject_warp .y2 ;
214
+ jint Noutliers = stats.Noutliers_board ;
214
215
215
- return ret;
216
+ // Actually call the constructor (TODO)
217
+ auto ret = env->NewObject (detectionClass, constructor, success, intrinsics,
218
+ rms_err, residuals, warp_x, warp_y, Noutliers);
216
219
220
+ return ret;
217
221
} catch (...) {
218
222
std::cerr << " Calibration exception: " << what () << std::endl;
219
223
220
224
static char buff[512 ];
221
- strcpy (buff, what ().c_str ());
225
+ std:: strcpy (buff, what ().c_str ());
222
226
env->ThrowNew (env->FindClass (" java/lang/Exception" ), buff);
223
227
}
224
228
}
0 commit comments