diff --git a/include/lardon3d/sparse_sfm_bundle_adjustment.h b/include/lardon3d/sparse_sfm_bundle_adjustment.h new file mode 100644 index 0000000..4f332c6 --- /dev/null +++ b/include/lardon3d/sparse_sfm_bundle_adjustment.h @@ -0,0 +1,113 @@ +#ifndef LARDON3D_SPARSE_SFM_BUNDLE_ADJUSTMENT_H +#define LARDON3D_SPARSE_SFM_BUNDLE_ADJUSTMENT_H + +#include +#include +#include + +#include + +#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 diff --git a/meson.build b/meson.build index c4dfc1b..d04019a 100644 --- a/meson.build +++ b/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: [ diff --git a/src/sparse_sfm_bundle_adjustment.cpp b/src/sparse_sfm_bundle_adjustment.cpp new file mode 100644 index 0000000..0708591 --- /dev/null +++ b/src/sparse_sfm_bundle_adjustment.cpp @@ -0,0 +1,463 @@ +#include + +#include +#include +#include +#include +#include + +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 images; + std::vector components; + std::vector cameras; + std::vector landmarks; + std::vector observations; + std::vector resolved_observations; + std::vector 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 &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(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(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 +void copy_view(std::vector *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 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 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 component_camera_counts(result.component_count, 0); + std::vector 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(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(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 landmark_observation_counts(result.landmark_count, 0); + std::vector 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(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(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(value)); +} +#endif diff --git a/tests/test_sparse_sfm_bundle_adjustment.cpp b/tests/test_sparse_sfm_bundle_adjustment.cpp new file mode 100644 index 0000000..2b4a381 --- /dev/null +++ b/tests/test_sparse_sfm_bundle_adjustment.cpp @@ -0,0 +1,356 @@ +#include + +#include +#include +#include +#include + +#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::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(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(); +}