pinhole

/*
Copyright (C) 2018-2024 Geoffrey Daniels. https://gpdaniels.com/

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, version 3 of the License only.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

#pragma once
#ifndef GTL_VISION_CAMERA_MODEL_PINHOLE_HPP
#define GTL_VISION_CAMERA_MODEL_PINHOLE_HPP

// Summary: Pinhole camera model with no distortion. [wip]

#ifndef NDEBUG
#   if defined(_MSC_VER)
#       define __builtin_trap() __debugbreak()
#   endif
/// @brief A simple assert macro to break the program if the pinhole is misused.
#   define GTL_PINHOLE_ASSERT(ASSERTION, MESSAGE) static_cast<void>((ASSERTION) || (__builtin_trap(), 0))
#else
/// @brief At release time the assert macro is implemented as a nop.
#   define GTL_PINHOLE_ASSERT(ASSERTION, MESSAGE) static_cast<void>(0)
#endif

namespace {
    using size_t = decltype(sizeof(0));
}

namespace gtl {
    template <typename type>
    class pinhole final {
    public:
        constexpr static const size_t parameter_count = 4;

    private:
        type focal_lengths[2];
        type centre_points[2];

    private:
        constexpr static const type abs(type value) {
            if ((value + type(0)) < 0) {
                return -value;
            }
            return value;
        }

    public:
        pinhole()
            : focal_lengths{ type(1.0), type(1.0) }
            , centre_points{ type(0.5), type(0.5) } {
        }

        pinhole(const type* const __restrict parameters, const size_t parameters_length) {
            GTL_PINHOLE_ASSERT(parameters_length == pinhole::parameter_count, "Parameters length must match internal number of parameters.");
            this->set_parameters(parameters, parameters_length);
        }

    public:
        void set_parameters(const type* const __restrict parameters, const size_t parameters_length) {
            GTL_PINHOLE_ASSERT(parameters_length == pinhole::parameter_count, "Parameters length must match internal number of parameters.");
            static_cast<void>(parameters_length);
            this->focal_lengths[0] = parameters[0];
            this->focal_lengths[1] = parameters[1];
            this->centre_points[0] = parameters[2];
            this->centre_points[1] = parameters[3];
            GTL_PINHOLE_ASSERT(this->focal_lengths[0] > 0, "Horiztonal focal length must be greater than zero.");
            GTL_PINHOLE_ASSERT(this->focal_lengths[1] > 0, "Vertical focal length must be greater than zero.");
        }

        void get_parameters(type* const __restrict parameters, const size_t parameters_length) const {
            GTL_PINHOLE_ASSERT(parameters_length == pinhole::parameter_count, "Parameters length must match internal number of parameters.");
            static_cast<void>(parameters_length);
            parameters[0] = this->focal_lengths[0];
            parameters[1] = this->focal_lengths[1];
            parameters[2] = this->centre_points[0];
            parameters[3] = this->centre_points[1];
        }

    public:
        bool project(
            const type* const __restrict point_xyz,
            type* const __restrict point_xy,
            type* const __restrict jacobian_projection = nullptr,
            type* const __restrict jacobian_parameters = nullptr
        ) const {
            // Cannot project points with no depth, also return false for those behind the camera.
            if (abs(point_xyz[2]) < type(1.0e-12)) {
                return false;
            }

            // Project.
            const type inverse_z = type(1.0) / point_xyz[2];
            const type undistorted_xy[2] = {
                point_xyz[0] * inverse_z,
                point_xyz[1] * inverse_z
            };

            // Scale and offset.
            point_xy[0] = this->focal_lengths[0] * undistorted_xy[0] + this->centre_points[0];
            point_xy[1] = this->focal_lengths[1] * undistorted_xy[1] + this->centre_points[1];

            // Compute the projection jacobian.
            if (jacobian_projection != nullptr) {
                // Python:
                // import sympy
                // fx, fy, cx, cy, X, Y, Z = sympy.symbols('fx, fy, cx, cy, X, Y, Z')
                // sympy.cse(sympy.Matrix([fx*X/Z+cx, fy*Y/Z+cy]).jacobian(sympy.Matrix([X, Y, Z])))
                const type inverse_z_squared = inverse_z * inverse_z;
                jacobian_projection[0 * 3 + 0] =  this->focal_lengths[0] * inverse_z;
                jacobian_projection[0 * 3 + 1] =  type(0.0);
                jacobian_projection[0 * 3 + 2] = -this->focal_lengths[0] * point_xyz[0] * inverse_z_squared;
                jacobian_projection[1 * 3 + 0] =  type(0.0);
                jacobian_projection[1 * 3 + 1] =  this->focal_lengths[1] * inverse_z;
                jacobian_projection[1 * 3 + 2] = -this->focal_lengths[1] * point_xyz[1] * inverse_z_squared;
            }

            // Compute the parameters jacobian.
            if (jacobian_parameters != nullptr) {
                // Python:
                // import sympy
                // fx, fy, cx, cy, X, Y, Z = sympy.symbols('fx, fy, cx, cy, X, Y, Z')
                // sympy.cse(sympy.Matrix([fx*X/Z+cx, fy*Y/Z+cy]).jacobian(sympy.Matrix([fx, fy, cx, cy])))
                jacobian_parameters[0 * pinhole::parameter_count + 0] = undistorted_xy[0];
                jacobian_parameters[0 * pinhole::parameter_count + 1] = type(0.0);
                jacobian_parameters[0 * pinhole::parameter_count + 2] = type(1.0);
                jacobian_parameters[0 * pinhole::parameter_count + 3] = type(0.0);
                jacobian_parameters[1 * pinhole::parameter_count + 0] = type(0.0);
                jacobian_parameters[1 * pinhole::parameter_count + 1] = undistorted_xy[1];
                jacobian_parameters[1 * pinhole::parameter_count + 2] = type(0.0);
                jacobian_parameters[1 * pinhole::parameter_count + 3] = type(1.0);
            }

            return true;
        }

        bool unproject(
            const type* const __restrict point_xy,
            type* const __restrict ray_xyz,
            type* const __restrict jacobian_unprojection = nullptr
        ) const {
            // Scale and offset.
            const type undistorted_xy[2] = {
                (point_xy[0] - this->centre_points[0]) / this->focal_lengths[0],
                (point_xy[1] - this->centre_points[1]) / this->focal_lengths[1]
            };

            // Unproject.
            ray_xyz[0] = undistorted_xy[0];
            ray_xyz[1] = undistorted_xy[1];
            ray_xyz[2] = type(1.0);

            // Compute the unprojection jacobian.
            if (jacobian_unprojection != nullptr) {
                // Python:
                // import sympy
                // fx, fy, cx, cy, X, Y, Z = sympy.symbols('fx, fy, cx, cy, X, Y, Z')
                // sympy.cse(sympy.Matrix([(X-cx)/fx, (Y-cy)/fy, 1]).jacobian(sympy.Matrix([X, Y])))
                jacobian_unprojection[0 * 2 + 0] = type(1.0) / this->focal_lengths[0];
                jacobian_unprojection[0 * 2 + 1] = type(0.0);
                jacobian_unprojection[1 * 2 + 0] = type(0.0);
                jacobian_unprojection[1 * 2 + 1] = type(1.0) / this->focal_lengths[1];
                jacobian_unprojection[2 * 2 + 0] = type(0.0);
                jacobian_unprojection[2 * 2 + 1] = type(0.0);
            }

            return true;
        }
    };
}

#undef GTL_PINHOLE_ASSERT

#endif // GTL_VISION_CAMERA_MODEL_PINHOLE_HPP