feat(sparse-sfm): add Gate E bundle adjustment foundation
This commit is contained in:
fy59
parent
a0de28092e
commit
ca3170d7fe
4 changed files with 946 additions and 0 deletions
113
include/lardon3d/sparse_sfm_bundle_adjustment.h
Normal file
113
include/lardon3d/sparse_sfm_bundle_adjustment.h
Normal file
|
|
@ -0,0 +1,113 @@
|
|||
#ifndef LARDON3D_SPARSE_SFM_BUNDLE_ADJUSTMENT_H
|
||||
#define LARDON3D_SPARSE_SFM_BUNDLE_ADJUSTMENT_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <lardon3d/sparse_sfm_incremental.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/* COMPLETE means every eligible component was accepted, PARTIAL means accepted
|
||||
* and rejected eligible components coexist, and FAILED means none was
|
||||
* accepted. E1 defines the ABI but produces no final status before E2. */
|
||||
typedef enum {
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_COMPLETE = 0,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_PARTIAL,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_FAILED
|
||||
} Lardon3DSparseBundleAdjustmentStatus;
|
||||
|
||||
typedef enum {
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_NONE = 0,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_Y,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_Z
|
||||
} Lardon3DSparseBundleAdjustmentScaleAxis;
|
||||
|
||||
typedef enum {
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NOT_RUN = 0,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_CONVERGED,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NO_CONVERGENCE,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_FAILURE
|
||||
} Lardon3DSparseBundleAdjustmentTermination;
|
||||
|
||||
typedef enum {
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE = 0,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INPUT,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONFINITE,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NO_CONVERGENCE,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_SOLVER_FAILURE,
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_COST_REGRESSION
|
||||
} Lardon3DSparseBundleAdjustmentRejectionReason;
|
||||
|
||||
/* Both views are caller-owned and immutable for the future synchronous Gate E
|
||||
* call. incremental_result is authoritative for components, registered
|
||||
* image_id values, world-to-camera poses, landmarks and final associations.
|
||||
* images/observations are the same resolved scientific view used for Gate D.
|
||||
* An observation is identified by (feature_set_id, feature_index); its image_id
|
||||
* must agree with the final association. Source binary32 x/y become binary64
|
||||
* source-image pixels, origin top-left, +x right and +y down. Calibrations are
|
||||
* known, fixed and immutable. Each pointer is paired with its size_t count. */
|
||||
typedef struct {
|
||||
const Lardon3DSparseIncrementalResult *incremental_result;
|
||||
const Lardon3DSparseIncrementalImage *images;
|
||||
size_t image_count;
|
||||
const Lardon3DSparseIncrementalObservation *observations;
|
||||
size_t observation_count;
|
||||
} Lardon3DSparseBundleAdjustmentInput;
|
||||
|
||||
/* Counts describe one component. pose_anchor_image_id fixes the complete pose;
|
||||
* scale_anchor_image_id and scale_axis identify the fixed camera-center
|
||||
* coordinate. Costs are 0.5*sum(Huber(dx*dx+dy*dy)) with delta 2 source pixels. RMSE is
|
||||
* sqrt(sum(dx*dx+dy*dy)/observation_count) in source pixels. has_costs,
|
||||
* has_rmse and has_anchors determine whether the corresponding fields are
|
||||
* available; unavailable doubles are zero, never NaN sentinels. */
|
||||
typedef struct {
|
||||
uint64_t component_key;
|
||||
uint64_t camera_count;
|
||||
uint64_t landmark_count;
|
||||
uint64_t observation_count;
|
||||
bool eligible;
|
||||
bool has_anchors;
|
||||
uint64_t pose_anchor_image_id;
|
||||
uint64_t scale_anchor_image_id;
|
||||
Lardon3DSparseBundleAdjustmentScaleAxis scale_axis;
|
||||
bool has_costs;
|
||||
double initial_robust_cost;
|
||||
double final_robust_cost;
|
||||
bool has_rmse;
|
||||
double initial_reprojection_rmse_px;
|
||||
double final_reprojection_rmse_px;
|
||||
uint32_t iteration_count;
|
||||
Lardon3DSparseBundleAdjustmentTermination termination;
|
||||
bool accepted;
|
||||
Lardon3DSparseBundleAdjustmentRejectionReason rejection_reason;
|
||||
} Lardon3DSparseBundleAdjustmentComponentDiagnostic;
|
||||
|
||||
/* Future E2 output. All arrays are owned by the result and ordered by the
|
||||
* canonical component/image/Track/observation identities. Poses remain
|
||||
* world-to-camera binary64. A destruction function is added with the E2 run;
|
||||
* E1 intentionally exposes no fake execution function. */
|
||||
typedef struct {
|
||||
Lardon3DSparseBundleAdjustmentStatus status;
|
||||
Lardon3DSparseIncrementalComponent *components;
|
||||
size_t component_count;
|
||||
Lardon3DSparseIncrementalCamera *cameras;
|
||||
size_t camera_count;
|
||||
Lardon3DSparseIncrementalLandmark *landmarks;
|
||||
size_t landmark_count;
|
||||
Lardon3DSparseIncrementalLandmarkObservation *observations;
|
||||
size_t observation_count;
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostics;
|
||||
} Lardon3DSparseBundleAdjustmentResult;
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
14
meson.build
14
meson.build
|
|
@ -146,6 +146,7 @@ executable(
|
|||
'src/project_db.c', 'src/project_db_sparse_sfm.c',
|
||||
'src/sparse_sfm_geometry.cpp',
|
||||
'src/sparse_sfm_incremental.cpp',
|
||||
'src/sparse_sfm_bundle_adjustment.cpp',
|
||||
'src/task.c',
|
||||
'src/task_checkpoint.c',
|
||||
'src/task_kind_registry.c',
|
||||
|
|
@ -546,6 +547,19 @@ sparse_sfm_incremental_test = executable(
|
|||
|
||||
test('sparse-sfm-incremental', sparse_sfm_incremental_test, timeout: 60)
|
||||
|
||||
sparse_sfm_bundle_adjustment_test = executable(
|
||||
'test-sparse-sfm-bundle-adjustment',
|
||||
sources: [
|
||||
'tests/test_sparse_sfm_bundle_adjustment.cpp',
|
||||
'src/sparse_sfm_bundle_adjustment.cpp',
|
||||
],
|
||||
cpp_args: ['-DLARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TESTING'],
|
||||
include_directories: include_directories('include'),
|
||||
)
|
||||
|
||||
test('sparse-sfm-bundle-adjustment', sparse_sfm_bundle_adjustment_test,
|
||||
timeout: 30)
|
||||
|
||||
executable(
|
||||
'benchmark-sparse-sfm-incremental',
|
||||
sources: [
|
||||
|
|
|
|||
463
src/sparse_sfm_bundle_adjustment.cpp
Normal file
463
src/sparse_sfm_bundle_adjustment.cpp
Normal file
|
|
@ -0,0 +1,463 @@
|
|||
#include <lardon3d/sparse_sfm_bundle_adjustment.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <new>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace lardon3d::sparse_bundle_adjustment {
|
||||
|
||||
constexpr size_t maximum_images = 4096;
|
||||
constexpr size_t maximum_landmarks = 250000;
|
||||
constexpr size_t maximum_observations = 1000000;
|
||||
constexpr double depth_epsilon = 1e-9;
|
||||
constexpr double gauge_epsilon = 1e-9;
|
||||
|
||||
enum class PreparationStatus { prepared, invalid_argument, out_of_memory };
|
||||
enum class PrivateTermination { converged, no_convergence, failure };
|
||||
|
||||
struct ResolvedObservation {
|
||||
Lardon3DSparseIncrementalObservation source;
|
||||
size_t image_index;
|
||||
};
|
||||
|
||||
struct Preparation {
|
||||
std::vector<Lardon3DSparseIncrementalImage> images;
|
||||
std::vector<Lardon3DSparseIncrementalComponent> components;
|
||||
std::vector<Lardon3DSparseIncrementalCamera> cameras;
|
||||
std::vector<Lardon3DSparseIncrementalLandmark> landmarks;
|
||||
std::vector<Lardon3DSparseIncrementalLandmarkObservation> observations;
|
||||
std::vector<ResolvedObservation> resolved_observations;
|
||||
std::vector<Lardon3DSparseBundleAdjustmentComponentDiagnostic> diagnostics;
|
||||
};
|
||||
|
||||
struct ImageIndex {
|
||||
uint64_t image_id;
|
||||
size_t index;
|
||||
};
|
||||
|
||||
struct ObservationIndex {
|
||||
uint64_t feature_set_id;
|
||||
uint32_t feature_index;
|
||||
size_t index;
|
||||
};
|
||||
|
||||
bool finite_calibration(const Lardon3DSparseGeometryCalibration &value) {
|
||||
return value.width > 0 && value.height > 0 && std::isfinite(value.fx) &&
|
||||
std::isfinite(value.fy) && value.fx > 0.0 && value.fy > 0.0 &&
|
||||
std::isfinite(value.cx) && std::isfinite(value.cy) && value.cx >= 0.0 &&
|
||||
value.cy >= 0.0 && value.cx < value.width && value.cy < value.height &&
|
||||
std::isfinite(value.k1) && std::isfinite(value.k2) &&
|
||||
std::isfinite(value.p1) && std::isfinite(value.p2);
|
||||
}
|
||||
|
||||
bool finite_pose(const Lardon3DSparseGeometryPose &value) {
|
||||
for (double item : value.rotation_cw)
|
||||
if (!std::isfinite(item)) return false;
|
||||
for (double item : value.translation_cw)
|
||||
if (!std::isfinite(item)) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool valid_rotation(const Lardon3DSparseGeometryPose &value) {
|
||||
const double *r = value.rotation_cw;
|
||||
for (size_t row = 0; row < 3; ++row) {
|
||||
for (size_t column = 0; column < 3; ++column) {
|
||||
double dot = 0.0;
|
||||
for (size_t item = 0; item < 3; ++item)
|
||||
dot += r[row * 3 + item] * r[column * 3 + item];
|
||||
if (!std::isfinite(dot) ||
|
||||
std::abs(dot - (row == column ? 1.0 : 0.0)) >= 1e-6)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
const double determinant =
|
||||
r[0] * (r[4] * r[8] - r[5] * r[7]) -
|
||||
r[1] * (r[3] * r[8] - r[5] * r[6]) +
|
||||
r[2] * (r[3] * r[7] - r[4] * r[6]);
|
||||
return std::isfinite(determinant) && std::abs(determinant - 1.0) < 1e-6;
|
||||
}
|
||||
|
||||
bool finite_point(const Lardon3DSparseGeometryPoint3 &value) {
|
||||
return std::isfinite(value.x) && std::isfinite(value.y) && std::isfinite(value.z);
|
||||
}
|
||||
|
||||
bool camera_center(const Lardon3DSparseGeometryPose &pose, double center[3]) {
|
||||
if (!finite_pose(pose) || !valid_rotation(pose)) return false;
|
||||
for (size_t column = 0; column < 3; ++column) {
|
||||
center[column] = -(pose.rotation_cw[column] * pose.translation_cw[0] +
|
||||
pose.rotation_cw[3 + column] * pose.translation_cw[1] +
|
||||
pose.rotation_cw[6 + column] * pose.translation_cw[2]);
|
||||
if (!std::isfinite(center[column])) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool select_anchors(const std::vector<Lardon3DSparseIncrementalCamera> &cameras,
|
||||
uint64_t component_key,
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostic) {
|
||||
const Lardon3DSparseIncrementalCamera *pose_anchor = nullptr;
|
||||
for (const auto &camera : cameras) {
|
||||
if (camera.component_key == component_key &&
|
||||
(!pose_anchor || camera.image_id < pose_anchor->image_id))
|
||||
pose_anchor = &camera;
|
||||
}
|
||||
if (!pose_anchor) return false;
|
||||
double anchor_center[3];
|
||||
if (!camera_center(pose_anchor->pose_cw, anchor_center)) return false;
|
||||
const Lardon3DSparseIncrementalCamera *scale_anchor = nullptr;
|
||||
double best_distance = -1.0;
|
||||
double best_delta[3] = {};
|
||||
for (const auto &camera : cameras) {
|
||||
if (camera.component_key != component_key || camera.image_id == pose_anchor->image_id)
|
||||
continue;
|
||||
double center[3];
|
||||
if (!camera_center(camera.pose_cw, center)) return false;
|
||||
double delta[3] = {center[0] - anchor_center[0], center[1] - anchor_center[1],
|
||||
center[2] - anchor_center[2]};
|
||||
const double distance = std::hypot(delta[0], delta[1], delta[2]);
|
||||
if (!std::isfinite(distance)) return false;
|
||||
if (!scale_anchor || distance > best_distance ||
|
||||
(distance == best_distance && camera.image_id < scale_anchor->image_id)) {
|
||||
scale_anchor = &camera;
|
||||
best_distance = distance;
|
||||
std::copy(delta, delta + 3, best_delta);
|
||||
}
|
||||
}
|
||||
if (!scale_anchor) return false;
|
||||
size_t axis = 0;
|
||||
if (std::abs(best_delta[1]) > std::abs(best_delta[axis])) axis = 1;
|
||||
if (std::abs(best_delta[2]) > std::abs(best_delta[axis])) axis = 2;
|
||||
diagnostic->pose_anchor_image_id = pose_anchor->image_id;
|
||||
diagnostic->scale_anchor_image_id = scale_anchor->image_id;
|
||||
diagnostic->scale_axis =
|
||||
static_cast<Lardon3DSparseBundleAdjustmentScaleAxis>(axis + 1);
|
||||
diagnostic->has_anchors = std::abs(best_delta[axis]) > gauge_epsilon;
|
||||
return diagnostic->has_anchors;
|
||||
}
|
||||
|
||||
bool termination_accepted(PrivateTermination value) {
|
||||
return value == PrivateTermination::converged;
|
||||
}
|
||||
|
||||
bool project(const Lardon3DSparseGeometryCalibration &calibration,
|
||||
const Lardon3DSparseGeometryPose &pose,
|
||||
const Lardon3DSparseGeometryPoint3 &point,
|
||||
Lardon3DSparseGeometryPoint2 *pixel) {
|
||||
if (!pixel || !finite_calibration(calibration) || !finite_pose(pose) ||
|
||||
!valid_rotation(pose) || !finite_point(point))
|
||||
return false;
|
||||
const double *r = pose.rotation_cw;
|
||||
const double *t = pose.translation_cw;
|
||||
const double x = r[0] * point.x + r[1] * point.y + r[2] * point.z + t[0];
|
||||
const double y = r[3] * point.x + r[4] * point.y + r[5] * point.z + t[1];
|
||||
const double z = r[6] * point.x + r[7] * point.y + r[8] * point.z + t[2];
|
||||
if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) ||
|
||||
z <= depth_epsilon)
|
||||
return false;
|
||||
const double xn = x / z;
|
||||
const double yn = y / z;
|
||||
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;
|
||||
pixel->x = calibration.fx * xd + calibration.cx;
|
||||
pixel->y = calibration.fy * yd + calibration.cy;
|
||||
return std::isfinite(pixel->x) && std::isfinite(pixel->y);
|
||||
}
|
||||
|
||||
bool residual_metrics(const double *residuals, size_t count, double *rmse,
|
||||
double *huber_cost) {
|
||||
if (!residuals || count == 0 || !rmse || !huber_cost || count > SIZE_MAX / 2)
|
||||
return false;
|
||||
double squared_sum = 0.0;
|
||||
double rho_sum = 0.0;
|
||||
for (size_t index = 0; index < count; ++index) {
|
||||
const double dx = residuals[index * 2];
|
||||
const double dy = residuals[index * 2 + 1];
|
||||
const double squared = dx * dx + dy * dy;
|
||||
if (!std::isfinite(dx) || !std::isfinite(dy) || !std::isfinite(squared))
|
||||
return false;
|
||||
const double rho = squared <= 4.0 ? squared : 4.0 * std::sqrt(squared) - 4.0;
|
||||
squared_sum += squared;
|
||||
rho_sum += rho;
|
||||
if (!std::isfinite(rho) || !std::isfinite(squared_sum) || !std::isfinite(rho_sum))
|
||||
return false;
|
||||
}
|
||||
*rmse = std::sqrt(squared_sum / static_cast<double>(count));
|
||||
*huber_cost = 0.5 * rho_sum;
|
||||
return std::isfinite(*rmse) && std::isfinite(*huber_cost);
|
||||
}
|
||||
|
||||
bool cost_acceptable(double initial_cost, double final_cost) {
|
||||
if (!std::isfinite(initial_cost) || !std::isfinite(final_cost)) return false;
|
||||
const double tolerance = 1e-12 * std::max(1.0, std::abs(initial_cost));
|
||||
return std::isfinite(tolerance) && final_cost <= initial_cost + tolerance;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void copy_view(std::vector<T> *destination, const T *source, size_t count) {
|
||||
destination->clear();
|
||||
if (count != 0) destination->assign(source, source + count);
|
||||
}
|
||||
|
||||
PreparationStatus prepare(const Lardon3DSparseBundleAdjustmentInput &input,
|
||||
Preparation *preparation) {
|
||||
if (!preparation || !input.incremental_result)
|
||||
return PreparationStatus::invalid_argument;
|
||||
const auto &result = *input.incremental_result;
|
||||
if (result.status < LARDON3D_SPARSE_INCREMENTAL_COMPLETE ||
|
||||
result.status > LARDON3D_SPARSE_INCREMENTAL_FAILED ||
|
||||
input.image_count > maximum_images || result.camera_count > maximum_images ||
|
||||
result.landmark_count > maximum_landmarks ||
|
||||
result.observation_count > maximum_observations ||
|
||||
input.observation_count > maximum_observations ||
|
||||
(input.image_count && !input.images) ||
|
||||
(input.observation_count && !input.observations) ||
|
||||
(result.component_count && !result.components) ||
|
||||
(result.camera_count && !result.cameras) ||
|
||||
(result.landmark_count && !result.landmarks) ||
|
||||
(result.observation_count && !result.observations))
|
||||
return PreparationStatus::invalid_argument;
|
||||
try {
|
||||
Preparation candidate;
|
||||
copy_view(&candidate.images, input.images, input.image_count);
|
||||
copy_view(&candidate.components, result.components, result.component_count);
|
||||
copy_view(&candidate.cameras, result.cameras, result.camera_count);
|
||||
copy_view(&candidate.landmarks, result.landmarks, result.landmark_count);
|
||||
copy_view(&candidate.observations, result.observations, result.observation_count);
|
||||
std::sort(candidate.images.begin(), candidate.images.end(),
|
||||
[](const auto &a, const auto &b) { return a.image_id < b.image_id; });
|
||||
|
||||
std::vector<ImageIndex> images;
|
||||
images.reserve(input.image_count);
|
||||
for (size_t index = 0; index < input.image_count; ++index) {
|
||||
const auto &image = candidate.images[index];
|
||||
if (image.image_id == 0 || !finite_calibration(image.calibration) ||
|
||||
(index && candidate.images[index - 1].image_id == image.image_id))
|
||||
return PreparationStatus::invalid_argument;
|
||||
images.push_back({image.image_id, index});
|
||||
}
|
||||
|
||||
std::vector<ObservationIndex> observation_index;
|
||||
observation_index.reserve(input.observation_count);
|
||||
for (size_t index = 0; index < input.observation_count; ++index) {
|
||||
const auto &observation = input.observations[index];
|
||||
if (observation.track_id == 0 || observation.image_id == 0 ||
|
||||
observation.feature_set_id == 0 || observation.feature_count == 0 ||
|
||||
observation.feature_index >= observation.feature_count ||
|
||||
!std::isfinite(observation.x) || !std::isfinite(observation.y))
|
||||
return PreparationStatus::invalid_argument;
|
||||
auto image = std::lower_bound(
|
||||
images.begin(), images.end(), observation.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
if (image == images.end() || image->image_id != observation.image_id)
|
||||
return PreparationStatus::invalid_argument;
|
||||
observation_index.push_back(
|
||||
{observation.feature_set_id, observation.feature_index, index});
|
||||
}
|
||||
std::sort(observation_index.begin(), observation_index.end(),
|
||||
[](const auto &a, const auto &b) {
|
||||
return std::pair{a.feature_set_id, a.feature_index} <
|
||||
std::pair{b.feature_set_id, b.feature_index};
|
||||
});
|
||||
for (size_t index = 1; index < observation_index.size(); ++index) {
|
||||
if (observation_index[index - 1].feature_set_id ==
|
||||
observation_index[index].feature_set_id &&
|
||||
observation_index[index - 1].feature_index ==
|
||||
observation_index[index].feature_index)
|
||||
return PreparationStatus::invalid_argument;
|
||||
}
|
||||
|
||||
std::vector<uint64_t> component_camera_counts(result.component_count, 0);
|
||||
std::vector<uint64_t> component_landmark_counts(result.component_count, 0);
|
||||
for (size_t index = 0; index < result.component_count; ++index) {
|
||||
const auto &component = candidate.components[index];
|
||||
if (component.component_key == 0 ||
|
||||
(index && candidate.components[index - 1].component_key >=
|
||||
component.component_key) ||
|
||||
component.registered_image_count > component.image_count)
|
||||
return PreparationStatus::invalid_argument;
|
||||
}
|
||||
for (size_t index = 0; index < result.camera_count; ++index) {
|
||||
const auto &camera = candidate.cameras[index];
|
||||
if (camera.image_id == 0 ||
|
||||
(index && candidate.cameras[index - 1].image_id >= camera.image_id) ||
|
||||
!finite_pose(camera.pose_cw) || !valid_rotation(camera.pose_cw))
|
||||
return PreparationStatus::invalid_argument;
|
||||
auto component = std::lower_bound(
|
||||
candidate.components.begin(), candidate.components.end(), camera.component_key,
|
||||
[](const auto &item, uint64_t key) { return item.component_key < key; });
|
||||
auto image = std::lower_bound(
|
||||
images.begin(), images.end(), camera.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
if (component == candidate.components.end() ||
|
||||
component->component_key != camera.component_key || image == images.end() ||
|
||||
image->image_id != camera.image_id)
|
||||
return PreparationStatus::invalid_argument;
|
||||
++component_camera_counts[static_cast<size_t>(component - candidate.components.begin())];
|
||||
}
|
||||
for (size_t index = 0; index < result.landmark_count; ++index) {
|
||||
const auto &landmark = candidate.landmarks[index];
|
||||
if (landmark.landmark_id == 0 || landmark.track_id == 0 ||
|
||||
landmark.observation_count < 2 || !finite_point(landmark.point) ||
|
||||
(index && candidate.landmarks[index - 1].track_id >= landmark.track_id))
|
||||
return PreparationStatus::invalid_argument;
|
||||
auto component = std::lower_bound(
|
||||
candidate.components.begin(), candidate.components.end(), landmark.component_key,
|
||||
[](const auto &item, uint64_t key) { return item.component_key < key; });
|
||||
if (component == candidate.components.end() ||
|
||||
component->component_key != landmark.component_key)
|
||||
return PreparationStatus::invalid_argument;
|
||||
++component_landmark_counts[static_cast<size_t>(component - candidate.components.begin())];
|
||||
}
|
||||
for (size_t index = 0; index < result.component_count; ++index) {
|
||||
if (candidate.components[index].registered_image_count !=
|
||||
component_camera_counts[index] ||
|
||||
candidate.components[index].landmark_count != component_landmark_counts[index])
|
||||
return PreparationStatus::invalid_argument;
|
||||
}
|
||||
|
||||
candidate.diagnostics.resize(result.component_count);
|
||||
std::vector<uint64_t> landmark_observation_counts(result.landmark_count, 0);
|
||||
std::vector<uint8_t> published_observations(input.observation_count, 0);
|
||||
candidate.resolved_observations.reserve(result.observation_count);
|
||||
uint64_t previous_landmark = 0;
|
||||
uint32_t previous_position = 0;
|
||||
for (size_t index = 0; index < result.observation_count; ++index) {
|
||||
const auto &published = candidate.observations[index];
|
||||
const bool ordered = index == 0 || published.landmark_id > previous_landmark ||
|
||||
(published.landmark_id == previous_landmark &&
|
||||
published.position_in_track > previous_position);
|
||||
auto landmark = std::lower_bound(
|
||||
candidate.landmarks.begin(), candidate.landmarks.end(), published.track_id,
|
||||
[](const auto &item, uint64_t key) { return item.track_id < key; });
|
||||
auto observation = std::lower_bound(
|
||||
observation_index.begin(), observation_index.end(),
|
||||
std::pair{published.feature_set_id, published.feature_index},
|
||||
[](const auto &item, const auto &key) {
|
||||
return std::pair{item.feature_set_id, item.feature_index} < key;
|
||||
});
|
||||
auto image = std::lower_bound(
|
||||
images.begin(), images.end(), published.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
if (!ordered || landmark == candidate.landmarks.end() ||
|
||||
landmark->landmark_id != published.landmark_id ||
|
||||
landmark->track_id != published.track_id || observation == observation_index.end() ||
|
||||
observation->feature_set_id != published.feature_set_id ||
|
||||
observation->feature_index != published.feature_index || image == images.end() ||
|
||||
image->image_id != published.image_id)
|
||||
return PreparationStatus::invalid_argument;
|
||||
const auto &source = input.observations[observation->index];
|
||||
if (published_observations[observation->index])
|
||||
return PreparationStatus::invalid_argument;
|
||||
published_observations[observation->index] = 1;
|
||||
auto camera = std::lower_bound(
|
||||
candidate.cameras.begin(), candidate.cameras.end(), published.image_id,
|
||||
[](const auto &item, uint64_t key) { return item.image_id < key; });
|
||||
if (source.track_id != published.track_id || source.image_id != published.image_id ||
|
||||
camera == candidate.cameras.end() || camera->image_id != published.image_id ||
|
||||
camera->component_key != landmark->component_key)
|
||||
return PreparationStatus::invalid_argument;
|
||||
const size_t landmark_index = static_cast<size_t>(landmark - candidate.landmarks.begin());
|
||||
auto component = std::lower_bound(
|
||||
candidate.components.begin(), candidate.components.end(), landmark->component_key,
|
||||
[](const auto &item, uint64_t key) { return item.component_key < key; });
|
||||
const size_t component_index =
|
||||
static_cast<size_t>(component - candidate.components.begin());
|
||||
++landmark_observation_counts[landmark_index];
|
||||
++candidate.diagnostics[component_index].observation_count;
|
||||
candidate.resolved_observations.push_back({source, image->index});
|
||||
previous_landmark = published.landmark_id;
|
||||
previous_position = published.position_in_track;
|
||||
}
|
||||
for (size_t index = 0; index < candidate.landmarks.size(); ++index) {
|
||||
if (candidate.landmarks[index].observation_count !=
|
||||
landmark_observation_counts[index])
|
||||
return PreparationStatus::invalid_argument;
|
||||
}
|
||||
for (size_t index = 0; index < candidate.components.size(); ++index) {
|
||||
auto &diagnostic = candidate.diagnostics[index];
|
||||
const auto &component = candidate.components[index];
|
||||
diagnostic.component_key = component.component_key;
|
||||
diagnostic.camera_count = component.registered_image_count;
|
||||
diagnostic.landmark_count = component.landmark_count;
|
||||
diagnostic.termination = LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NOT_RUN;
|
||||
diagnostic.rejection_reason =
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_INELIGIBLE;
|
||||
diagnostic.eligible = component.registered_image_count >= 2 &&
|
||||
component.landmark_count >= 1 &&
|
||||
select_anchors(candidate.cameras, component.component_key,
|
||||
&diagnostic);
|
||||
if (diagnostic.eligible)
|
||||
diagnostic.rejection_reason = LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_NONE;
|
||||
else if (!diagnostic.has_anchors && component.registered_image_count >= 2)
|
||||
diagnostic.rejection_reason =
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE;
|
||||
}
|
||||
*preparation = std::move(candidate);
|
||||
return PreparationStatus::prepared;
|
||||
} catch (const std::bad_alloc &) {
|
||||
return PreparationStatus::out_of_memory;
|
||||
} catch (...) {
|
||||
return PreparationStatus::invalid_argument;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace lardon3d::sparse_bundle_adjustment
|
||||
|
||||
#ifdef LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TESTING
|
||||
extern "C" int lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
const Lardon3DSparseBundleAdjustmentInput *input,
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostics,
|
||||
size_t diagnostic_capacity, Lardon3DSparseIncrementalObservation *resolved,
|
||||
size_t resolved_capacity, size_t *diagnostic_count, size_t *resolved_count) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (!input || !diagnostic_count || !resolved_count) return 1;
|
||||
Preparation preparation;
|
||||
const auto status = prepare(*input, &preparation);
|
||||
if (status != PreparationStatus::prepared)
|
||||
return status == PreparationStatus::out_of_memory ? 2 : 1;
|
||||
*diagnostic_count = preparation.diagnostics.size();
|
||||
*resolved_count = preparation.resolved_observations.size();
|
||||
if ((preparation.diagnostics.size() > diagnostic_capacity) ||
|
||||
(preparation.resolved_observations.size() > resolved_capacity) ||
|
||||
(!preparation.diagnostics.empty() && !diagnostics) ||
|
||||
(!preparation.resolved_observations.empty() && !resolved))
|
||||
return 1;
|
||||
std::copy(preparation.diagnostics.begin(), preparation.diagnostics.end(), diagnostics);
|
||||
for (size_t index = 0; index < preparation.resolved_observations.size(); ++index)
|
||||
resolved[index] = preparation.resolved_observations[index].source;
|
||||
return 0;
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_project(
|
||||
const Lardon3DSparseGeometryCalibration *calibration,
|
||||
const Lardon3DSparseGeometryPose *pose,
|
||||
const Lardon3DSparseGeometryPoint3 *point,
|
||||
Lardon3DSparseGeometryPoint2 *pixel) {
|
||||
return calibration && pose && point &&
|
||||
lardon3d::sparse_bundle_adjustment::project(*calibration, *pose, *point, pixel);
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_metrics(
|
||||
const double *residuals, size_t count, double *rmse, double *huber_cost) {
|
||||
return lardon3d::sparse_bundle_adjustment::residual_metrics(
|
||||
residuals, count, rmse, huber_cost);
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_cost_acceptable(
|
||||
double initial_cost, double final_cost) {
|
||||
return lardon3d::sparse_bundle_adjustment::cost_acceptable(initial_cost, final_cost);
|
||||
}
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_termination_accepted(
|
||||
int value) {
|
||||
using namespace lardon3d::sparse_bundle_adjustment;
|
||||
if (value < 0 || value > 2) return false;
|
||||
return termination_accepted(static_cast<PrivateTermination>(value));
|
||||
}
|
||||
#endif
|
||||
356
tests/test_sparse_sfm_bundle_adjustment.cpp
Normal file
356
tests/test_sparse_sfm_bundle_adjustment.cpp
Normal file
|
|
@ -0,0 +1,356 @@
|
|||
#include <lardon3d/sparse_sfm_bundle_adjustment.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
|
||||
#define CHECK(value) \
|
||||
do { \
|
||||
if (!(value)) { \
|
||||
std::fprintf(stderr, "bundle adjustment check failed at line %d: %s\n", \
|
||||
__LINE__, #value); \
|
||||
return 1; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_termination_accepted(
|
||||
int value);
|
||||
extern "C" int lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
const Lardon3DSparseBundleAdjustmentInput *input,
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic *diagnostics,
|
||||
size_t diagnostic_capacity, Lardon3DSparseIncrementalObservation *resolved,
|
||||
size_t resolved_capacity, size_t *diagnostic_count, size_t *resolved_count);
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_project(
|
||||
const Lardon3DSparseGeometryCalibration *calibration,
|
||||
const Lardon3DSparseGeometryPose *pose,
|
||||
const Lardon3DSparseGeometryPoint3 *point,
|
||||
Lardon3DSparseGeometryPoint2 *pixel);
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_metrics(
|
||||
const double *residuals, size_t count, double *rmse, double *huber_cost);
|
||||
extern "C" bool lardon3d_sparse_bundle_adjustment_test_cost_acceptable(
|
||||
double initial_cost, double final_cost);
|
||||
|
||||
static Lardon3DSparseGeometryCalibration calibration() {
|
||||
return {1280, 960, 800.0, 810.0, 640.0, 480.0, 0.0, 0.0, 0.0, 0.0};
|
||||
}
|
||||
|
||||
static int test_helpers() {
|
||||
const Lardon3DSparseGeometryPose pose = {
|
||||
{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, {0.0, 0.0, 0.0}};
|
||||
const Lardon3DSparseGeometryPoint3 point = {0.2, -0.1, 2.0};
|
||||
auto camera = calibration();
|
||||
Lardon3DSparseGeometryPoint2 pixel = {};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_project(&camera, &pose, &point, &pixel));
|
||||
CHECK(std::abs(pixel.x - 720.0) < 1e-12);
|
||||
CHECK(std::abs(pixel.y - 439.5) < 1e-12);
|
||||
|
||||
camera.k1 = 0.1;
|
||||
camera.k2 = -0.02;
|
||||
camera.p1 = 0.003;
|
||||
camera.p2 = -0.004;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_project(&camera, &pose, &point, &pixel));
|
||||
const double xn = 0.1;
|
||||
const double yn = -0.05;
|
||||
const double r2 = xn * xn + yn * yn;
|
||||
const double radial = 1.0 + camera.k1 * r2 + camera.k2 * r2 * r2;
|
||||
const double xd = xn * radial + 2.0 * camera.p1 * xn * yn +
|
||||
camera.p2 * (r2 + 2.0 * xn * xn);
|
||||
const double yd = yn * radial + camera.p1 * (r2 + 2.0 * yn * yn) +
|
||||
2.0 * camera.p2 * xn * yn;
|
||||
CHECK(std::abs(pixel.x - (camera.fx * xd + camera.cx)) < 1e-12);
|
||||
CHECK(std::abs(pixel.y - (camera.fy * yd + camera.cy)) < 1e-12);
|
||||
|
||||
double residuals[4] = {3.0, 4.0, 0.0, 0.0};
|
||||
double rmse = 0.0;
|
||||
double cost = 0.0;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_metrics(residuals, 2, &rmse, &cost));
|
||||
CHECK(std::abs(rmse - std::sqrt(12.5)) < 1e-12);
|
||||
CHECK(cost == 8.0);
|
||||
double boundary[2] = {2.0, 0.0};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_metrics(boundary, 1, &rmse, &cost));
|
||||
CHECK(rmse == 2.0 && cost == 2.0);
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_cost_acceptable(10.0, 10.0 + 1e-11));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_cost_acceptable(10.0,
|
||||
10.0 + 2e-11));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_cost_acceptable(NAN, 0.0));
|
||||
|
||||
residuals[0] = std::numeric_limits<double>::infinity();
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_metrics(residuals, 2, &rmse, &cost));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(nullptr, &pose, &point, &pixel));
|
||||
Lardon3DSparseGeometryPoint3 behind = {0.0, 0.0, -1.0};
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(&camera, &pose, &behind,
|
||||
&pixel));
|
||||
Lardon3DSparseGeometryPoint3 depth_boundary = {0.0, 0.0, 1e-9};
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&camera, &pose, &depth_boundary, &pixel));
|
||||
depth_boundary.z = std::nextafter(1e-9, 1.0);
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&camera, &pose, &depth_boundary, &pixel));
|
||||
auto invalid_pose = pose;
|
||||
invalid_pose.rotation_cw[0] += 1e-6;
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&camera, &invalid_pose, &point, &pixel));
|
||||
auto invalid_calibration = camera;
|
||||
invalid_calibration.cx = -1.0;
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_project(
|
||||
&invalid_calibration, &pose, &point, &pixel));
|
||||
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_termination_accepted(0));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(1));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(2));
|
||||
CHECK(!lardon3d_sparse_bundle_adjustment_test_termination_accepted(3));
|
||||
return 0;
|
||||
}
|
||||
|
||||
struct Fixture {
|
||||
Lardon3DSparseIncrementalImage images[3];
|
||||
Lardon3DSparseIncrementalObservation input_observations[3];
|
||||
Lardon3DSparseIncrementalComponent components[1];
|
||||
Lardon3DSparseIncrementalCamera cameras[3];
|
||||
Lardon3DSparseIncrementalLandmark landmarks[1];
|
||||
Lardon3DSparseIncrementalLandmarkObservation result_observations[3];
|
||||
Lardon3DSparseIncrementalUnregisteredImage unregistered_images[1];
|
||||
Lardon3DSparseIncrementalResult result;
|
||||
Lardon3DSparseBundleAdjustmentInput input;
|
||||
};
|
||||
|
||||
static Fixture fixture() {
|
||||
Fixture value = {};
|
||||
const auto intrinsic = calibration();
|
||||
value.images[0] = {10, intrinsic};
|
||||
value.images[1] = {20, intrinsic};
|
||||
value.images[2] = {30, intrinsic};
|
||||
value.input_observations[0] = {7, 10, 100, 1, 2, 650.0, 480.0};
|
||||
value.input_observations[1] = {7, 20, 200, 1, 2, 500.0, 480.0};
|
||||
value.input_observations[2] = {7, 30, 300, 1, 2, 700.0, 480.0};
|
||||
value.components[0] = {10, 3, 3, 1};
|
||||
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
std::memcpy(value.cameras[0].pose_cw.rotation_cw, identity, sizeof(identity));
|
||||
std::memcpy(value.cameras[1].pose_cw.rotation_cw, identity, sizeof(identity));
|
||||
std::memcpy(value.cameras[2].pose_cw.rotation_cw, identity, sizeof(identity));
|
||||
value.cameras[0].image_id = 10;
|
||||
value.cameras[1].image_id = 20;
|
||||
value.cameras[2].image_id = 30;
|
||||
for (auto &camera : value.cameras) camera.component_key = 10;
|
||||
value.cameras[1].pose_cw.translation_cw[0] = -2.0;
|
||||
value.cameras[1].pose_cw.translation_cw[1] = -2.0;
|
||||
value.cameras[2].pose_cw.translation_cw[0] = 2.0;
|
||||
value.cameras[2].pose_cw.translation_cw[1] = 2.0;
|
||||
value.landmarks[0] = {1, 7, 10, {0.0, 0.0, 5.0}, 0.5, 0.4, 3};
|
||||
for (size_t index = 0; index < 3; ++index) {
|
||||
value.result_observations[index] = {
|
||||
1, 7, value.images[index].image_id,
|
||||
value.input_observations[index].feature_set_id,
|
||||
value.input_observations[index].feature_index,
|
||||
static_cast<uint32_t>(index)};
|
||||
}
|
||||
value.result.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE;
|
||||
value.result.track_set_id = 77;
|
||||
value.result.calibration_scope_id = 88;
|
||||
value.result.component_count = 1;
|
||||
value.result.camera_count = 3;
|
||||
value.result.landmark_count = 1;
|
||||
value.result.observation_count = 3;
|
||||
value.unregistered_images[0] = {99, 10};
|
||||
value.result.unregistered_image_count = 1;
|
||||
value.result.seed_candidates_considered = 3;
|
||||
value.result.seed_candidates_available = 4;
|
||||
value.result.seed_image_a = 10;
|
||||
value.result.seed_image_b = 20;
|
||||
value.result.last_seed_geometry_status = 2;
|
||||
value.result.last_seed_parallax_rad = 0.25;
|
||||
value.result.registration_rounds = 5;
|
||||
value.result.registration_attempts = 6;
|
||||
value.result.registration_successes = 3;
|
||||
value.result.registration_failures = 3;
|
||||
value.result.last_pnp_inlier_count = 8;
|
||||
value.result.triangulation_attempts = 9;
|
||||
value.result.triangulation_failures = 1;
|
||||
value.result.rejected_behind_camera = 2;
|
||||
value.result.rejected_reprojection = 3;
|
||||
value.result.rejected_landmarks = 4;
|
||||
value.result.last_triangulation_status = 5;
|
||||
value.result.landmark_update_attempts = 6;
|
||||
value.result.landmark_update_successes = 7;
|
||||
value.result.landmark_update_failures = 8;
|
||||
value.result.no_growth_terminations = 9;
|
||||
value.result.round_limit_terminations = 10;
|
||||
value.result.point_refinement_attempts = 11;
|
||||
value.result.point_refinement_successes = 12;
|
||||
return value;
|
||||
}
|
||||
|
||||
static void bind_fixture(Fixture *value) {
|
||||
value->result.components = value->components;
|
||||
value->result.cameras = value->cameras;
|
||||
value->result.landmarks = value->landmarks;
|
||||
value->result.observations = value->result_observations;
|
||||
value->result.unregistered_images = value->unregistered_images;
|
||||
value->input = {&value->result, value->images, 3, value->input_observations, 3};
|
||||
}
|
||||
|
||||
static int test_preparation() {
|
||||
Fixture value = fixture();
|
||||
bind_fixture(&value);
|
||||
const Fixture original = value;
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostics[1] = {};
|
||||
Lardon3DSparseIncrementalObservation resolved[3] = {};
|
||||
size_t diagnostic_count = 0;
|
||||
size_t resolved_count = 0;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 0);
|
||||
CHECK(diagnostic_count == 1 && resolved_count == 3);
|
||||
CHECK(resolved[1].image_id == 20 && resolved[1].x == 500.0);
|
||||
CHECK(diagnostics[0].eligible);
|
||||
CHECK(diagnostics[0].pose_anchor_image_id == 10);
|
||||
CHECK(diagnostics[0].scale_anchor_image_id == 20);
|
||||
CHECK(diagnostics[0].scale_axis ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X);
|
||||
CHECK(diagnostics[0].termination ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_TERMINATION_NOT_RUN);
|
||||
CHECK(!diagnostics[0].has_costs && !diagnostics[0].has_rmse);
|
||||
CHECK(!diagnostics[0].accepted);
|
||||
CHECK(std::memcmp(&value.result, &original.result, sizeof(value.result)) == 0);
|
||||
CHECK(std::memcmp(value.images, original.images, sizeof(value.images)) == 0);
|
||||
CHECK(std::memcmp(value.input_observations, original.input_observations,
|
||||
sizeof(value.input_observations)) == 0);
|
||||
CHECK(std::memcmp(value.cameras, original.cameras, sizeof(value.cameras)) == 0);
|
||||
CHECK(std::memcmp(value.landmarks, original.landmarks, sizeof(value.landmarks)) == 0);
|
||||
CHECK(std::memcmp(value.components, original.components, sizeof(value.components)) == 0);
|
||||
CHECK(std::memcmp(value.result_observations, original.result_observations,
|
||||
sizeof(value.result_observations)) == 0);
|
||||
CHECK(std::memcmp(value.unregistered_images, original.unregistered_images,
|
||||
sizeof(value.unregistered_images)) == 0);
|
||||
|
||||
value = fixture();
|
||||
bind_fixture(&value);
|
||||
value.cameras[1].pose_cw.translation_cw[0] = -1e-9;
|
||||
value.cameras[1].pose_cw.translation_cw[1] = 0.0;
|
||||
value.cameras[2].pose_cw.translation_cw[0] = 1e-9;
|
||||
value.cameras[2].pose_cw.translation_cw[1] = 0.0;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 0);
|
||||
CHECK(!diagnostics[0].eligible);
|
||||
CHECK(diagnostics[0].rejection_reason ==
|
||||
LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_REJECTION_GAUGE_DEGENERATE);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int test_invalid() {
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostics[1] = {};
|
||||
Lardon3DSparseIncrementalObservation resolved[3] = {};
|
||||
size_t diagnostic_count = 0;
|
||||
size_t resolved_count = 0;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
nullptr, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
Fixture value = fixture();
|
||||
bind_fixture(&value);
|
||||
value.input_observations[1].feature_set_id = 999;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
value = fixture();
|
||||
bind_fixture(&value);
|
||||
value.input_observations[1].image_id = 30;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
value = fixture();
|
||||
bind_fixture(&value);
|
||||
value.input_observations[1].x = NAN;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
value = fixture();
|
||||
bind_fixture(&value);
|
||||
value.input_observations[1].feature_set_id = value.input_observations[0].feature_set_id;
|
||||
value.input_observations[1].feature_index = value.input_observations[0].feature_index;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
value = fixture();
|
||||
bind_fixture(&value);
|
||||
std::swap(value.cameras[0], value.cameras[1]);
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
value = fixture();
|
||||
bind_fixture(&value);
|
||||
value.input.image_count = 4097;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&value.input, diagnostics, 1, resolved, 3, &diagnostic_count,
|
||||
&resolved_count) == 1);
|
||||
|
||||
Lardon3DSparseIncrementalResult empty = {};
|
||||
empty.status = LARDON3D_SPARSE_INCREMENTAL_FAILED;
|
||||
Lardon3DSparseBundleAdjustmentInput empty_input = {&empty, nullptr, 0, nullptr, 0};
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&empty_input, nullptr, 0, nullptr, 0, &diagnostic_count,
|
||||
&resolved_count) == 0);
|
||||
CHECK(diagnostic_count == 0 && resolved_count == 0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int test_multiple_components() {
|
||||
const auto intrinsic = calibration();
|
||||
Lardon3DSparseIncrementalImage images[4] = {
|
||||
{10, intrinsic}, {20, intrinsic}, {30, intrinsic}, {40, intrinsic}};
|
||||
Lardon3DSparseIncrementalObservation source_observations[4] = {
|
||||
{7, 10, 100, 1, 2, 640.0, 480.0},
|
||||
{7, 20, 200, 1, 2, 600.0, 480.0},
|
||||
{8, 30, 300, 1, 2, 640.0, 480.0},
|
||||
{8, 40, 400, 1, 2, 600.0, 480.0}};
|
||||
Lardon3DSparseIncrementalComponent components[2] = {
|
||||
{10, 2, 2, 1}, {30, 2, 2, 1}};
|
||||
Lardon3DSparseIncrementalCamera cameras[4] = {};
|
||||
const double identity[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
|
||||
for (size_t index = 0; index < 4; ++index) {
|
||||
cameras[index].image_id = images[index].image_id;
|
||||
cameras[index].component_key = index < 2 ? 10 : 30;
|
||||
std::memcpy(cameras[index].pose_cw.rotation_cw, identity, sizeof(identity));
|
||||
}
|
||||
cameras[1].pose_cw.translation_cw[0] = -1.0;
|
||||
cameras[3].pose_cw.translation_cw[1] = -1.0;
|
||||
Lardon3DSparseIncrementalLandmark landmarks[2] = {
|
||||
{1, 7, 10, {0.0, 0.0, 5.0}, 0.5, 0.4, 2},
|
||||
{2, 8, 30, {0.0, 0.0, 6.0}, 0.5, 0.4, 2}};
|
||||
Lardon3DSparseIncrementalLandmarkObservation observations[4] = {
|
||||
{1, 7, 10, 100, 1, 0}, {1, 7, 20, 200, 1, 1},
|
||||
{2, 8, 30, 300, 1, 0}, {2, 8, 40, 400, 1, 1}};
|
||||
Lardon3DSparseIncrementalResult result = {};
|
||||
result.status = LARDON3D_SPARSE_INCREMENTAL_COMPLETE;
|
||||
result.components = components;
|
||||
result.component_count = 2;
|
||||
result.cameras = cameras;
|
||||
result.camera_count = 4;
|
||||
result.landmarks = landmarks;
|
||||
result.landmark_count = 2;
|
||||
result.observations = observations;
|
||||
result.observation_count = 4;
|
||||
Lardon3DSparseBundleAdjustmentInput input = {
|
||||
&result, images, 4, source_observations, 4};
|
||||
Lardon3DSparseBundleAdjustmentComponentDiagnostic diagnostics[2] = {};
|
||||
Lardon3DSparseIncrementalObservation resolved[4] = {};
|
||||
size_t diagnostic_count = 0;
|
||||
size_t resolved_count = 0;
|
||||
CHECK(lardon3d_sparse_bundle_adjustment_test_prepare(
|
||||
&input, diagnostics, 2, resolved, 4, &diagnostic_count,
|
||||
&resolved_count) == 0);
|
||||
CHECK(diagnostic_count == 2 && resolved_count == 4);
|
||||
CHECK(diagnostics[0].component_key == 10 && diagnostics[0].observation_count == 2);
|
||||
CHECK(diagnostics[1].component_key == 30 && diagnostics[1].observation_count == 2);
|
||||
CHECK(diagnostics[0].scale_axis == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_X);
|
||||
CHECK(diagnostics[1].scale_axis == LARDON3D_SPARSE_BUNDLE_ADJUSTMENT_AXIS_Y);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main() {
|
||||
if (test_helpers() != 0) return 1;
|
||||
if (test_preparation() != 0) return 1;
|
||||
if (test_invalid() != 0) return 1;
|
||||
return test_multiple_components();
|
||||
}
|
||||
Loading…
Reference in a new issue