apex-solver 1.4.0

High-performance nonlinear least squares optimization with Lie group support for SLAM and bundle adjustment
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64

#pragma once

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <string>

#include "../../common/include/ba_benchmark_utils.h"

// Ceres cost function for BAL reprojection error
// Must be in header due to C++ template requirements for automatic differentiation
struct BALReprojectionError {
    BALReprojectionError(double observed_x, double observed_y)
        : observed_x(observed_x), observed_y(observed_y) {}

    template <typename T>
    bool operator()(const T* const camera,
                    const T* const point,
                    T* residuals) const {
        // camera[0,1,2]: axis-angle rotation
        // camera[3,4,5]: translation
        // camera[6]: focal length
        // camera[7,8]: radial distortion k1, k2
        
        // Rotate point
        T p[3];
        ceres::AngleAxisRotatePoint(camera, point, p);
        
        // Add translation
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];
        
        // Project to normalized image plane
        // BAL convention: camera looks down NEGATIVE Z axis
        // Projection: p' = -P / P.z (note the negation)
        T xp = -p[0] / p[2];
        T yp = -p[1] / p[2];
        
        // Apply radial distortion
        T r2 = xp * xp + yp * yp;
        T distortion = T(1.0) + camera[7] * r2 + camera[8] * r2 * r2;
        
        // Apply focal length
        T predicted_x = camera[6] * distortion * xp;
        T predicted_y = camera[6] * distortion * yp;
        
        // Residual
        residuals[0] = predicted_x - T(observed_x);
        residuals[1] = predicted_y - T(observed_y);
        
        return true;
    }

    static ceres::CostFunction* Create(double observed_x, double observed_y) {
        return new ceres::AutoDiffCostFunction<BALReprojectionError, 2, 9, 3>(
            new BALReprojectionError(observed_x, observed_y));
    }

    double observed_x;
    double observed_y;
};

// Function declaration for Ceres bundle adjustment benchmark
benchmark_utils::BenchmarkResult BenchmarkCeres(const std::string& dataset_path);