179 lines
8.2 KiB
C++
179 lines
8.2 KiB
C++
#include "sparse_sfm_gate_f_internal.h"
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <limits>
|
|
#include <openssl/evp.h>
|
|
#include <vector>
|
|
#include <algorithm>
|
|
|
|
namespace {
|
|
bool put_u32(unsigned char *record, size_t *offset, uint32_t value) {
|
|
if (*offset > 372U - 4U) return false;
|
|
for (unsigned int byte = 0; byte < 4; ++byte)
|
|
record[(*offset)++] = static_cast<unsigned char>(value >> (byte * 8U));
|
|
return true;
|
|
}
|
|
|
|
bool put_u64(unsigned char *record, size_t *offset, uint64_t value) {
|
|
if (*offset > 372U - 8U) return false;
|
|
for (unsigned int byte = 0; byte < 8; ++byte)
|
|
record[(*offset)++] = static_cast<unsigned char>(value >> (byte * 8U));
|
|
return true;
|
|
}
|
|
|
|
bool put_f64(unsigned char *record, size_t *offset, double value) {
|
|
if (!std::isfinite(value)) return false;
|
|
if (value == 0.0) value = 0.0;
|
|
uint64_t bits = 0;
|
|
static_assert(sizeof(bits) == sizeof(value));
|
|
std::memcpy(&bits, &value, sizeof(bits));
|
|
return put_u64(record, offset, bits);
|
|
}
|
|
|
|
bool checked_add(uint64_t left, uint64_t right, uint64_t *result) {
|
|
if (right > std::numeric_limits<uint64_t>::max() - left) return false;
|
|
*result = left + right;
|
|
return true;
|
|
}
|
|
|
|
bool checked_mul(uint64_t left, uint64_t right, uint64_t *result) {
|
|
if (left != 0 && right > std::numeric_limits<uint64_t>::max() / left) return false;
|
|
*result = left * right;
|
|
return true;
|
|
}
|
|
} // namespace
|
|
|
|
bool lardon3d_sparse_sfm_fingerprint_record(
|
|
const Lardon3DSparseIncrementalParameters *p, unsigned char record[372]) {
|
|
if (!p || !record) return false;
|
|
std::memset(record, 0, 372);
|
|
std::memcpy(record, "L3DSFMFP", 8);
|
|
size_t offset = 8;
|
|
bool ok = put_u32(record, &offset, 1) &&
|
|
put_u32(record, &offset, p->minimum_seed_tracks) &&
|
|
put_u32(record, &offset, p->minimum_seed_landmarks) &&
|
|
put_u32(record, &offset, p->minimum_pnp_correspondences) &&
|
|
put_u32(record, &offset, p->maximum_seed_candidates) &&
|
|
put_u32(record, &offset, p->maximum_registration_rounds) &&
|
|
put_u32(record, &offset, p->maximum_landmarks_per_round) &&
|
|
put_u32(record, &offset, p->maximum_images) &&
|
|
put_u64(record, &offset, p->maximum_observations) &&
|
|
put_u64(record, &offset, p->maximum_tracks) &&
|
|
put_f64(record, &offset, p->reprojection_threshold_px) &&
|
|
put_f64(record, &offset, p->minimum_track_parallax_rad) &&
|
|
put_f64(record, &offset, p->relative_pose.robust_threshold_px) &&
|
|
put_f64(record, &offset, p->relative_pose.confidence) &&
|
|
put_u32(record, &offset, p->relative_pose.max_iterations) &&
|
|
put_u32(record, &offset, p->relative_pose.minimum_inliers) &&
|
|
put_f64(record, &offset, p->relative_pose.minimum_inlier_ratio) &&
|
|
put_f64(record, &offset, p->relative_pose.minimum_parallax_rad) &&
|
|
put_f64(record, &offset, p->relative_pose.minimum_cheirality_ratio) &&
|
|
put_u64(record, &offset, p->relative_pose.deterministic_seed) &&
|
|
put_f64(record, &offset, p->pnp.reprojection_threshold_px) &&
|
|
put_f64(record, &offset, p->pnp.confidence) &&
|
|
put_u32(record, &offset, p->pnp.max_iterations) &&
|
|
put_u32(record, &offset, p->pnp.minimum_inliers) &&
|
|
put_f64(record, &offset, p->pnp.minimum_inlier_ratio) &&
|
|
put_u64(record, &offset, p->pnp.deterministic_seed) &&
|
|
put_u32(record, &offset, p->refinement.max_iterations) &&
|
|
put_f64(record, &offset, p->refinement.convergence_tolerance);
|
|
for (unsigned int policy = 0; ok && policy < 19; ++policy)
|
|
ok = put_u32(record, &offset, 1);
|
|
ok = ok && put_f64(record, &offset, 1e-9);
|
|
for (unsigned int policy = 0; ok && policy < 2; ++policy)
|
|
ok = put_u32(record, &offset, 1);
|
|
ok = ok && put_f64(record, &offset, 1e-9) && put_u32(record, &offset, 1) &&
|
|
put_f64(record, &offset, 2.0);
|
|
for (unsigned int policy = 0; ok && policy < 6; ++policy)
|
|
ok = put_u32(record, &offset, 1);
|
|
ok = ok && put_u32(record, &offset, 1) && put_u32(record, &offset, 50) &&
|
|
put_f64(record, &offset, 1e-6) && put_f64(record, &offset, 1e-10) &&
|
|
put_f64(record, &offset, 1e-8) && put_u32(record, &offset, 0) &&
|
|
put_u32(record, &offset, 1) && put_f64(record, &offset, 1e-12) &&
|
|
put_u32(record, &offset, 1) && put_u32(record, &offset, 1);
|
|
return ok && offset == 372;
|
|
}
|
|
|
|
bool lardon3d_sparse_sfm_parameter_fingerprint(
|
|
const Lardon3DSparseIncrementalParameters *parameters, unsigned char digest[32]) {
|
|
if (!digest) return false;
|
|
unsigned char record[372];
|
|
unsigned int length = 0;
|
|
return lardon3d_sparse_sfm_fingerprint_record(parameters, record) &&
|
|
EVP_Digest(record, sizeof(record), digest, &length, EVP_sha256(), nullptr) == 1 &&
|
|
length == 32;
|
|
}
|
|
|
|
bool lardon3d_sparse_sfm_resource_estimate(uint64_t image_count, uint64_t track_count,
|
|
uint64_t observation_count,
|
|
Lardon3DResourceEstimate *estimate) {
|
|
if (!estimate) return false;
|
|
uint64_t images = 0, tracks = 0, observations = 0, raw = 134217728ULL;
|
|
if (!checked_mul(image_count, 65536ULL, &images) ||
|
|
!checked_mul(track_count, 2048ULL, &tracks) ||
|
|
!checked_mul(observation_count, 512ULL, &observations) ||
|
|
!checked_add(raw, images, &raw) || !checked_add(raw, tracks, &raw) ||
|
|
!checked_add(raw, observations, &raw)) return false;
|
|
constexpr uint64_t mib = 1024ULL * 1024ULL;
|
|
uint64_t remainder = raw % mib;
|
|
if (remainder != 0 && !checked_add(raw, mib - remainder, &raw)) return false;
|
|
*estimate = {raw, 0, 0, 0, 1, 1, 1, 0, 1, LARDON3D_RESOURCE_TASK_CPU};
|
|
return true;
|
|
}
|
|
|
|
bool lardon3d_sparse_sfm_component_persistable(uint64_t registered_image_count,
|
|
uint64_t landmark_count) {
|
|
return registered_image_count > 0 && landmark_count > 0;
|
|
}
|
|
|
|
bool lardon3d_sparse_sfm_publication_metrics(const double *squared_errors, size_t count,
|
|
double *rmse, double *median) {
|
|
if (!squared_errors || count == 0 || !rmse || !median) return false;
|
|
try {
|
|
std::vector<double> errors;
|
|
errors.reserve(count);
|
|
double sum = 0.0;
|
|
for (size_t index = 0; index < count; ++index) {
|
|
double squared = squared_errors[index];
|
|
if (!std::isfinite(squared) || squared < 0.0 || !std::isfinite(sum + squared)) return false;
|
|
sum += squared;
|
|
errors.push_back(std::sqrt(squared));
|
|
}
|
|
std::sort(errors.begin(), errors.end());
|
|
*rmse = std::sqrt(sum / static_cast<double>(count));
|
|
size_t middle = count / 2;
|
|
*median = count % 2 ? errors[middle]
|
|
: errors[middle - 1] + (errors[middle] - errors[middle - 1]) / 2.0;
|
|
return std::isfinite(*rmse) && std::isfinite(*median);
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool lardon3d_sparse_sfm_squared_reprojection_error(
|
|
const Lardon3DSparseGeometryCalibration *calibration,
|
|
const Lardon3DSparseGeometryPose *pose,
|
|
const Lardon3DSparseGeometryPoint3 *point,
|
|
const Lardon3DSparseGeometryPoint2 *observed, double *squared_error) {
|
|
if (!calibration || !pose || !point || !observed || !squared_error) return false;
|
|
const double *r = pose->rotation_cw;
|
|
const double *t = pose->translation_cw;
|
|
const double xc = r[0] * point->x + r[1] * point->y + r[2] * point->z + t[0];
|
|
const double yc = r[3] * point->x + r[4] * point->y + r[5] * point->z + t[1];
|
|
const double zc = r[6] * point->x + r[7] * point->y + r[8] * point->z + t[2];
|
|
if (!std::isfinite(xc) || !std::isfinite(yc) || !std::isfinite(zc) || zc <= 1e-9)
|
|
return false;
|
|
const double xn = xc / zc;
|
|
const double yn = yc / zc;
|
|
const double r2 = xn * xn + yn * yn;
|
|
const double radial = 1.0 + calibration->k1 * r2 + calibration->k2 * r2 * r2;
|
|
const double xd = xn * radial + 2.0 * calibration->p1 * xn * yn +
|
|
calibration->p2 * (r2 + 2.0 * xn * xn);
|
|
const double yd = yn * radial + calibration->p1 * (r2 + 2.0 * yn * yn) +
|
|
2.0 * calibration->p2 * xn * yn;
|
|
const double dx = calibration->fx * xd + calibration->cx - observed->x;
|
|
const double dy = calibration->fy * yd + calibration->cy - observed->y;
|
|
*squared_error = dx * dx + dy * dy;
|
|
return std::isfinite(*squared_error);
|
|
}
|