While studying the ORB-SLAM3 source code, I recreated and refactored its camera model implementation. A key discovery was that despite both pinhole and fisheye cameras having distortion parameters, the fisheye model cannot use cv::undistort for correction. After visualizing the depth-normalized plane of a fisheye camera, it became clear that fisheye images inherently don't require conventional undistortion.

This work is based on the reference: "A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses".

Camera Base Class

The utility header utils/glog.hpp wraps the glog library and defines the ASSERT(expr, msg) macro. The base class Base is initialized with image size, intrinsics, and distortion coefficients.

#ifndef ZJCV__CAMERA__BASE_HPP
#define ZJCV__CAMERA__BASE_HPP

#include <eigen>
#include <opencv2>
#include <sophus>
#include "utils/glog.hpp"

namespace camera {

typedef std::vector<float> Vectorf;
typedef std::vector<cv::Point2f> VectorPt2f;
typedef std::vector<cv::KeyPoint> VectorKp;

enum CameraType { PINHOLE, KANNALA_BRANDT };

class Base {
protected:
    cv::Size mImgSize;
    Vectorf mvParam;
    cv::Mat mMap1, mMap2;  // distortion correction maps
public:
    Sophus::SE3f T_cam_imu;
    typedef std::shared_ptr<Base> Ptr;

    explicit Base(const cv::Size imgSize, const Vectorf &intrinsics,
                  const Vectorf &distCoeffs,
                  const Sophus::SE3f &T_cam_imu = Sophus::SE3f())
        : mImgSize(imgSize), mvParam(intrinsics), T_cam_imu(T_cam_imu) {
        ASSERT(intrinsics.size() == 4, "Intrinsics size must be 4");
        mvParam.insert(mvParam.end(), distCoeffs.begin(), distCoeffs.end());
    }

    Base(const Base &) = delete;
    virtual CameraType get_type() const = 0;

    // Parameter access
    inline void set_param(int i, float value, bool safe = false) {
        if (!safe) LOG(WARNING) << "Unsafe set_param: " << i << " = " << value;
        mvParam[i] = value;
    }
    inline float get_param(int i) const { return mvParam[i]; }
    inline size_t get_param_size() const { return mvParam.size(); }
    Vectorf get_distcoeffs() const {
        return {mvParam.begin() + 4, mvParam.end()};
    }

    // Intrinsic matrix K
#define GETK(vp, K) \
    (K << vp[0], 0.f, vp[2], 0.f, vp[1], vp[3], 0.f, 0.f, 1.f)

    virtual cv::Mat getK() const {
        return GETK(mvParam, cv::Mat_<float>(3, 3));
    }
    virtual Eigen::Matrix3f getK_eig() const {
        return GETK(mvParam, Eigen::Matrix3f()).finished();
    }

    // 3D -> 2D projection
    virtual cv::Point2f project(const Eigen::Vector3f &v3D) const = 0;
    virtual Eigen::Vector2d project(const Eigen::Vector3d &v3D) const = 0;
    virtual Eigen::Vector2f project_eig(const cv::Point3f &p3D) const = 0;

    // 2D -> 3D unprojection
    virtual Eigen::Vector3f unproject(const cv::Point2f &p2D) const = 0;

    // Distortion removal
    virtual void undistort(const cv::Mat &src, cv::Mat &dst) const = 0;
    virtual void undistort(const VectorPt2f &src, VectorPt2f &dst) const = 0;
    virtual void undistort(const VectorKp &src, VectorKp &dst) const {
        VectorPt2f pts;
        for (const auto &kp : src) pts.push_back(kp.pt);
        undistort(pts, pts);
        for (size_t i = 0; i < src.size(); ++i) dst[i].pt = pts[i];
    }

    // Visualize normalized plane (z=1)
    void draw_normalized_plane(const cv::Mat &src, cv::Mat &dst) const;
};

}
#endif
</sophus></opencv2></eigen>

Fisheye Camera Model Implementation

To handle C++ function polymorphism efficiently without code duplication, macros are used to encapsulate the projectino and unprojection logic.

#ifndef ZJCV__CAMERA__KANNALA_BRANDT_HPP
#define ZJCV__CAMERA__KANNALA_BRANDT_HPP

#include "base.hpp"

namespace camera {

#define KANNALA_BRANDT_MAX_FOV M_PI  // maximum field of view (180°)

#define KANNALA_BRANDT_PROJECT(vp, x, y, z) \
  float R = this->computeR(atan2f(hypot(x, y), z)); \
  float psi = atan2f(y, x); \
  return {vp[0] * R * cosf(psi) + vp[2], vp[1] * R * sinf(psi) + vp[3]};

#define KANNALA_BRANDT_UNPROJECT_PRECISION 1e-6

#define KANNALA_BRANDT_UNPROJECT(cache, p2D) \
  cv::Mat wxy; \
  cv::getRectSubPix(cache, {1, 1}, p2D, wxy); \
  return {wxy.at<float>(0), wxy.at<float>(1), 1};

class KannalaBrandt : public Base {
protected:
    cv::Mat mUnprojectCache;
    void make_unproject_cache();
public:
    typedef std::shared_ptr<KannalaBrandt> Ptr;
    using Base::undistort;

    explicit KannalaBrandt(const cv::Size imgSize, const Vectorf &intrinsics,
                           const Vectorf &distCoeffs,
                           const Sophus::SE3f &T_cam_imu = Sophus::SE3f())
        : Base(imgSize, intrinsics, distCoeffs, T_cam_imu),
          mUnprojectCache(mImgSize, CV_32FC3) {
        ASSERT(distCoeffs.size() == 4, "Distortion coefficients size must be 4");
        make_unproject_cache();
    }

    KannalaBrandt(const KannalaBrandt &) = delete;
    CameraType get_type() const override { return CameraType::KANNALA_BRANDT; }

    // Project 3D point to 2D pixel
    float computeR(float theta) const;
    cv::Point2f project(const Eigen::Vector3f &v3D) const override {
        KANNALA_BRANDT_PROJECT(mvParam, v3D[0], v3D[1], v3D[2]);
    }
    Eigen::Vector2d project(const Eigen::Vector3d &v3D) const override {
        KANNALA_BRANDT_PROJECT(mvParam, v3D[0], v3D[1], v3D[2]);
    }
    Eigen::Vector2f project_eig(const cv::Point3f &p3D) const override {
        KANNALA_BRANDT_PROJECT(mvParam, p3D.x, p3D.y, p3D.z);
    }

    // Unproject 2D pixel to 3D normalized ray
    float solveWZ(float wx, float wy, size_t iterations = 10) const;
    Eigen::Vector3f unproject(const cv::Point2f &p2D) const override {
        KANNALA_BRANDT_UNPROJECT(mUnprojectCache, p2D);
    }

    // Undistortion: for Kannala-Brandt, images are already in a suitable form
    void undistort(const cv::Mat &src, cv::Mat &dst) const override {
        if (src.data != dst.data) dst = src.clone();
    }
    void undistort(const VectorPt2f &src, VectorPt2f &dst) const override {
        if (src.data() != dst.data()) dst = src;
    }
    void undistort(const VectorKp &src, VectorKp &dst) const override {
        if (src.data() != dst.data()) dst = src;
    }
};

}
#endif

Projection (3D World → 2D Pixel)

The projection function computes the radial distance R from the incident angle θ using a polynomial distortion model.

float KannalaBrandt::computeR(float theta) const {
    float theta2 = theta * theta;
    return theta + theta2 * (mvParam[4] + theta2 *
           (mvParam[5] + theta2 * (mvParam[6] + theta2 * mvParam[7])));
}

Unprojection (2D Pixel → 3D Ray)

For a given pixel, we compute wx, wy from normalized coordinates, then solve for wz using gradient descent to invert the projection.

void KannalaBrandt::make_unproject_cache() {
    float wx, wy, wz;
    for (int r = 0; r < mImgSize.height; ++r) {
        wy = (r - mvParam[3]) / mvParam[1];
        for (int c = 0; c < mImgSize.width; ++c) {
            wx = (c - mvParam[2]) / mvParam[0];
            wz = this->solveWZ(wx, wy);
            mUnprojectCache.at<cv::Vec3f>(r, c) = {wx / wz, wy / wz, 1};
        }
    }
}

float KannalaBrandt::solveWZ(float wx, float wy, size_t iterations) const {
    float wz = 1.f;
    float R = hypot(wx, wy);
    if (R > KANNALA_BRANDT_UNPROJECT_PRECISION) {
        float theta = KANNALA_BRANDT_MAX_FOV;
        if (R < this->computeR(theta)) {
            // Minimize loss: (poly(theta) - R)^2
            int i = 0;
            float e;
            for (; i < iterations; i++) {
                float theta2 = theta * theta;
                float theta4 = theta2 * theta2;
                float theta6 = theta4 * theta2;
                float theta8 = theta6 * theta2;
                float k0_theta2 = mvParam[4] * theta2;
                float k1_theta4 = mvParam[5] * theta4;
                float k2_theta6 = mvParam[6] * theta6;
                float k3_theta8 = mvParam[7] * theta8;
                e = theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - R;
                if (abs(e) < R * KANNALA_BRANDT_UNPROJECT_PRECISION) break;
                // Gradient descent: g = e / poly'(theta)
                float deriv = 1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8;
                theta -= e / deriv;
            }
            if (i == iterations)
                LOG(WARNING) << "solveWZ(" << wx << ", " << wy << "): relative error " << abs(e) / R;
        }
        wz = R / tanf(theta);
    }
    return wz;
}

Visualizing the Depth-Normalized Plane

The depth-normalized plane projects world points onto z = 1, effectively generating a remapped image. The algorithm first computes the unprojected boundaries of the image, then projects each normalized point back to pixel coordinates.

void Base::draw_normalized_plane(const cv::Mat &src, cv::Mat &dst) const {
    undistort(src, dst);
    cv::Mat npMap1 = cv::Mat(mImgSize, CV_32FC1);
    cv::Mat npMap2 = npMap1.clone();

    // Compute normalized plane boundaries (barrel distortion)
    float W = mImgSize.width - 1, H = mImgSize.height - 1;
    float x = this->unproject({0, H / 2})[0];
    float y = this->unproject({W / 2, 0})[1];
    float w = this->unproject({W, H / 2})[0] - x;
    float h = this->unproject({W / 2, H})[1] - y;
    LOG(INFO) << "Normalized plane: " << cv::Vec4f(x, y, x + w, y + h);

    // Build distortion correction mapping
    for (int r = 0; r < H; ++r) {
        for (int c = 0; c < W; ++c) {
            cv::Point2f p2D = this->project(
                Eigen::Vector3f(w * c / W + x, h * r / H + y, 1));
            npMap1.at<float>(r, c) = p2D.x;
            npMap2.at<float>(r, c) = p2D.y;
        }
    }
    cv::remap(dst, dst, npMap1, npMap2, cv::INTER_LINEAR);
}

Experimental Results with TUM-VI Dataset

Testing was conducted using the TUM-VI dataset with a Kannala-Brandt camera having the following parameters:

• Resolution: [512, 512]
• Intrinsics: [190.97848, 190.97331, 254.93171, 256.89744]
• Distortion coefficients: [0.0034824, 0.0007150, -0.0020532, 0.0002029]

The following code snippet (using custom auxiliary libraries) demonstrates loading and visualizing a fisheye image:

void fisheye_test() {
    // Load TUM-VI camera parameters
    dataset::TumVI tumvi("/home/workbench/data/dataset-corridor4_512_16/dso");
    YAML::Node cfg = tumvi.loadCfg();
    auto cam = camera::fromYAML<camera::KannalaBrandt>(cfg["cam0"]);

    // Load image list and read first image
    dataset::Timestamps vTimestamps;
    dataset::Filenames vFilename;
    tumvi.loadImage(vTimestamps, vFilename);
    cv::Mat img = cv::imread(vFilename[0]), dst1;

    // Show original and normalized plane images
    cv::imshow("Original", img);
    cam->draw_normalized_plane(img, img);
    cv::imshow("NormalizedPlane", img);
    cv::waitKey(0);
}