pyvale 2026.1.1__cp311-cp311-manylinux_2_17_i686.manylinux2014_i686.whl

pyvale/calib/cpp/calibopt.cpp

@@ -0,0 +1,347 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+// STD library Header files
+#include <iostream>
+#include <cstring>
+#include <omp.h>
+#include <ostream>
+#include <iomanip>
+#include <vector>
+
+// common_cpp Header files
+#include "../../common_cpp/defines.hpp"
+
+// Eigen Header Files
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+// pyvale header files
+#include "./calibopt.hpp"
+
+
+namespace optimization {
+
+    optimization::Output bundle_adjustment(Parameters &opt, const std::vector<double> &dots_cam0, const std::vector<double> &dots_cam1, 
+                                           const std::vector<double> &grid, const size_t num_img, const std::vector<int> &lengths){
+
+        int iter = 0;
+        double ftol = 0;
+        double xtol = 0;
+        bool converged = false;
+        opt.lambda = 0.001;
+
+        while (iter < opt.max_iter) {
+
+            // calculate updated parameters
+            iterate_cost(opt, dots_cam0, dots_cam1, grid, num_img, lengths);
+
+            ftol = std::abs(opt.costpdp - opt.costp);
+
+            std::cout << iter << " " << opt.costp << " " << opt.costpdp << " " << ftol << std::endl;
+
+            if ((xtol < opt.precision) && (ftol < opt.precision)) {
+                converged=true;
+                break;
+            }
+            iter++;
+        }
+
+        //return the residuals from the final iteration 
+        opt.p = opt.pdp;
+        optimization::Output final_results = calc_residuals(opt.p, dots_cam0, dots_cam1, grid, num_img, lengths, false);
+
+        return final_results;
+    }
+
+
+    void iterate_cost(Parameters &opt, const std::vector<double> &dots_cam0, const std::vector<double> &dots_cam1, 
+                    const std::vector<double> &grid, const size_t num_img, const std::vector<int> &lengths){
+
+
+        // Compute residuals at current point. p get updated in this
+        optimization::Output res = calc_residuals(opt.p, dots_cam0, dots_cam1, grid, num_img, lengths, false);
+
+        // calculate jacobian
+        Eigen::MatrixXd J = calc_jac(opt.p, res.residuals, dots_cam0, dots_cam1, grid, num_img, lengths);
+
+
+        // calc gradient
+        Eigen::VectorXd g = J.transpose() * res.residuals;
+
+        // Hessian
+        Eigen::MatrixXd H = J.transpose() * J;
+
+        // (H + lambda*Identity)
+        Eigen::MatrixXd A = H + opt.lambda * Eigen::MatrixXd::Identity(H.rows(), H.cols());
+
+        // get change in parameters
+        Eigen::VectorXd dp = A.ldlt().solve(-g);
+
+        // Updated parameters
+        for (int i = 0; i < opt.p.size(); i++) {
+            opt.pdp[i] = opt.p[i] + dp(i);
+        }
+
+
+        // Evaluate new cost
+        optimization::Output res_new = calc_residuals(opt.pdp, dots_cam0, dots_cam1, grid, num_img, lengths, false);
+        opt.costp = 0.0;
+        opt.costpdp = 0.0;
+        for (int i = 0; i < res_new.residuals.size(); i++){
+            opt.costp   += res.residuals(i) * res.residuals(i);
+            opt.costpdp += res_new.residuals(i) * res_new.residuals(i);
+        }
+
+        //std::cout << opt.costp << " " << opt.costpdp << std::endl;
+            
+        // Accept or reject step
+        if (opt.costpdp < opt.costp) {
+            opt.p = opt.pdp;
+            opt.lambda *= 0.1;
+        } 
+        else {
+            opt.lambda *= 10.0;
+        }
+
+    }
+
+
+    // Function to convert Rodrigues rotation vector to rotation matrix using Eigen
+    Eigen::Matrix3d rodrigues_to_matrix(const Eigen::Vector3d &rvec) {
+
+        double theta = rvec.norm();
+
+        // handle tiny angles
+        if (theta < 1e-10) {
+            return Eigen::Matrix3d::Identity();
+        }
+
+        // Normalise
+        Eigen::Vector3d axis = rvec / theta;
+
+        // rotation
+        Eigen::AngleAxisd rotation(theta, axis);
+        return rotation.toRotationMatrix();
+    }
+
+
+
+    // Project 3D points to 2D with distortion (assuming radial + tangential distortion)
+    std::vector<double> project_points(const std::vector<Eigen::Vector3d> &gridpoints_3d,
+                                                const Eigen::Vector3d &rvec,
+                                                const Eigen::Vector3d &tvec,
+                                                const Eigen::Matrix3d &K,
+                                                const Eigen::VectorXd &D) {
+
+
+        Eigen::Matrix3d R = rodrigues_to_matrix(rvec);
+        std::vector<double> projected(gridpoints_3d.size()*2);
+        int count = 0;
+
+        for (const auto& point : gridpoints_3d) {
+
+            // Transform to camera coordinates
+            Eigen::Vector3d p_cam = R * point + tvec;
+
+            // Normalize
+            const double x = p_cam(0) / p_cam(2);
+            const double y = p_cam(1) / p_cam(2);
+
+            // Apply distortion
+            const double r2 = x*x + y*y;
+            const double r4 = r2*r2;
+            const double r6 = r4*r2;
+
+            // Radial distortion
+            const double radial = 1 + D(0)*r2 + D(1)*r4 + D(4)*r6;
+
+            // Tangential distortion
+            const double dx = 2*D(2)*x*y + D(3)*(r2 + 2*x*x);
+            const double dy = D(2)*(r2 + 2*y*y) + 2*D(3)*x*y;
+
+            const double x_distorted = x * radial + dx;
+            const double y_distorted = y * radial + dy;
+
+            // Project to image coordinates
+            projected[2*count+0] = K(0,0) * x_distorted + K(0,2);
+            projected[2*count+1] = K(1,1) * y_distorted + K(1,2);
+            count++;
+        }
+        return projected;
+    }
+
+
+
+
+        optimization::Output calc_residuals(std::vector<double> &p, const std::vector<double> &dots_cam0,
+                                            const std::vector<double> &dots_cam1, const std::vector<double> &grid, 
+                                            const size_t num_img, const std::vector<int> &lengths,
+                                            const bool print_flag){
+
+        // ------------------------------------------------------
+        // unpack parameters
+        // ------------------------------------------------------
+
+
+        // Camera matrices
+        Eigen::Matrix3d K0, K1;
+        K0 << p[0], 0,  p[2], 0,  p[1],  p[3], 0, 0, 1;
+        K1 << p[9], 0, p[11], 0, p[10], p[12], 0, 0, 1;
+
+        // cam 0 distortion parameters
+        Eigen::VectorXd D0(5);
+        for (int i = 0; i < 5; i++) D0(i) = p[4 + i];
+
+        // cam1 distortion parameters
+        Eigen::VectorXd D1(5);
+        for (int i = 0; i < 5; i++) D1(i) = p[13 + i];
+
+
+        // Stereo translation and rotation
+        Eigen::Vector3d rvec_stereo(p[18], p[19], p[20]);
+        Eigen::Vector3d tvec_stereo(p[21], p[22], p[23]);
+        Eigen::Matrix3d R_stereo = rodrigues_to_matrix(rvec_stereo);
+
+        //cam0 projections start at element 24
+        int start_cam0 = 24;
+
+        // init residuals
+        Eigen::VectorXd residuals(2*dots_cam0.size());
+        std::vector<double> proj0(2*dots_cam0.size());
+        std::vector<double> proj1(2*dots_cam0.size());
+
+        std::vector<int> img_offsets(num_img);
+        int count = 0;
+        for (size_t i = 0; i < num_img; i++) {
+            img_offsets[i] = count;
+            count += lengths[i];
+        }
+
+
+        // loop over all imgs in stereo_calibration
+        #pragma omp parallel for
+        for (int i = 0; i < num_img; i++){
+
+            // rotation vector
+            Eigen::Vector3d rvec0(p[start_cam0 + i*6],
+                                  p[start_cam0 + i*6 + 1],
+                                  p[start_cam0 + i*6 + 2]);
+
+            // translation vector
+            Eigen::Vector3d tvec0(p[start_cam0 + i*6 + 3],
+                                  p[start_cam0 + i*6 + 4],
+                                  p[start_cam0 + i*6 + 5]);
+
+            // rotation matrix
+            Eigen::Matrix3d R0 = rodrigues_to_matrix(rvec0);
+
+            // Cam1 pose (derived from cam0 + stereo)
+            Eigen::Matrix3d R1 = R_stereo * R0;
+            Eigen::Vector3d T1 = R_stereo * tvec0 + tvec_stereo;
+
+            // Convert R1 back to rotation vector for projection
+            // Simple conversion (can be improved for numerical stability)
+            Eigen::Vector3d rvec1;
+            double trace = R1.trace();
+            double angle = acos((trace - 1) / 2);
+            if (angle < 1e-8) {
+                rvec1.setZero();
+            }
+            else {
+                Eigen::Vector3d axis;
+                axis(0) = R1(2,1) - R1(1,2);
+                axis(1) = R1(0,2) - R1(2,0);
+                axis(2) = R1(1,0) - R1(0,1);
+                axis.normalize();
+                rvec1 = angle * axis;
+            }
+
+            //convert grid for this image to a vector of eigen 3d points
+            std::vector<Eigen::Vector3d> grid_img_i(lengths[i]);
+            int idx_start_3d = 0;
+            int idx_start_2d = 0;
+
+            //get start index of the grid for this image
+            for (int j = 0; j < i; j++){
+                idx_start_3d+=3*lengths[j];
+                idx_start_2d+=2*lengths[j];
+            }
+
+            for (int j = 0; j < lengths[i]; j++){
+                grid_img_i[j](0) = grid[idx_start_3d+j*3+0];
+                grid_img_i[j](1) = grid[idx_start_3d+j*3+1];
+                grid_img_i[j](2) = grid[idx_start_3d+j*3+2];
+            }
+
+            // Projection
+            std::vector<double> proj0_i = project_points(grid_img_i, rvec0, tvec0, K0, D0);
+            std::vector<double> proj1_i = project_points(grid_img_i, rvec1, T1, K1, D1);
+
+            // offset in results arrays for local copy
+            int local_offset = img_offsets[i];
+
+            // residuals
+            for (size_t j = 0; j < lengths[i]; j++) {
+
+                //global index
+                int global_idx = local_offset+j;
+
+                if (print_flag) {
+                    std::cout << std::setprecision(10) << dots_cam0[idx_start_2d+2*j+0] << " " << dots_cam0[idx_start_2d+2*j+1] << " ";
+                    std::cout << std::setprecision(10) << proj0_i[2*j+0] << " " << proj0_i[2*j+1] << std::endl;
+                }
+
+                // residuals
+                residuals[4*global_idx+0] = proj0_i[2*j+0] - dots_cam0[idx_start_2d+2*j+0];
+                residuals[4*global_idx+1] = proj0_i[2*j+1] - dots_cam0[idx_start_2d+2*j+1];
+                residuals[4*global_idx+2] = proj1_i[2*j+0] - dots_cam1[idx_start_2d+2*j+0];
+                residuals[4*global_idx+3] = proj1_i[2*j+1] - dots_cam1[idx_start_2d+2*j+1];
+
+                // populate master reprojection array for every image
+                proj0[2*global_idx+0] = proj0_i[2*j+0];
+                proj0[2*global_idx+1] = proj0_i[2*j+1];
+                proj1[2*global_idx+0] = proj1_i[2*j+0];
+                proj1[2*global_idx+1] = proj1_i[2*j+1];
+
+                // increment count
+                count++;
+            }
+            if (print_flag) std::cout << std::endl;
+        }
+        return {residuals, proj0, proj1};
+    }
+
+
+    Eigen::MatrixXd calc_jac(std::vector<double> &p, const Eigen::VectorXd &r, const std::vector<double> &dots_cam0, const std::vector<double> &dots_cam1,
+                            const std::vector<double> &grid, const size_t num_img, const std::vector<int> &lengths){
+
+        const int m = r.size();
+        const int n = p.size();
+        const double h = 1e-8;
+
+        Eigen::MatrixXd jac(m,n);
+
+
+        // perturb one parameter at a time
+        #pragma omp parallel for
+        for (int j = 0; j < n; j++) {
+            std::vector<double> p_prime = p;
+            p_prime[j] += h;
+
+            auto [r_prime, proj0, proj1] = calc_residuals(p_prime, dots_cam0, dots_cam1, grid, num_img, lengths, false);
+
+            for (int i = 0; i < m; i++) {
+                jac(i, j) = (r_prime[i] - r[i]) / h;
+            }
+        }
+        return jac;
+
+
+    }
+
+}
+

pyvale/calib/cpp/calibopt.hpp

@@ -0,0 +1,84 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+#ifndef CALIBOPT_H
+#define CALIBOPT_H
+
+// STD library Header files
+#include <iostream>
+#include <cstring>
+#include <locale>
+#include <omp.h>
+#include <vector>
+
+// common_cpp header filesfiles
+#include <Eigen/Dense>
+
+
+namespace optimization {
+
+    struct Parameters {
+        int num_img;
+        int num_params;
+        double lambda; // damping
+        double costp; // cost function for current P values
+        double costpdp; // cost function for P+deltaP values
+        std::vector<double> p; // hard coded optimzation parameters
+        std::vector<double> dp; // deltaP
+        std::vector<double> pdp; // P + deltaP
+        int max_iter;
+        double precision;
+        int px_vert;
+        int px_hori;
+        Eigen::MatrixXd jac;
+        Eigen::MatrixXd H;
+
+
+        // Constructor to initialize vectors and other parameters
+        Parameters(int num_params_, int max_iter_, 
+                double precision_, int px_vert_, int px_hori_)
+            :
+            num_params(num_params_),
+            lambda(0.01),
+            costp(0.0),
+            costpdp(0.0),
+            p(num_params, 0.0),
+            dp(num_params, 0.0),
+            pdp(num_params, 0.0),
+            max_iter(max_iter_),
+            precision(precision_),
+            px_vert(px_vert_),
+            px_hori(px_hori_) {}
+    };
+
+
+    struct Output {
+        Eigen::VectorXd residuals;
+        std::vector<double> proj0;
+        std::vector<double> proj1;
+    };
+
+    // master optimization routine
+    optimization::Output bundle_adjustment(Parameters &opt, const std::vector<double> &dots_cam0, const std::vector<double> &dots_cam1,
+                            const std::vector<double> &grid, const size_t num_img, const std::vector<int> &lengths);
+
+    // single iteration of optimization
+    void iterate_cost(Parameters &opt, const std::vector<double> &dots_cam0, const std::vector<double> &dots_cam1, 
+                    const std::vector<double> &grid, const size_t num_img, const std::vector<int> &lengths);
+
+    // calculate jacobian
+    Eigen::MatrixXd calc_jac(std::vector<double> &p, const Eigen::VectorXd &r, const std::vector<double> &dots_cam0, const std::vector<double> &dots_cam1, 
+                            const std::vector<double> &grid, const size_t num_img, const std::vector<int> &lengths);
+
+    // calculate residuals
+    optimization::Output calc_residuals(std::vector<double> &p, const std::vector<double> &dots_cam0, 
+                                const std::vector<double> &dots_cam1, const std::vector<double> &grid, 
+                                const size_t num_img,  const std::vector<int> &lengths, const bool print_flag);
+
+
+}
+
+#endif // CALIBOPT_H

pyvale/calib/cpp/calibstereo.cpp

@@ -0,0 +1,95 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+// STD library Header files
+#include <iostream>
+#include <cstring>
+#include <omp.h>
+#include <vector>
+
+// Eigen header files
+#include <Eigen/Core>
+
+// pyvale header files
+#include "./calibopt.hpp"
+#include "./calibstereo.hpp"
+
+
+void stereo_calibration(const std::vector<double> &init_params,
+                        const std::vector<double> &dots_cam0,
+                        const std::vector<double> &dots_cam1,
+                        const std::vector<double> &grid,
+                        const std::vector<int> &lengths,
+                        const int px_hori, const int px_vert, const int num_img){
+
+    int num_params = 4*2 + 5*2 + 3 + 3 + 6*num_img;
+    optimization::Parameters opt(init_params.size(), 100, 0.001, px_hori, px_vert);
+
+    // assign initial guess for parameter values
+    for (int i = 0; i < init_params.size(); i++){
+        opt.p[i] = init_params[i];
+    }
+
+    // run optimization routine
+    optimization::Output output = optimization::bundle_adjustment(opt, dots_cam0, dots_cam1, grid, num_img, lengths);
+
+    // calculate the error for each image based on the final residuals
+    std::vector<double> err0(num_img,0.0);
+    std::vector<double> err1(num_img,0.0);
+
+    std::string formulation = "MatchID";
+
+    int img_start = 0;
+    for (int img = 0; img < num_img; img++){
+        for (int d = 0; d < lengths[img]; d++){
+
+            const int idx_x = img_start+2*d+0;
+            const int idx_y = img_start+2*d+1;
+
+
+            // length diff for cam0
+            const double dx0 = output.proj0[idx_x] - dots_cam0[idx_x];
+            const double dy0 = output.proj0[idx_y] - dots_cam0[idx_y];
+
+            // length diff for cam1
+            const double dx1 = output.proj1[idx_x] - dots_cam1[idx_x];
+            const double dy1 = output.proj1[idx_y] - dots_cam1[idx_y];
+
+            if (formulation=="MatchID"){
+                err0[img] += (dx0*dx0 + dy0*dy0)/(2.0*lengths[img]);
+                err1[img] += (dx1*dx1 + dy1*dy1)/(2.0*lengths[img]);
+            }
+            else if (formulation=="RMS"){
+                err0[img] += (dx0*dx0 + dy0*dy0)/(lengths[img]);
+                err1[img] += (dx1*dx1 + dy1*dy1)/(lengths[img]);
+            }
+            else if (formulation=="mean"){
+                err0[img] += std::sqrt(dx0*dx0 + dy0*dy0)/(lengths[img]);
+                err1[img] += std::sqrt(dx1*dx1 + dy1*dy1)/(lengths[img]);
+            }
+            else {
+                std::cout << "Unknown Reprojection Error formulation: '" << formulation << "'." << std::endl;
+                std::cout << "Allowed options: 'MatchID', 'RMS', 'mean'." << std::endl;
+            }
+
+
+
+        }
+
+        if (formulation=="RMS"){
+            err0[img] = std::sqrt(err0[img]);
+            err1[img] = std::sqrt(err1[img]);
+        }
+
+        img_start += lengths[img];
+        std::cout << "error image " << img << ": " << err0[img] << " (L) " << err1[img] << " (R) " << std::endl;
+    }
+}
+
+
+
+
+

pyvale/calib/cpp/calibstereo.hpp

@@ -0,0 +1,27 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+
+#ifndef CALIBSTEREO_H
+#define CALIBSTEREO_H
+
+
+// STD library Header files
+#include <iostream>
+#include <cstring>
+#include <omp.h>
+#include <vector>
+
+// master func that gets called from python
+void stereo_calibration(const std::vector<double> &init_params,
+                        const std::vector<double> &dots_cam0,
+                        const std::vector<double> &dots_cam1,
+                        const std::vector<double> &grid,
+                        const std::vector<int> &lengths,
+                        const int px_hori, const int px_vert, const int num_img_pairs);
+
+
+#endif

pyvale/common_cpp/__init__.py

@@ -0,0 +1,5 @@
+#===============================================================================
+# pyvale: the python validation engine
+# License: MIT
+# Copyright (C) 2025 The Computer Aided Validation Team
+#===============================================================================

pyvale/common_cpp/bindings.cpp

@@ -0,0 +1,33 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+// pybind header files
+#include <pybind11/pybind11.h>
+#include <pybind11/numpy.h>
+#include <pybind11/stl.h>
+#include <pybind11/iostream.h>
+
+// common_cpp Header Files
+#include "./util.hpp"
+
+namespace py = pybind11;
+
+PYBIND11_MODULE(common_cpp, m) {
+
+    py::add_ostream_redirect(m, "ostream_redirect");
+
+    py::class_<common_util::SaveConfig>(m, "SaveConfig")
+        .def(py::init<>())
+        .def_readwrite("basepath", &common_util::SaveConfig::basepath)
+        .def_readwrite("binary", &common_util::SaveConfig::binary)
+        .def_readwrite("prefix", &common_util::SaveConfig::prefix)
+        .def_readwrite("delimiter", &common_util::SaveConfig::delimiter)
+        .def_readwrite("at_end", &common_util::SaveConfig::at_end)
+        .def_readwrite("output_below_threshold", &common_util::SaveConfig::output_below_threshold)
+        .def_readwrite("shape_params", &common_util::SaveConfig::shape_params);
+
+    m.def("set_num_threads", &common_util::set_num_threads, "Set number of OMP threads");
+}

pyvale/common_cpp/defines.hpp

@@ -0,0 +1,39 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+// defines.hpp
+#ifndef DEFINES_H
+#define DEFINES_H
+
+#define TITLE(a) std::cout << std::string(80, '-') << std::endl; std::cout << std::string(38 - std::strlen(a)/2, '-') << " " << a << " " << std::string(38 - std::strlen(a)/2, '-') << std::endl; std::cout << std::string(80, '-') << std::endl;
+#define INFO_OUT(a,b) std::cout << "  - " << std::left << std::setw(50) << a << b << std::endl;
+#define DEBUGGER std::cout << __FILE__ << " " << __LINE__ << std::endl;
+
+#ifdef CUDA
+        #define CUDA_CALL(x) do { if((x) != cudaSuccess) {\
+        printf("Error at %s:%d\n",__FILE__,__LINE__);\
+        exit(EXIT_FAILURE);}} while(0)
+
+        #define CURAND_CALL(x) do { if((x)!=CURAND_STATUS_SUCCESS) { \
+        printf("Error at %s:%d\n",__FILE__,__LINE__);\
+        exit(EXIT_FAILURE);}} while(0)
+#endif
+
+// Use static function approach instead of inline variable
+inline int& get_debug_level() {
+    static int level = 0;
+    return level;
+}
+
+inline int& g_debug_level = get_debug_level();
+#define DEBUG_PROGRESS_BAR(level) \
+    if (g_debug_level >= level)
+#define DEBUG(level) \
+    if (g_debug_level >= level)
+
+#endif // DEFINES_HPP
+
+

pyvale/common_cpp/dicsignalhandler.cpp

@@ -0,0 +1,16 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+#include "./dicsignalhandler.hpp"
+#include <csignal>
+
+std::atomic<bool> stop_request = false;
+
+void signalHandler(int signal) {
+    if (signal == SIGINT) {
+        stop_request = true;
+    }
+}

pyvale/common_cpp/dicsignalhandler.hpp

@@ -0,0 +1,11 @@
+// ================================================================================
+// pyvale: the python validation engine
+// License: MIT
+// Copyright (C) 2025 The Computer Aided Validation Team
+// ================================================================================
+
+#pragma once
+#include <atomic>
+
+extern std::atomic<bool> stop_request;
+void signalHandler(int signal);