其他分享
首页 > 其他分享> > 视觉SLAM ch5代码总结（二）

视觉SLAM ch5代码总结（二）

作者：互联网

图像去畸变 

CMakeLists.txt

cmake_minimum_required(VERSION 3.10)
project(basics)

#Eigen
include_directories("/usr/include/eigen3")

#opencv
find_package(OpenCV REQUIRED)
#添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(imageBasics imageBasics.cpp)
target_link_libraries(imageBasics ${OpenCV_LIBS})

add_executable(undistortImage undistortImage.cpp)
target_link_libraries(undistortImage ${OpenCV_LIBS})

这部分代码和上一个imageBasics在同一文件夹下，所以关于CMakeLists.txt只看最后两行就行。

undistortImage.cpp

去畸变大概流程：

一、确定畸变参数  k1 k2 k3 p1 p2。 前三个是径向畸变的参数 ， 后两个是切向畸变参数

二、归一化平面任意一点P的坐标[ x,y ] ,  极坐标为  [r,\theta]  ,  (坐标都是列向量的形式，懒得打转置符号了)

  程序中遍历的是像素坐标[u,v] （u和v是已知量，x和y是未知量），因此这里是把像素坐标转为归一化坐标  （书99页 公式5.5）

 x = (u - cx) / fx,   y = (v - cy) / fy;

三、再对刚刚计算得到的归一化平面上的点计算径向畸变和切向畸变

r = sqrt(x*x+y*y)
x_d = x*(1+k1*r*r+k2*r*r*r*r)+2*p1*x*y+p2*(r*r+2*x*x);        (k3 = 0)
y_d=y*(1+k1*r*r+k2*r*r*r*r)+2*p2*x*y+p1*(r*r+2*y*y);

四、对畸变后的点通过内参矩阵投影到像素平面上        P(u_d,v_d) = K P(x_d,y_d)

u_d=fx*x_d+cx;
v_d=fy*y_d+cy;

最后得到了该点在图像的正确位置 （u_d,v_d）

五 赋值 （最近邻插值）  去畸变

最近邻插值法（nearest_neighbor） - wancy - 博客园 (cnblogs.com)https://www.cnblogs.com/wancy/p/15068519.html如果畸变后像素在图像方位内，则将其映射到去畸变图像相应的坐标位置

双目视觉

从像素坐标很容易还原回 路标点在归一化平面上的坐标Pc1 = [X'/Z', Y'/Z', 1] = [x, y, 1]，再通过双目相机的视差估计出深度信息，我们最终得到路标点在相机坐标系下的坐标Pc = [X', Y', Z']，这些点共同组成了一个点云图。

双目流程：

 一、遍历图像的像素

二、像素坐标转化为归一化坐标

x = (u - cx) / fx,   y = (v - cy) / fy;

三、SGBM算法将算到的视差以像素的形式存储到视差图disparity中

深度 =  fx * b  /  d             （书99页  fx = \alpha f，计算深度的时候把 f 当做 fx，不是很理解，有懂的可以评论下） 

四、根据深度可以计算相机坐标系下所有点的坐标，这些点组成了点云图。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(stereoVision)

find_package(OpenCV REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(
			"/usr/include/eigen3"
			${OpenCV_INCLUDE_DIRS}
			${Pangolin_INCLUDE_DIRS}
)

add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

steroVision.cpp

1.  StereoSGBM::create()

static Ptr<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 = StereoSGBM::MODE_SGBM);

semi-global matching（SGM）是一种用于计算双目视觉中视差（disparity）的半全局匹配算法，

minDisparity 最小的可能的视差值。正常情况下，它为零，但有时校正算法会移动图像，因此需要相应调整此参数。

numDisparity 最大视差减去最小视差。该值始终大于零。在实现中，此参数必须可以被16整除。

blockSize   匹配的块大小。它必须是奇数>=1。通常情况下，它应该在3-11范围

P1, P2：控制视差变化平滑性的参数。P1、P2的值越大，视差越平滑。P1是相邻像素点视差增/减 1 时的惩罚系数；P2是相邻像素点视差变化值大于1时的惩罚系数。P2必须大于P1。

disp12MaxDiff是左右图视差检查所允许的最大的不同值

preFilterCap：预处理滤波器的截断值，该算法首先计算每个像素的x导数，预处理的输出值仅保留[-preFilterCap, preFilterCap]范围内的值。结果值将传递给Birchfield-Tomasi像素代价函数。

uniquenessRatio：视差唯一性百分比。通常，5-15范围内的值就足够了。

speckleWindowSize  检查视差连通区域变化度的窗口大小, 值为 0 时取消 speckle 检查。否则，将其设置在50-200范围内。

speckleRange：视差变化阈值，当窗口内视差变化大于阈值时，该窗口内的视差清零，否则将参数设置为正值，它将隐式地乘以16。通常，1或2就足够了。

mode 将其设置为StereoSGBM:：MODE_HH以运行全刻度双通道动态编程算法。默认情况下，它设置为false。

2. cv::Ptr<>  Ptr类模板   （这种模板在以后会多次出现）

(1条消息) OpenCV中Ptr<>的应用的几点问题_耕读传家远的博客-CSDN博客_opencv ptrhttps://blog.csdn.net/qq_22329695/article/details/70880080Ptr<>模板类就是一种智能指针，也可以理解成一个指针，或者是一个指针类，可以防止内存泄漏等问题，比普通指针好用。

例：

Ptr<StereoSGBM> sgbm

Ptr<StereoSGBM>就是一个模板类，定义的变量sgbm是一个指向StereoSGBM对象的一个指针。

3. vector<xxx,aligned_allocator<xxx>   关于自定义动态申请变量的方法重复出现多次

vector<Vector4d,Eigen::aligned_allocator<Vector4d>>   pointcloud
vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>  poses;
//typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d> >  TrajectoryType;     

//typedef定义一个容器类型TrajectoryType，里面装的是位姿(time 平移，四元数【旋转】)

aligned_allocator在做一件事，告诉vector容器 (Vector4d/Isometry3d/SE3D)的内存大小。

这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。

关于什么时候加自定义动态申请变量：

当vector中元素是Eigen中的类型的时候，必须调用Eigen::aligned_allocator<>分配内存空间，否则编译不会报错，运行会报错 

4.  convertTo（）

函数用法：src.convertTo(Mat& m, int rtype, double alpha=1, double beta=0 ）

m  目标矩阵。如果m的大小与原矩阵不一样，或者数据类型与参数不匹配，那么在函数convertTo内部会先给m重新分配空间。

rtype 指定从原矩阵进行转换后的数据类型，即目标矩阵m的数据类型。当然，矩阵m的通道数应该与原矩阵一样的。如果rtype是负数，那么m矩阵的数据类型应该与原矩阵一样。

alpha 缩放因子。默认值是1。即把原矩阵中的每一个元素都乘以alpha。

beta 增量。默认值是0。即把原矩阵中的每一个元素都乘以alpha，再加上beta。

例：
cv::Mat src, src_f;
src.convertTo(src_f, CV_32F, 1.0/255, 0);
图像大小没有变化，但是类型变为了FLOAT32位，

注意也不是所有格式的Mat型数据都能被使用保存为图片，目前OpenCV主要只支持单通道和3通道的图像，并且此时要求其深度为8bit和16bit无符号(即CV_16U)，所以其他一些数据类型是不支持的，比如说float型等。（书中代码要求转换为CV_32F）
如果Mat类型数据的深度和通道数不满足上面的要求，则需要使用convertTo()函数和cvtColor()函数来进行转换。
convertTo()函数负责转换数据类型不同的Mat，即可以将类似float型的Mat转换到imwrite()函数能够接受的类型。
而cvtColor()函数是负责转换不同通道的Mat，因为该函数的第4个参数就可以设置目的Mat数据的通道数。

opencv——convertTo_yangjianqiao0的专栏-CSDN博客icon-default.png?t=LA92https://blog.csdn.net/yangjianqiao0/article/details/380100515.视差图和深度图

视差图是指将左右相机观测同一点计算的视差值作为像素的图像

深度图像也叫距离影像，是指将从图像采集器到场景中各点的距离（深度）值作为像素值的图像

6.基于范围for循环  c++11新特性

就是定义的变量遍历后边数组或者容器的每一个值

例：

vector<int> vec;

for (auto iter = vec.begin(); iter != vec.end(); iter++) { // before c++11
    cout << *iter << endl;
}

for (auto i : vec) { // c++11基于范围的for循环
    cout << "i" << endl;
}

 如果要修改vec内容：

for (auto& i : vec) { 
    i++;             增加vec元素中的值
}

for (auto i : vec) { 
    cout << "i" << endl;   输出vec的值
}

请注意范围变量 i，在它的名称前面有一个 & 符号，这表示它被声明为引用变量。当循环执行时，i 变量将不再只是元素的副本，而是变成了元素本身的别名。因此，对 i 变量作出的任何修改都会实际作用于它当前引用的元素。

相比之下，后一个变量 i 前面就没有 & 符号，这是因为在此无须将 i 声明为引用变量。该循环只是要显示元素，而不是改变它们。

基于范围的 for 循环和常规 for 循环对比

基于范围的 for 循环可用于需要遍历数组所有元素的任何情形，并且不需要使用元素下标。但是，如果出于某些目的需要使用元素下标时，基于范围的 for 循环就不能使用了。另外，如果循环控制变量被用于访问两个或两个以上不同数组的元素，那么它也不适合使用。在这些情况下，可以使用常规 for 循环。

C++基于范围的for循环详解 (biancheng.net)icon-default.png?t=LA92http://c.biancheng.net/view/1416.html#:~:text=%E5%9B%A0%E4%B8%BA%E5%9F%BA%E4%BA%8E%E8%8C%83%E5%9B%B4%E7%9A%84%20for%20%E5%BE%AA%E7%8E%AF%E5%8F%AF%E4%BB%A5%E8%87%AA%E5%8A%A8%E7%9F%A5%E9%81%93%E6%95%B0%E7%BB%84%E4%B8%AD%E5%85%83%E7%B4%A0%E7%9A%84%E4%B8%AA%E6%95%B0%EF%BC%8C%E6%89%80%E4%BB%A5%E4%B8%8D%E5%BF%85%E4%BD%BF%E7%94%A8%E8%AE%A1%E6%95%B0%E5%99%A8%E5%8F%98%E9%87%8F%E6%8E%A7%E5%88%B6%E5%85%B6%E8%BF%AD%E4%BB%A3%EF%BC%8C%E4%B9%9F%E4%B8%8D%E5%BF%85%E6%8B%85%E5%BF%83%E6%95%B0%E7%BB%84%E4%B8%8B%E6%A0%87%E8%B6%8A%E7%95%8C%E7%9A%84%E9%97%AE%E9%A2%98%E3%80%82%20%E5%9F%BA%E4%BA%8E%E8%8C%83%E5%9B%B4%E7%9A%84%20for%20%E5%BE%AA%E7%8E%AF%E4%BD%BF%E7%94%A8%E4%BA%86%E4%B8%80%E4%B8%AA%E7%A7%B0%E4%B8%BA%20%E8%8C%83%E5%9B%B4%E5%8F%98%E9%87%8F%20%E7%9A%84%E5%86%85%E7%BD%AE%E5%8F%98%E9%87%8F%E3%80%82,%E6%AF%8F%E6%AC%A1%E5%9F%BA%E4%BA%8E%E8%8C%83%E5%9B%B4%E7%9A%84%20for%20%E5%BE%AA%E7%8E%AF%E8%BF%AD%E4%BB%A3%E6%97%B6%EF%BC%8C%E5%AE%83%E9%83%BD%E4%BC%9A%E5%A4%8D%E5%88%B6%E4%B8%8B%E4%B8%80%E4%B8%AA%E6%95%B0%E7%BB%84%E5%85%83%E7%B4%A0%E5%88%B0%E8%8C%83%E5%9B%B4%E5%8F%98%E9%87%8F%E3%80%82%20%E4%BE%8B%E5%A6%82%EF%BC%8C%E7%AC%AC%E4%B8%80%E6%AC%A1%E5%BE%AA%E7%8E%AF%E8%BF%AD%E4%BB%A3%EF%BC%8C%E8%8C%83%E5%9B%B4%E5%8F%98%E9%87%8F%E5%B0%86%E5%8C%85%E5%90%AB%E5%85%83%E7%B4%A0%200%20%E7%9A%84%E5%80%BC%EF%BC%9B%E7%AC%AC%E4%BA%8C%E6%AC%A1%E5%BE%AA%E7%8E%AF%E8%BF%AD%E4%BB%A3%EF%BC%8C%E8%8C%83%E5%9B%B4%E5%8F%98%E9%87%8F%E5%B0%86%E5%8C%85%E5%90%AB%E5%85%83%E7%B4%A0%201%20%E7%9A%84%E5%80%BC%EF%BC%8C%E4%BB%A5%E6%AD%A4%E7%B1%BB%E6%8E%A8%E3%80%827.使用pangolin库画图

Pangolin是一个基于OpenGL的轻量级开源绘图库。

这部分代码前面已经写过两次了，感觉可以把一直到 glClearColor（）函数前面部分当作一个模板，当画图的时候可以当过来直接用。

glClearColor(1.0f , 1.0f , 1.0f , 1.0f);   //设置背景颜色为白色

前面两个画图画的是线，这里画的是点，因此关于设置线或者点的大小代码不同

glLineWidth(2);   线宽 
glPointSize(2);   点的大小

glBegin() 告诉计算机我们是要画线还是点   glBegin()和glEnd()是成对出现的，不管画什么写在他们之间

glBegin(GL_LINES)   画线
glBegin(GL_POINTS)  画点

例：画直线 

void display()
{
	glClear( GL_COLOR_BUFFER_BIT);   设置背景颜色
	glBegin( GL_LINES); 
    glColor3f(0.0,0.0,0.0);       指定颜色 三个参数分别表示 R G B
		glVertex2f( 0.0f, 0.0f);
		glVertex2f( -0.3f, 0.1f);   二维的点
		glVertex2f( 0.1f, 0.3f);
		glVertex2f( 0.4f, 0.6f);
	glEnd();
	pangolin::FinishFrame();            按照上面的设置执行渲染
    usleep(5000);
}

上述代码画了两条直线，（0，0）——（-0.3，0.1）        （0.1，0.3）——（0.4，0.6）

画点：

void display()
{
	glClear( GL_COLOR_BUFFER_BIT);   设置背景颜色
	glBegin( GL_POINTS); 
    glColor3f(0.0,0.0,0.0);       指定颜色 三个参数分别表示 R G B
    glVertex3f( 0.0f, 0.0f,0.0f);  三维的点
	glEnd();
	pangolin::FinishFrame();            按照上面的设置执行渲染
    usleep(5000);
}

openGL学习笔记三 ： 绘制点、线以及多边形_hushuai1992-CSDN博客_glvertex2ficon-default.png?t=LA92https://blog.csdn.net/u013642494/article/details/418113178. usleep()函数

sleep()和usleep()的使用和区别 - 简书 (jianshu.com)icon-default.png?t=LA92https://www.jianshu.com/p/ccdb9be5c6ef

sleep()
头文件: #include <windows.h> // 在VC中使用带上头文件，或#include <unistd.h> // 在gcc编译器中，使用的头文件因gcc版本的不同而不同
功 能: 执行挂起指定的秒数
语 法: sleep(unsigned seconds );

函数名: usleep()
头文件: #include <unistd.h>
功 能: usleep功能把进程挂起一段时间, 单位是微秒（百万分之一秒）；
语 法: usleep(int micro_seconds);
内容说明：本函数可暂时使程序停止执行。参数 micro_seconds 为要暂停的微秒数(us)。
注 意：
这个函数不能工作在windows 操作系统中。用在Linux的测试环境下面。一般情况下，延迟时间数量级是秒的时候，尽可能使用sleep()函数。 如果延迟时间为几十毫秒（1ms = 1000us），或者更小，尽可能使用usleep()函数。这样才能最佳的利用CPU时间。

总结：

功能与sleep类似，只是传入的参数单位是微妙
若想最佳利用cpu，在更小的时间情况下，选择用usleep
usleep传入的参数是整形，所以不能传小数
usleep不能工作在windows上，只能在linux下

标签：8C%,E5%,E6%,E7%,SLAM,ch5,E4%,视觉,视差
来源： https://blog.csdn.net/qq_41451702/article/details/122043813