Kannala-Brandt 鱼眼相机模型

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

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

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

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

相机基类模型

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

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

#ifndef ZJSLAM__CAMERA__BASE_HPP
#define ZJSLAM__CAMERA__BASE_HPP

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

#include "../logging.hpp"

namespace camera {

typedef std::vector<float> Vectorf;


enum CameraType {
    PINHOLE, FISHEYE
};


class Base {

protected:
    cv::Size mImgSize;
    Vectorf mvParam;
    cv::Mat mMap1, mMap2;   // 畸变矫正映射

public:
    Sophus::SE3d T_cam_imu;

    typedef std::shared_ptr<Base> Ptr;

    explicit Base(const cv::Size imgSize, const Vectorf &intrinsics, const Vectorf &distCoeffs,
                  const Sophus::SE3d &T_cam_imu = Sophus::SE3d()
    ) : 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());
    }

    virtual CameraType getType() const = 0;

    // 参数读取
    inline void setParam(int i, float value) { mvParam[i] = value; }

    inline float getParam(int i) const { return mvParam[i]; }

    inline size_t getParamSize() const { return mvParam.size(); }

    Vectorf getDistCoeffs() 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 getKEig() const { return GETK(mvParam, Eigen::Matrix3f()).finished(); };

    // 3D -> 2D
    virtual cv::Point2f project(const cv::Point3f &p3D) const = 0;

    virtual Eigen::Vector2d project(const Eigen::Vector3d &v3D) const = 0;

    virtual Eigen::Vector2f project(const Eigen::Vector3f &v3D) const = 0;

    virtual Eigen::Vector2f projectEig(const cv::Point3f &p3D) const = 0;

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

    virtual Eigen::Vector3f unprojectEig(const cv::Point2f &p2D) const = 0;

    // 去畸变
    virtual void undistort(const cv::Mat &src, cv::Mat &dst) = 0;

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

#endif

鱼眼相机模型

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

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

#ifndef ZJSLAM__CAMERA__KANNALA_BRANDT_HPP
#define ZJSLAM__CAMERA__KANNALA_BRANDT_HPP

#include "base.hpp"

namespace camera {

// 最大视场角 (90)
#define KANNALA_BRANDT_MAX_FOV M_PI_2

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

#define KANNALA_BRANDT_PROJECT_BY_VEC3(vp, v3D) \
  float R = this->computeR(atan2f(hypot(v3D[0], v3D[1]), v3D[2])); \
  float phi = atan2f(v3D[1], v3D[0]); \
  return {vp[0] * R * cosf(phi) + vp[2], vp[1] * R * sinf(phi) + vp[3]};

// 2D -> 3D
#define KANNALA_BRANDT_UNPROJECT_PRECISION 1e-6

#define KANNALA_BRANDT_UNPROJECT_BY_XY(cache, p2D) \
  cv::Vec2f wxy = cache.at<cv::Vec2f>(p2D.y, p2D.x); \
  return {wxy[0], wxy[1], 1};


class KannalaBrandt8 : public Base {

protected:
    cv::Mat mUnprojectCache;

    void makeUnprojectCache();

public:
    typedef std::shared_ptr<KannalaBrandt8> Ptr;

    explicit KannalaBrandt8(const cv::Size imgSize, const Vectorf &intrinsics, const Vectorf &distCoeffs,
                            const Sophus::SE3d &T_cam_imu = Sophus::SE3d()
    ) : Base(imgSize, intrinsics, distCoeffs, T_cam_imu), mUnprojectCache(mImgSize, CV_32FC2) {
      ASSERT(distCoeffs.size() == 4, "Distortion coefficients size must be 4")
      makeUnprojectCache();
    }

    CameraType getType() const override { return CameraType::FISHEYE; }

    // 3D -> 2D
    float computeR(float theta) const;

    cv::Point2f project(const cv::Point3f &p3D) const override { KANNALA_BRANDT_PROJECT_BY_XYZ(mvParam, p3D) }

    Eigen::Vector2d project(const Eigen::Vector3d &v3D) const override { KANNALA_BRANDT_PROJECT_BY_VEC3(mvParam, v3D) }

    Eigen::Vector2f project(const Eigen::Vector3f &v3D) const override { KANNALA_BRANDT_PROJECT_BY_VEC3(mvParam, v3D) }

    Eigen::Vector2f projectEig(const cv::Point3f &p3D) const override { KANNALA_BRANDT_PROJECT_BY_XYZ(mvParam, p3D) }

    // 2D -> 3D
    float solveWZ(float wx, float wy, size_t iterations = 10) const;

    cv::Point3f unproject(const cv::Point2f &p2D) const override { KANNALA_BRANDT_UNPROJECT_BY_XY(mUnprojectCache, p2D) }

    Eigen::Vector3f unprojectEig(const cv::Point2f &p2D) const override { KANNALA_BRANDT_UNPROJECT_BY_XY(mUnprojectCache, p2D) }

    // 去畸变
    void undistort(const cv::Mat &src, cv::Mat &dst) override { if (src.data != dst.data) dst = src.clone(); }
};
}

#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 KannalaBrandt8::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 KannalaBrandt8::makeUnprojectCache() {
  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::Vec2f>(r, c) = {wx / wz, wy / wz};
    }
  }
}


float KannalaBrandt8::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);
  float maxR = this->computeR(KANNALA_BRANDT_MAX_FOV);
  if (R > KANNALA_BRANDT_UNPROJECT_PRECISION) {
    float theta = KANNALA_BRANDT_MAX_FOV;
    if (R < maxR) {
      // 最小化损失: (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::drawNormalizedPlane(const cv::Mat &src, cv::Mat &dst) {
  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}).x, y = this->unproject({W / 2, 0}).y,
      w = this->unproject({W, H / 2}).x - x, h = this->unproject({W / 2, H}).y - 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(cv::Point3f(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);
}

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.mfbz.cn/a/594181.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

CNN实现卫星图像分类(tensorflow)

CNN实现卫星图像分类(tensorflow)

使用的数据集卫星图像有两类&#xff0c;airplane和lake&#xff0c;每个类别样本量各700张&#xff0c;大小为256*256&#xff0c;RGB三通道彩色卫星影像。搭建深度卷积神经网络&#xff0c;实现卫星影像二分类。 数据链接百度网盘地址&#xff0c;提取码: cq47 1、查看tenso…
阅读更多...
swift微调多模态大语言模型

swift微调多模态大语言模型

微调训练数据集指定方式的问题请教 Issue #813 modelscope/swift GitHubQwen1.5微调训练脚本中&#xff0c;我用到了--dataset new_data.jsonl 这个选项&#xff0c; 可以训练成功&#xff0c;但我看文档有提到--custom_train_dataset_path这个选项&#xff0c;这两个有什么…
阅读更多...
C语言中字符串输入的3种方式

C语言中字符串输入的3种方式

Ⅰ gets() 函数 gets() 函数的功能是从输入缓冲区中读取一个字符串存储到字符指针变量 str 所指向的内存空间 # include <stdio.h> int main(void) {char a[256] {0};gets(a);printf("%s",a);return 0; }Ⅱ getchar() # include <stdio.h> int mai…
阅读更多...
「 网络安全常用术语解读 」通用安全通告框架CSAF详解

「 网络安全常用术语解读 」通用安全通告框架CSAF详解

1. 简介 通用安全通告框架&#xff08;Common Security Advisory Framework&#xff0c;CSAF&#xff09;通过标准化结构化机器可读安全咨询的创建和分发&#xff0c;支持漏洞管理的自动化。CSAF是OASIS公开的官方标准。开发CSAF的技术委员会包括许多公共和私营部门的技术领导…
阅读更多...
VsCode插件 -- Power Mode

VsCode插件 -- Power Mode

一、安装插件 1. 首先在扩展市场里搜索 Power Mode 插件&#xff0c;如下图 二、配置插件 设置 点击小齿轮 打上勾 就可以了 第二种设置方法 1. 安装完成之后&#xff0c;使用快捷键 Ctrl Shift P 打开命令面板&#xff0c;在命令行中输入 settings.json &#xff0c; 选择首…
阅读更多...
流畅的python-学习笔记_数据结构

流畅的python-学习笔记_数据结构

概念 抽象基类&#xff1a;ABC, Abstract Base Class 序列 内置序列类型 分类 可分为容器类型和扁平类型 容器类型有list&#xff0c; tuple&#xff0c; collections.deque等&#xff0c;存储元素类型可不同&#xff0c;存储的元素也是内容的引用而非内容实际占用内存 …
阅读更多...
.排序总讲.

.排序总讲.

在这里赘叙一下我对y总前四节所讲排序的分治思想以及递归的深度理解。 就以788.逆序对 这一题来讲&#xff08;我认为这一题对于分治和递归的思想体现的淋淋尽致&#xff09;。 题目&#xff1a; 给定一个长度为 n&#x1d45b; 的整数数列&#xff0c;请你计算数列中的逆序对…
阅读更多...
易语言IDE界面美化支持库

易语言IDE界面美化支持库

易语言IDE界面美化支持库 下载下来可以看到&#xff0c;是一个压缩包。 那么&#xff0c;怎么安装到易语言中呢&#xff1f; 解压之后&#xff0c;得到这两个文件。 直接将clr和lib丢到易语言安装目录中&#xff0c;这样子就安装完成了。 打开易语言&#xff0c;在支持库配置…
阅读更多...
C#-快速剖析文件和流，并使用（持续更新）

C#-快速剖析文件和流，并使用（持续更新）

目录 一、概述 二、文件系统 1、检查驱动器信息 2、Path 3、文件和文件夹 三、流 1、FileStream 2、StreamWriter与StreamReader 3、BinaryWriter与BinaryReader 一、概述 文件&#xff0c;具有永久存储及特定顺序的字节组成的一个有序、具有名称的集合&#xff1b; …
阅读更多...
【系统架构师】-选择题（十三）

【系统架构师】-选择题（十三）

1、在某企业的营销管理系统设计阶段&#xff0c;属性"员工"在考勤管理子系统中被称为"员工"&#xff0c;而在档案管理子系统中被称为"职工"&#xff0c;这类冲突称为&#xff08; 命名冲突&#xff09;。 同一个实体在同系统中存在不同的命名&am…
阅读更多...
【4087】基于小程序实现的电影票订票小程序软件

【4087】基于小程序实现的电影票订票小程序软件

作者主页&#xff1a;Java码库 主营内容&#xff1a;SpringBoot、Vue、SSM、HLMT、Jsp、PHP、Nodejs、Python、爬虫、数据可视化、小程序、安卓app等设计与开发。 收藏点赞不迷路 关注作者有好处 文末获取源码 技术选型 【后端】&#xff1a;Java 【框架】&#xff1a;ssm 【…
阅读更多...
局部性原理和磁盘预读

局部性原理和磁盘预读

局部性原理 磁盘预读 \
阅读更多...
Linux 基础命令、性能监控

Linux 基础命令、性能监控

一、Linux 基础命令 grep&#xff1a;在文件中执行关键词搜索&#xff0c;并显示匹配的结果。 -c 仅显示找到的行数 -i 忽略大小写 -n 显示行号 -v 反向选择: 仅列出没有关键词的行 (invert) -r 递归搜索文件目录 -C n 打印匹配行的前后 n 行grep login user.cpp # 在…
阅读更多...
编译官方原版的openwrt并加入第三方软件包

编译官方原版的openwrt并加入第三方软件包

最近又重新编译了最新的官方原版openwrt-2305&#xff08;2024.3.22&#xff09;&#xff0c;此处记录一下以待日后参考。 目录 1.源码下载 1.1 通过官网直接下载 1.2 映射github加速下载 1.2.1 使用github账号fork源码 1.2.2 创建gitee账号映射github openwrt 2.编译准…
阅读更多...
cordova build android 下载gradle太慢

cordova build android 下载gradle太慢

一、 在使用cordova run android / cordova build android 的时候 gradle在线下载 对于国内的链接地址下载太慢。 等待了很长时间之后还会报错。 默认第一次编译在线下载 gradle-7.6.1-all.zip 然后解压缩到 C:\Users\Administrator\.gradle 文件夹中，下载慢导致失败。 二…
阅读更多...
（论文阅读-优化器）A Cost Model for SPARK SQL

（论文阅读-优化器）A Cost Model for SPARK SQL

目录 Abstract 1 Introduction 2 Related Work 3 Background and Spark Basics 4 Cost Model Basic Bricks 4.1 Cluster Abastraction and Cost Model Parameters 4.2 Read 4.3 Write 4.4 Shuffle Read 4.5 Broadcast 5 Modeling GPSJ Queries 5.1 Statistics and S…
阅读更多...
【论文阅读笔记】Order Matters（AAAI 20）

【论文阅读笔记】Order Matters（AAAI 20）

个人博客地址 注&#xff1a;部分内容参考自GPT生成的内容 论文笔记&#xff1a;Order Matters&#xff08;AAAI 20&#xff09; 用于二进制代码相似性检测的语义感知神经网络 论文:《Order Matters: Semantic-Aware Neural Networks for Binary Code Similarity Detection》…
阅读更多...
Spring Boot 整合 socket 实现简单聊天

Spring Boot 整合 socket 实现简单聊天

来看一下实现的界面效果 pom.xml的maven依赖 <!-- 引入 socket --><dependency><groupId>org.springframework.boot</groupId><artifactId>spring-boot-starter-websocket</artifactId></dependency><!-- 引入 Fastjson &#x…
阅读更多...
【CV-CUDA实战】使用Python+TensorRT+CVCUDA优化YOLOv8

【CV-CUDA实战】使用Python+TensorRT+CVCUDA优化YOLOv8

目录 什么是CV-CUDA环境准备准备CV-CUDA静态库解压添加至变量将PyBind静态库复制到env下算子设计前处理算子 TensorRT模型加载后处理函数 完整代码输出演示为什么重新写了&#xff1f;结语 什么是CV-CUDA NVIDIA CV-CUDA™ 是一个开源项目&#xff0c;用于构建云规模人工智能 (…
阅读更多...
【数据结构（邓俊辉）学习笔记】列表02——无序列表

【数据结构（邓俊辉）学习笔记】列表02——无序列表

文章目录 0.概述1.插入与构造1.1 插入1.1.1 前插入1.1.2后插入1.1.3 复杂度 1.2 基于复制构造1.2.1 copyNodes()1.2.2 基于复制构造1.2.3 复杂度 2.删除与析构2.1 删除2.1.1 实现2.1.2 复杂度 2.2 析构2.2.1 释放资源及清除节点2.2.2 复杂度 3.查找3.1 实现3.2 复杂度 4.唯一化…
阅读更多...
最新文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发 尧图建网站