SLAM十四讲之第五讲：图像与相机

图像与相机

opencv编译问题：选择3.4以上版本就没那么多事

1 图像

​ 灰度图像，（x, y）表示了像素的位置，每一个像素对应一个灰度值I（0 ~ 255）。
$$I(x,y):{\mathbb{R}}^2\mapsto \mathbb{R}$$
​ 深度图像，包含各个像素与相机之间的距离。单位1表示1毫米（mm），所以取16位整数，最大距离可表达65米，即一个像素可占16位空间。

​ 彩色图，常用如BGR，三个通道，蓝色、绿色和红色。每个通道都是由8位整数表示，即（0 ~255）。所以一个像素占3*8=24位空间。

​ 一般访问图像某个位置的像素如下（在程序中，图像以二维数组形式存储。它的第一个下标是指数组的行，而第二个下标则是列。）

unsigned char pixel image [y][x]; // [行][列]      

​ 有时候，为了处理方便，OpenCV会把经过填充后的长度指定为有效宽度，实际宽度并不一定等于有效宽度。之所以填充，是因为如果行的长度是4或8的整数倍，处理性能会更高。我们利用cols得到的是实际宽度，而利用step得到单位是字节的有效宽度。

• step:矩阵第一行元素的字节数
• step[0]:矩阵第一行元素的字节数
• step[1]:矩阵中一个元素的字节数
• step1(0):矩阵中一行有几个通道数
• step1(1):一个元素有几个通道数(channel())

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

int main(int argc, char** argv) {

    cv::Mat image = cv::imread("F:/背景图片/333.png", -1);
    cout << image.step[0] <<" "<< image.elemSize() * image.cols << endl;
    cout << image.elemSize();   // 按原图像格式-1读取，4通道；按bgr格式读取，3通道；都是无符号字符型，一个通道一个字节
                                                 // elemSize()是单个像素的字节数
                                                 // 
    if (image.isContinuous() != true)
        cout << " 图像填充了像素" << endl;
    else
        cout <<" 图像没有填充了像素" << endl;
    image = image.reshape(4, 480);    // 第一个参数为通道数，第二个是新的行数
    cv::imshow("resize", image);
    cv::waitKey(0);
}

2 相机模型

2.1 成像基础

​ 小孔成像：倒立的像

​ 小孔成像的前提：小孔的孔径要足够小。当孔径逐渐增大时，通过障碍物的光线数量也随之增加。这样成像平面上的每个点都可能受到来自三维物体的多个点发出的光线的影响，成像会逐渐变模糊。

​ 实际上相机镜头加了透镜，它可以汇聚光线，解决了成像清晰和高亮度不能兼顾的问题。

​ 但是成像仍然是倒立的，所以实际当中把倒立的像对称的的放到相机前方.如下所示，看起来比实际的图像小一些的蜡烛就是对称的立正的像。

彩图来源：计算机视觉，从 3D 重建到识别 (stanford.edu)

2.2 针孔相机模型

单目相机成像的过程：

1. 世界坐标系下一固定点$P$，世界坐标位$P_w$
2. 相机在不停的运动，其中旋转矩阵R和平移t描述了相应的运动。相机坐标系下坐标为$P=R\ast P_w+t$，坐标$P$对应的分量为X, Y, Z。
3. 归一化坐标系（图像坐标系）。将相机坐标系下的坐标分量都除以Z，得到归一化坐标$P_c=[X/Z,Y/Z,1{]}^T$。归一化坐标意味着相机前面距离为1，即Z = 1的平面上的点P。

$$\left(\mathbf{R}{\mathbf{P}}_{\mathrm{w}}+\mathbf{t}\right)=\underset{\text{相机坐标 }}{\underbrace{[X,Y,Z{]}^{\mathrm{T}}}}\to \underset{\text{归一化坐标 }}{\underbrace{[X/Z,Y/Z,1{]}^{\mathrm{T}}}}.$$

4. 若图像由畸变，根据畸变公式计算畸变后的坐标。参数r是归一化坐标的极坐标表示。注意这里x，y是无畸变坐标，而$x_{distorted}、y_{distorted}$是畸变坐标。

$$\{\begin{array}{l}{x}_{\text{distorted }}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ y_{\text{distorted }}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}$$

5. 经过相机内参矩阵K，得到像素坐标系下坐标

$$\{\begin{array}{l}u=f_x{x}_{\text{distorted }}+c_x\\ v=f_y{y}_{\text{distorted }}+c_y\end{array}$$

6. 关于畸变，畸变分为径向畸变和切向畸变。径向畸变又分为桶形畸变和枕形畸变。考虑归一化平面上的任意一点$p$,它的坐标为$[x,y{]}^T$,也可写成极坐标的形式$[r,\theta {]}^T$,其中r表示点$p$与坐标系原点之间的距离，$\theta$表示与水平轴的夹角。**径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角$\theta$发生了变化。**比如说，上面公式中k全部大于0的就是枕形畸变，而k小于0的就是桶形畸变，区别就是桶形畸变是往里弯，变小了；而枕形畸变是往外弯，变大了。

​ 切向畸变一般是因为透镜与成像平面的不平行导致的

注意：

1. 像素坐标系通常的定义方式是：原点o位于图像的左上角，u轴向右与x轴平行，v轴向下与y轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。这里$c_x,c_y$的单位是像素，而$\alpha 、\beta$的单位是像素/m。

$$\{\begin{array}{l}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}$$

2. 小孔成像的原理是三角形相似。单目相机无法从图像的像素得到它的一个距离，这是因为相机坐标系到归一化坐标系是利用相似三角来计算的。只要点P在那条线上，我们就无法得到它的具体位置。
3. 相机的内参是矩阵K，外参是旋转矩阵R和平移向量t

​ 相机的内参数由焦距$f$(Focal Length)、主点(Principal Point)、径向畸变(Radical Distortion)和切向畸变(Tangential Distortion)组成。

• 焦距指投影中心到物理成像平面的距离；
• 主点指相机主光轴与物理成像平面的交点；

$$K=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)$$

2.2 双目相机模型

​ 在左右双目相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的，意味着两个相机的光圈中心都位于x轴上。两者之间的距离称为双目相机的基线（记作b)。

从上面的公式可以看出，如果要计算空间点P的深度Z，必须要知道：

1. 相机焦距f，左右相机基线b——这些参数可以通过相机标定得到；

2. 视差d——即左相机成像面上的每个像素点$p_L$和右相机成像面上的对应点$p_R$的关系，即对应像素点的横坐标之差

2.3 RGB-D相机模型

​ RGB-D相机自己完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成，点云（Point Cloud)。

​ RGB-D相机与单目相机的区别是否只是差一个深度信息。

2.4 鱼眼相机

​ 鱼眼镜头一般是由十几个不同的透镜组合而成的，在成像的过程中，入射光线经过不同程度的折射，投影到尺寸有限的成像平面上，使得鱼眼镜头与普通镜头相比起来拥有了更大的视野范围。

鱼眼相机成像原理 - 简书 (jianshu.com)

3 安装

3.1 Ubuntu

安装依赖

sudo apt install build-essential cmake git pkg-config libgtk-3-dev \
    libavcodec-dev libavformat-dev libswscale-dev libv4l-dev \
    libxvidcore-dev libx264-dev libjpeg-dev libpng-dev libtiff-dev \
    gfortran openexr libatlas-base-dev python3-dev python3-numpy \
    libtbb2 libtbb-dev libdc1394-22-dev libopenexr-dev \
    libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev

不要安装3.1-3.3之间的版本，可能在cmake就会报错，还需要很麻烦的改，直接装3.4.16版本就不会报错。

cd opencv-3.4.16
mkdir build && cd build
cmake ..
make -j4       # 可能会有点慢
sudo make install

3.2 Windows

​ opencv提供了编译好的Windows文件，可以直接下载后使用。Releases - OpenCV

​ 在vs项目中，分别将头文件和库文件加入，即可编译opencv程序。

将下载的opencv文件（官方已经编译好），添加到路径中。

添加库路径

点击链接器，选择输入，点击附加依赖项，添加lib文件(opencv\build\x64\vc15\lib)

完成后试验程序

#include <iostream>
#include <chrono>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(int argc, char** argv) {
   
    cv::Mat image;
    image = cv::imread("F:/背景图片/111.png"); 
    cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
    cv::imshow("image", image);      
    cv::waitKey(0);                  
}

​ 注意：可能会出现报错找不到opencv_worldxxxxd.dll，其中xxxx表示了你的opencv版本，我这里显示的是找不到opencv_world3416d.dll

解决办法：把opencv\build\x64\vc15\bin路径下的dll后缀文件复制到C:\Windows\System32

4 实践

4.1 opencv基本操作

终端执行代码时候

 ./imageBasics ../ubuntu.png       # tab补全   注意图象是放在当前文件的上一级，两个点

• 1 cv::Mat创建图像（或其他）矩阵

Mat(int rows, int cols, int type, const Scalar& s);
// scalar标量，即初始化图像的每一个像素的值为s  

n-dimensional dense array class;   // n维稠密矩阵类

cv::Mat image;   		// 创建图像 
image.cols;				// 图像列维度
image.rows;				// 行维
image.channels();		// 通道数
image.data;				// uchar* data; 无符号字符型只在，应该是把图像数据当作向量的指针
image.type();			// 矩阵元素类型 eg CV_8UC1 
image.clone();   		// 复制图像，深度拷贝，不会改变原先图像的值
image2.copyTo(image); 		// 复制图像，深度拷贝
image.at<uchar>(row, col);			// 获取该位置的像素值大小  单通道 灰度图
image.at<uchar>(row, col)[0];				// 三通道 [0]b [1]g [2]r
image.convertTo(img, datatype, scale)  		// 转换图像的数据类型
image.empty()   			// 如果矩阵没有元素，返回true   
    
image.ptr<unsigned char>(y)    // 行指针
    

• 2 cv::imread()读取图像

cv::imread(const String& filename, int flags = IMREAD_COLOR)

1 const String& filename   // 文件名;
2 flags  // 可以采用cv:: ImreadModes 的标志，函数默认是采用 IMREAD_COLOR
    flags = 1    // 彩色图  3通道   IMREAD_COLOR
    flags = 0    // 灰度图	 1通道   IMREAD_GRAYSCALE
    flags = -1	 // 以原图像本身格式 IMREAD_UNCHANGED	

这里有更多的flags格式

点进源码，也可以知道

IMREAD_UNCHANGED            = -1, 
IMREAD_GRAYSCALE            = 0, 
IMREAD_COLOR                = 1,  
IMREAD_ANYDEPTH             = 2,  
IMREAD_ANYCOLOR             = 4,  
IMREAD_LOAD_GDAL            = 8,  
......
#define CV_8U   0   // image.type()  不同的矩阵数据类型
#define CV_8S   1
#define CV_16U  2
#define CV_16S  3
#define CV_32S  4
#define CV_32F  5
#define CV_64F  6
#define CV_USRTYPE1 7			// 对应图像矩阵数据类型
// image.type()类型  一种类型不同通道数量
#define CV_8UC1 CV_MAKETYPE(CV_8U,1)  // C1指通道数为1 
#define CV_8UC2 CV_MAKETYPE(CV_8U,2)
#define CV_8UC3 CV_MAKETYPE(CV_8U,3)  // 即BGR图像
#define CV_8UC4 CV_MAKETYPE(CV_8U,4)    //带Alph通道的RGB图像–是--4通道图像

• 3 typedef unsigned long size_t

size_t  // 无符号长整型

• 4 实验

#include <iostream>
#include <chrono>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(int argc, char **argv) {
  // 读取argv[1]指定的图像    argv[i] 指命令行的第i个参数，这里很重要！！！
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

  // 1. 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }
  // 2. 图像类型
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {   
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 3. 访问图像位于 x,y 处的像素
  for (size_t y = 0; y < image.rows; y++) {
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char *row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) {
      // data_ptr 指向待访问的像素数据  每个像素在这一行占1*通道数  索引 = 通道数*列数x
      unsigned char *data_ptr = &row_ptr[x * image.channels()]; 
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }

  // 4. 关于 cv::Mat 的拷贝
  // 直接赋值并不会拷贝数据
  cv::Mat image_another = image;      // 修改 image_another 会导致 image 发生变化
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零

  // 使用clone函数来拷贝数据
  cv::Mat image_clone = image.clone();
  image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);

  return 0;
}

4.2 图像去畸变

​ 在用代码实现时有一个技巧，假设已经得到的是无畸变图像，得到畸变后的图像坐标$(u,v)$,然后再利用插值的方法来填补对应位置的灰度值。

#include <opencv2/opencv.hpp>
#include <string>
using namespace std;

int main(int argc, char **argv) {

  if(argc != 2){cout << "image is used:"<< endl;return 1;}
  
  cv::Mat image;
  image = cv::imread(argv[1], 0);    // 图像是灰度图，CV_8UC1 必须指出0，要不然默认1是彩色。
  // 畸变参数
  double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
  // 内参
  double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;
    
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);  //去畸变以后的图 CV_8UC1单通道

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
        
      // 1. 像素坐标系 转 归一化坐标系
      double x = (u - cx) / fx, y = (v - cy) / fy;
      // 2. 归一化坐标的极坐标表示r
      double r = sqrt(x * x + y * y);
      // 3. 畸变公式
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
      // 4. 归一化 转 像素坐标系
      double u_distorted = fx * x_distorted + cx;
      double v_distorted = fy * y_distorted + cy;

      // 赋值 (最近邻插值)
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
         // 讲对应索引的像素一一赋值
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
      } 
        else // 超出原图像索引的赋值为0
        {
        image_undistort.at<uchar>(v, u) = 0;
      }
    }
  }

  // 画图去畸变后图像
  cv::imshow("distorted", image);
  cv::imshow("undistorted", image_undistort);
  cv::waitKey();
  return 0;
}

4.3 双目视觉

• 1 cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32)双目匹配器

cv::StereoSGBM::create(int minDisparity = 0, int numDisparities = 16, int blockSize = 3, int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, int preFilterCap = 0, int uniquenessRatio = 0, int speckleWindowSize = 0, int speckleRange = 0, int mode = 0)

// 双目深度算法类
// 创建 StereoSGBM 对象   参数:
minDisparity // 最小视差，一般为0
numDisparities // 最大视差 - 最小视差   这个参数必须能被16整除
blockSize // 匹配块大小，奇数，3-11
P1 		// 平滑度参数
P2 		// The larger the values are, the smoother the disparity is. P1 is the penalty on the disparity change by plus or minus 1 between neighbor pixels. P2 is the penalty on the disparity change by more than 1 between neighbor pixels. The algorithm requires P2 > P1 
disp12MaxDiff 		// 左右视差中允许的最大差值
preFilterCap  		// 预过滤图像像素的截断值
uniquenessRatio 	// 最佳(最小)计算成本函数  5-15
speckleWindowSize	// 考虑噪声斑点的平滑视差区域的最大尺寸 50-200
speckleRange		// 每个连通分量内的最大视差变化      

• 2 cv::Ptr<cv::StereoSGBM>

cv::Ptr<cv::StereoSGBM> a;  // 定义 模板类指针 eg

cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);

sgbm就是StereoSGBM模板类的指针，可以调用该类下的一些列成员变量和函数，eg   
sgbm->compute(left, right, disparity_sgbm);
    

• 3 cv::Mat::convertTo类型转换函数

void cv::Mat::convertTo(cv::OutputArray m, int rtype, double alpha = (1.0), double beta = (0.0)) const
   
1 m – 输出矩阵，维度要保证与输入一致
2 rtype – 输出类型 the depth since the number of channels are the same as the input has; 
3 alpha – 可选比例因子.

• 实验stereoVision.cpp

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;


// vector默认的第二个参数是std内存分配器，这里应该改为了Eigen下的内存分配器
// 在pangolin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, 
                    Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;    // 单位m

    // 读取图像
    cv::Mat left = cv::imread(argv[1], 0);
    cv::Mat right = cv::imread(argv[2], 0);
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    
    // sgbm视差，视差图像矩阵
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm); // 利用sgbm计算视差
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);  // 类型转换

    // 生成点云 4维向量 double
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
	// 深度图画的是左图，双目是利用两张图像来求取深度。在这画出深度图，实际上可以利用也可rgb图
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)   // y
        for (int u = 0; u < left.cols; u++) // x
        {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
			// 像素归一化
            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            // 像素坐标系  到  归一化坐标系
            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            // z = f*b/d，视差就是d
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);
    // 画出点云
    showPointCloud(pointcloud);
    return 0;
}

// 画图程序暂时未看
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

4.4 RGB-D视觉

• 实验

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>


using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv) {
    // 这里cv::Mat是为了后面方便类型转换，仍然是一个动态数组容器
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    TrajectoryType poses;         // 相机位姿

    ifstream fin("./pose.txt");   // 读取文件  ifstream对象fin
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) {
        boost::format fmt("./%s/%d.%s"); //图像文件格式
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像，按解码方式读取 读取深度图  默认1读取rgb图

        double data[7] = {0};
        for (auto &d:data)
            fin >> d;  // 我们使用流插入运算符（ << ）向文件写入信息
        			   // 我们使用流提取运算符（ >> ）从文件读取信息
        // 李代数对象pose(q, t) 从四元数和平移向量创建李代数对象se(3)
        Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                          Eigen::Vector3d(data[0], data[1], data[2]));
        // 5张图片，有5行数据，i=0-4  每行一个一个的读取数据
        poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000); // reserve将容器的大小至少设置为1000000

    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];  // 把容器中每个图像 建立 图像矩阵对象
        cv::Mat depth = depthImgs[i];
        
        Sophus::SE3d T = poses[i];		// 
        
        for (int v = 0; v < color.rows; v++)
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值
                if (d == 0) continue; // 为0表示没有测量到
                
                Eigen::Vector3d point;
                // 深度 像素坐标系到归一化坐标系(都是相机坐标系)
                point[2] = double(d) / depthScale;
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                // 相机坐标系 转换 世界坐标系pointWorld
                Eigen::Vector3d pointWorld = T * point;

                Vector6d p;
                // 把世界坐标系位置x,y,z放到前三个位置
                p.head<3>() = pointWorld;
                // 后三位放置色彩值  .stepj
                p[5] = color.data[v * color.step + u * color.channels()];   // blue
                p[4] = color.data[v * color.step + u * color.channels() + 1]; // green
                p[3] = color.data[v * color.step + u * color.channels() + 2]; // red
                pointcloud.push_back(p);
            }
    }

    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

5 相机标定

​ 相机标定是指确定相机的内部参数和外部参数的过程，它可以帮助我们确定图像的真实尺寸和位置关系，进而进行三维重建或其他图像处理操作。包括张正友标定法、极线标定法、卡尔曼滤波标定法等。

​ 这些方法均需要使用标定板，标定板是一种用于相机标定的工具，通常由一个平面板和若干经过标定的点构成。通过测量摄像机与标定板之间的几何关系，可以确定相机的内部参数和外部参数。

5.1 kalibr标定

相机标定实验

5.2 张正友标定

张正友标定视频

张正友标定论文

不需要三维，只在二维上标定。本质是找到了一组比较好的参数初值

​ 标定过程主要分为两个部分：

1. 根据几何光学约束关系，求解成像模型光学和物理参数—初值

2. 建立约束关系，迭代优化已求解参数

5.2.1 求内参

​ 平面靶标上各标记点世界坐标系下坐标齐次表示$(U_i,V_i,1{)}^T$，记为，像素坐标系下坐标记为$(u_i,v_i,1{)}^T$。旋转矩阵前两列记作$R_1$和$R_2$,得到如下公式，其中$A$是内参，$R$是外参，将两者的乘积$A\left(R1R2T1\right)$叫做单应性矩阵$H$。
$$Z_i\left(\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right)=A\left(R1R2T1\right)\left(\begin{array}{c}{U}_i\\ V_i\\ 1\end{array}\right)$$
​ 令$H$的前三列为$(H_1{H}_2{H}_3)$,代入上式中，可得到
$$\left(\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right)=\frac{1}{z_i}H\left(\begin{array}{c}{U}_i\\ V_i\\ 1\end{array}\right)=\frac{1}{Z_i}\left[\begin{array}{ccc}{H}_{11}& H_{12}& H_{13}\\ H_{21}& H_{22}& H_{23}\\ H_{31}& H_{32}& H_{33}\end{array}\right]\left(\begin{array}{c}{U}_i\\ V_i\\ 1\end{array}\right)=(H_1{H}_2{H}_3)$$
​ 因为第三行坐标都是1，所以我们就可以得到关于$Z_i$的表达式，然后消除$Z_i$

$$\begin{array}{r}{u}_i=\frac{H_{11}{U}_i+H_{12}{v}_i+H_{13}}{H_{31}{U}_i+H_{32}{V}_i+H_{33}}\end{array}$$

$$v_i=\frac{H_{21}{U}_i+H_{22}{V}_i+H_{23}}{H_{31}{U}_i+H_{32}{V}_i+H_{33}}$$

​ 式中H为非满秩的齐次矩阵，即独立的未知元素为8个。一个像素点$(u,v)$可以提供两个约束方程，所以我们只需要4个以上的标记点，既可以通过最小二乘法求解对应的齐次矩阵$H$。（这里像极了$p2p$中求解1单应性矩阵$H$!）

​ 因为旋转矩阵是正交矩阵，所以其满足两个性质：

1. 其内列向量两两正交

$$R{1}^{T}R2=0$$

2. 旋转矩阵的转置就是其逆

$$R{1}^{T}R1=R{2}^{T}R2=I$$

​ 然后上面根据$H$和$R$的关系，可以推出：
$$R1=A^{-1}{H}_1,R2=A^{-1}{H}_2$$
​ 把这两个表达式分别带入旋转矩阵的两个性质当中去，可知
$${H_1}^T{A}^{-T}{A}^{-1}{H}_1={H_2}^T{A}^{-T}{A}^{-1}{H}_2=I$$
​ 然后，令$B=A^{-T}{A}^{-1}$,令$A=\left[\begin{array}{ccc}\alpha & \gamma & u_0\\ 0& \beta & v_0\\ 0& 0& 1\end{array}\right]$，把$B$表示为3*3矩阵$B=A^{-T}{A}^{-1}=\left[\begin{array}{ccc}{B}_{11}& B_{12}& B_{13}\\ B_{12}& B_{22}& B_{23}\\ B_{13}& B_{23}& B_{33}\end{array}\right]$,是一个对称矩阵

，所以可以简化为一个6维向量$b=[B_{11}{B}_{12}{B}_{22}{B}_{13}{B}_{23}{B}_{33}{]}^T$

​ 带入前面，可以得到
$$H_i^{T}B{H}_j=v_{ij}^{T}b$$
其中，
$$\begin{array}{cc}{v}_{ij}& \\ & =[H_{i1}{H}_{j1}{H}_{i1}{H}_{j2}+H_{i2}{H}_{j1}{H}_{i2}{H}_{j2}{H}_{i3}{H}_{j1}+H_{i1}{H}_{j3}{H}_{i3}{H}_{j2}+H_{i2}{H}_{j3}{H}_{i2}{H}_{j2}+H_{i2}{H}_{j3}{H}_{i3}{H}_{j3}\end{array}$$
重写相机内参约束等式
$$\left[\begin{array}{c}{v}_{12}^T\\ (v_{11}-v_{22}{)}^T\end{array}\right]b=0$$
​ 由于齐次矩阵$H$已知，矩阵$v$由元素$H$组成，因此矩阵$v$可以被求得。每张标定图像会提供两个约束方程，$b$有6个未知数，因此三张标定样张可求出$b$。当标定样张数多于三张时，采用最小二乘求解最优值。
$$v_0=\frac{B_{12}{B}_{13}-B}{B_{11}{B}_{22}-B_{12}^2}$$

$$\begin{array}{rl}& \{\begin{array}{c}\\ \alpha =\sqrt{\frac{1}{B_{11}}}\\ \beta =\sqrt{\frac{B_{11}}{B_{22}-B_{12}^2}}\\ \gamma =-B_{12}{\alpha }^2\beta \\ u_0=\frac{\gamma v_0}{\beta }-B_{13}{\alpha }^2\end{array}\end{array}$$

5.2.2 求外参

求得摄像机的内参，接下来进行外参求取$A\left(R1R2T1\right)=H$。根据，求得每一张样张对应的$\left(R1R2T1\right)$。根据$R1R2R3$空间方位关系
$$R3=R1\times R2$$

5.2.3 标定镜头畸变

​ 张正友标定法仅考虑畸变模型中影响较大的径向畸变
$$\{\begin{array}{l}\hat{x}=x\left(1+k_1{r}^2+k_2{r}^4\right)\\ \hat{y}=y\left(1+k_1{r}^2+k_2{r}^4\right)\end{array}$$
​ 图像坐标系下坐标与像素坐标系下坐标转化关系
$$u=\frac{x}{dx}+{\hat{u}}_0,v=\frac{y}{dY}+v_0$$
​ 同理，畸变图像像素坐标系下坐标转换关系
$$\hat{u}=\frac{\hat{x}}{dX}+u_0,\hat{v}=\frac{\hat{y}}{dY}+v_0$$
​ 我们把这两个式子带入到上面的径向畸变当中去，可得
$$\{\begin{array}{l}\hat{u}-u_0=(u-u_0)(1+k_1{r}^2+k_2{r}^4)\\ \hat{v}-v_0=(v-v_0)(1+k_1{r}^2+k_2{r}^4)\end{array}$$
化简
$$\{\begin{array}{l}\hat{u}=u+\left(u-u_0\right)\left(k_1{r}^2+k_2{r}^4\right)\\ \hat{v}=v+\left(v-v_0\right)\left(k_1{r}^2+k_2{r}^4\right)\end{array}$$

$$\left[\begin{array}{cc}\left(u-u_0\right)r^2& \left(u-u_0\right)r^4\\ \left(v-v_0\right)r^2& \left(v-v_0\right)r^4\end{array}\right]\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]={\left[\begin{array}{c}\hat{u}-u\\ \hat{v}-v\end{array}\right]}^2$$

即解 $Dk=d$ （线性最小二乘法通式）
$$k=\left[\begin{array}{c}{k}_1\\ k_2\end{array}\right]=(D^{T}D{)}^{-1}{D}^{T}d$$

5.2.4 重投影优化

​ 上面直接求取结果只能作为初值，接下来根据实际的像素坐标和计算得到的像素坐标计算重投影误差。最小值优化重投影误差，反复迭代摄像机的内外参数$ARt$和畸变系数$(k_1,k_2)$，直至收敛。
$$F^{{}^{\prime \prime }}=\sum _{i=1}^n\sum _{j=1}^m{\Vert I_{ij}-{\hat{I}}_{ij}\left(A,k_1,k_2,R_i,t\right)\Vert }^2$$
​ 也就是说，张定友标定法只是求取了一个比较好的优化初始值，避免了陷入局部最优的情况。最后我们仍需要利用最小二乘法来计算得到最佳的目标参数。

5.3 其他标定

c++opencv相机标定（calibrateCamera）详解 - 知乎 (zhihu.com)

6 习题

1. 寻找一部相机(你的手机或笔记本的摄像头即可)，标定它的内参。你可能会用到标定板，或者自己打印一张标定用的棋盘格。

相机标定实验

2. 叙述相机内参的物理意义。如果一部相机的分辨率变为原来的两倍而其他地方不变，那么它的内参将如何变化？

都扩大二倍。图像放大2倍，那么意味着缩放比例也要变大，平移量也要多平移一倍。
参考这里投影模型最后的一个问题，有对这个的推导

3. opencv中鱼眼相机的标定

OpenCV: Fisheye camera model

4. 调研全局快门（global shutter）相机和卷帘快门（rolling shutter）相机的异同。它们在SLAM中有何优缺点？

• 全局快门相机在SLAM中的优点是可以清晰地拍摄高速运动的物体；缺点是当曝光时间过长时，可能会出现成像模糊，且其成本较高，易产生噪声。
• 卷帘快门相机在SLAM中的优点是拍摄的帧速率较高，读取速度快，成像延迟小；缺点是不适合高速运动物体的拍摄。

5. RGB-D相机是如何进行标定的？以 Kinect 为例，需要标定哪些参数？

GitHub - code-iai/iai_kinect2：在 ROS 中使用 Kinect One （Kinect v2） 的工具

6. 除了示例程序演示的遍历图像的方式，你还能举出哪些遍历图像的方法？

1 基本提取

Mat H(100, 100, CV_64F);
for(int i = 0; i < H.rows; i++)
    for(int j = 0; j < H.cols; j++)
        H.at<double>(i,j)=1./(i+j+1);  // 根据不同的矩阵数据类型确定<type>

• If matrix is of type CV_8U then use Mat.at<uchar>(y,x).无符号8位
• If matrix is of type CV_8S then use Mat.at<schar>(y,x).
• If matrix is of type CV_16U then use Mat.at<ushort>(y,x).
• If matrix is of type CV_16S then use Mat.at<short>(y,x).
• If matrix is of type CV_32S then use Mat.at<int>(y,x).
• If matrix is of type CV_32F then use Mat.at<float>(y,x).
• If matrix is of type CV_64F then use Mat.at<double>(y,x).

2 指针扫描图像

• 把图像的每一个像素都设置为0，即黑色。

彩色图像中，一个宽为W，高为H的图像所需要的内存为W*H*3个uchars，一个uchars的字节数为1.

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

int main(int argc, char** argv) {
   
    cv::Mat image1;
    image1 = cv::imread("F:/1.png", -1);                               
    cv::Mat image = image1.clone();   // 深拷贝

    for (size_t y = 0; y < image.rows; y++)
    {
        unsigned char* row_ptr = image.ptr<uchar>(y);   // 行指针
        for (size_t x = 0; x < image.cols;x++)
        {
            unsigned char* data = &row_ptr[x * image.channels()];  // row_ptr[x * image.channels()]是对应索引的值
            for ( int c = 0;c< image.channels();c++)
            {
                data[c] = 0;
            }
        }
    }
    cv::imshow("image", image1);
    cv::imshow("image1", image);
    cv::waitKey(0);
}

• 减色算法

​ 将每个像素的每个通道的值除以$N$(整数除法)。那么

原有的颜色总数 = $256\ast 256\ast 256$

减色后的颜色总数 = $(256/N) *(256/N) *(256/N) $

在下面的例子当中，$N=32$，也就是说每一个通道只有8种颜色，将原来图像的像素值取每个32区间的中间值，分别是$16，48，80，144，176，208，240$.即0-32中取16，32-64中取48，都是取中间值。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

int main(int argc, char** argv) {

    cv::Mat image1;
    image1 = cv::imread("F:/背景图片/333.png", 1);
    cv::Mat image = image1.clone();   // 深拷贝

    for (size_t y = 0; y < image.rows; y++)
    {
        unsigned char* row_ptr = image.ptr<uchar>(y);   // 行指针
        for (size_t x = 0; x < image.cols; x++)
        {
            unsigned char* data = &row_ptr[x * image.channels()];  // row_ptr[x * image.channels()]是对应索引的值
            for (int c = 0; c < image.channels(); c++)
            {
                data[c] = data[c] / 32 * 32 + 16;   // data[c]可能存在的最大值255（单一通道），所以减色算法后data[c]最大是7*32+16
            }
        }
    }
    cv::imshow("image", image1);
    cv::imshow("image1", image);
    cv::waitKey(0);
}

• 高度设置为1，宽度为W*H

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

int main(int argc, char** argv) {

    cv::Mat image = cv::imread("F:/1.png", -1);
    int row = image.rows; int c = image.channels();
    image = image.reshape(1, 1);    // 第一个参数为通道数，第二个是新的行数
    int n1 = image.rows;
    int nc = image.cols * image.channels();
    uchar* data = image.data;  // 获取内存块第一个元素的指针
    for (int i = 0; i < nc; i++)
    {
        data[i] = 0;
    }

    cv::imshow("resize1", image);
L
    image = image.reshape(c,row);
    cv::imshow("resize2", image);
    cv::waitKey(0);
}

3 迭代器扫描图像

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

using namespace std;

void colorReduce(cv::Mat& image, int div = 64) {
    //在初始位置获得迭代器
    cv::Mat_<cv::Vec3b>::iterator it = image.begin<cv::Vec3b>();
    //获得结束位置
    cv::Mat_<cv::Vec3b>::iterator itend = image.end<cv::Vec3b>();
    //循环遍历
    for (; it != itend; ++it) {
        // 处理每个像素 ---------------------
        (*it)[0] = (*it)[0] / div * div + div / 2;
        (*it)[1] = (*it)[1] / div * div + div / 2;
        (*it)[2] = (*it)[2] / div * div + div / 2;
        //像素处理结束-----------------------
    }
}

int main() {

    cv::Mat image = cv::imread("F:/背景图片/333.png");
    int N = 64;
    cv::Mat_<cv::Vec3b>::iterator it = image.begin<cv::Vec3b>();   // cv::MatIterator_<cv::Vec3b>::iterator
    auto iend = image.end<cv::Vec3b>();   
    for (;it != iend;++it)
    {
        (*it)[0] = (*it)[0]/N*N + N/2;
        (*it)[1] = (*it)[1] / N * N + N/2;
        (*it)[2] = (*it)[2] / N * N + N/2;
    }
    // colorReduce(image);
    cv::imshow("image", image);
    cv::waitKey(0);
}

图像遍历程序参考：Opencv计算机视觉编程攻略