Kannala-Brandt 鱼眼相机模型

荷碧TongZJ

已于 2024-07-31 15:00:23 修改

阅读量1k

点赞数 6

分类专栏：计算机视觉文章标签： SLAM c++ 相机计算机视觉

于 2024-05-05 14:02:06 首次发布

本文链接：https://blog.csdn.net/qq_55745968/article/details/138465252

版权

计算机视觉专栏收录该内容

15 篇文章 37 订阅

订阅专栏

文章探讨了在ORB-SLAM3中模仿和重构鱼眼相机模型时遇到的畸变问题，指出鱼眼相机无需常规的去畸变处理，并详细介绍了Kannala-Brandt相机模型的实现，包括3D-2D投影和2D-3D反投影的计算。作者还展示了如何在TUM-VI数据集上测试鱼眼相机模型的性能。

摘要由CSDN通过智能技术生成

最近在学习 ORB-SLAM3 的源代码，并模仿、重构了相机模型的实现

在学习的过程中发现针孔相机 (Pinhole) 与鱼眼相机 (Fisheye) 都有畸变参数，但是鱼眼相机无法使用 cv::undistort 函数去畸变

在对鱼眼相机的深度归一化平面进行可视化后，发现鱼眼相机真的不需要去畸变

参考文献：A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses

相机基类模型

utils/glog.hpp 中主要调用了 glog 库，并定义了 ASSERT(expr, msg) 宏

基类 Base 初始化时需要输入 imgSize (图像尺寸)、intrinsics (相机内参)、distCoeffs (畸变参数)

#ifndef ZJCV__CAMERA__BASE_HPP
#define ZJCV__CAMERA__BASE_HPP

#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>

#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;   // 畸变矫正映射

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;

    // 参数读写
    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()}; }

    // 内参矩阵 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
    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
    virtual Eigen::Vector3f unproject(const cv::Point2f &p2D) const = 0;

    // 去畸变
    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];
    }

    // 绘制归一化平面 (z=1)
    void draw_normalized_plane(const cv::Mat &src, cv::Mat &dst) const;
};

}

#endif

鱼眼相机模型

因为在实现 C++ 的函数多态时，需要根据不同的输入值类型设计对应的计算过程 —— 但往往计算过程都是极其相似的，这给代码维护造成了麻烦

所以本文使用宏定义实现了这些计算过程

#ifndef ZJCV__CAMERA__KANNALA_BRANDT_HPP
#define ZJCV__CAMERA__KANNALA_BRANDT_HPP

#include "base.hpp"

namespace camera {

// 最大视场角 (180)
#define KANNALA_BRANDT_MAX_FOV M_PI

// 3D -> 2D
#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]};

// 2D -> 3D
#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; }

    // 3D -> 2D
    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) }

    // 2D -> 3D
    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) }

    // 去畸变
    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

与针孔类型相似的，鱼眼模型也有焦距 $f_x, f_y$ ，光心 $c_x, c_y$ ，以及畸变参数 $k_1, k_2, k_3, k_4$

借助这些参数，可以实现对世界坐标系下的点 $(X_c, Y_c, Z_c)$ 、像素坐标系下的点 $(x, y)$ 实现相互变换

project (世界坐标 → 像素坐标)

$\theta = \arctan(\frac{\sqrt{X_c^2+Y_c^2}}{Z_c}), \psi = \arctan(\frac{Y_c}{X_c})$

$R(\theta)=k_1 \theta + k_2 \theta^3 + k_3 \theta^5 + k_4 \theta^7$

$x = f_x R \cos(\psi) + c_x, y = f_y R \sin(\psi) + c_y$

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

unproject (像素坐标 → 世界坐标)

根据 project 的过程，可以由像素坐标计算得到 $R(\theta)$ ，并反向求得 $\theta$ ：

$X_c= \frac{x - c_x}{f_x}, Y_c = \frac{y - c_y}{f_y}$

$R(X_c, Y_c) = \sqrt{X_c^2 + Y_c^2 } \in [0, \infty)$

由于 $\theta$ 的取值是有上限的 (假设为 $\frac{\pi}{2}$ )，也就是说 $R_{max} = R(\frac{\pi}{2})$

所以当 $R(X_c, Y_c) > R_{max}$ 时，应当检查相机内参是否出错

使用梯度下降法使得 $l(\theta)=(R(\theta) - R(X_c, Y_c))^2=0$ ，以求解 $\theta$

由于 $l(\theta)$ 是一个凹函数，所以只要保证迭代量正负号正确即可

当求得 $\theta$ 时，便可以得到 $Z_c$ ：

$Z_c =R(X_c, Y_c) / \tan(\theta)$

而由于单目相机的深度没有什么意义，把 $(X_c / Z_c, Y_c / Z_c, 1)$ 作为对应的世界坐标

(这里使用缓存的方式实现 unproject)

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 {
  // wz = lim_{theta -> 0} R / tan(theta) = 1
  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)) {
      // 最小化损失: (poly(theta) - R)^2
      int i = 0;
      float e;
      for (; i < iterations; i++) {
        float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 = theta6 * theta2;
        float k0_theta2 = mvParam[4] * theta2, k1_theta4 = mvParam[5] * theta4,
            k2_theta6 = mvParam[6] * theta6, 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;
        // 梯度下降法: g = (poly(theta) - R) / poly'(theta)
        theta -= e / (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8);
      }
      if (i == iterations) LOG(WARNING) << "solveWZ(" << wx << ", " << wy << "): relative error " << abs(e) / R;
    }
    wz = R / tanf(theta);
  }
  return wz;
}

绘制深度归一化平面

深度归一化平面，即世界坐标点在 $Z_c = 1$ 平面上的投影，也就是一幅图像

基本思路就是，通过 unproject 获取深度归一化平面的边界，然后通过 project 获取平面上各个点对应图像中的位置

void Base::draw_normalized_plane(const cv::Mat &src, cv::Mat &dst) const {
  undistort(src, dst);
  cv::Mat npMap1 = cv::Mat(mImgSize, CV_32FC1), npMap2 = npMap1.clone();
  // 获取归一化平面边界 (桶形畸变)
  float x, y, w, h, W = mImgSize.width - 1, H = mImgSize.height - 1;
  x = this->unproject({0, H / 2})[0], y = this->unproject({W / 2, 0})[1],
  w = this->unproject({W, H / 2})[0] - x, h = this->unproject({W / 2, H})[1] - y;
  LOG(INFO) << "Normalized plane: " << cv::Vec4f(x, y, x + w, y + h);
  // 计算畸变矫正映射
  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);
}

本文使用了 TUM-VI 数据集进行实验，Kannala-Brandt 相机的参数如下：

resolution: [512, 512]

intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]

dist_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182]

(下面这段代码用了我自己写的其它东西，仅作参考)

void fisheye_test() {
  // 加载 TUM-VI 数据集 相机参数
  dataset::TumVI tumvi("/home/workbench/data/dataset-corridor4_512_16/dso");
  YAML::Node cfg = tumvi.loadCfg();
  auto cam(camera::fromYAML<camera::KannalaBrandt8>(cfg["cam0"]));

  // 加载图像列表, 读取第一张图像
  GrayLoader loader;
  dataset::Timestamps vTimestamps;
  dataset::Filenames vFilename;
  tumvi.loadImage(vTimestamps, vFilename);
  cv::Mat img = loader(vFilename[0]), dst1;

  // 显示原始图像, 以及去畸变后的图像
  cv::imshow("Origin", img);
  cam->drawNormalizedPlane(img, img);
  cv::imshow("NormalizedPlane", img);
  cv::waitKey(0);
}