视觉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,视觉SLAM ch5代码总结(二)]  ,  (坐标都是列向量的形式,懒得打转置符号了)

  程序中遍历的是像素坐标[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)视觉SLAM ch5代码总结(二)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 = 视觉SLAM ch5代码总结(二) 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<>的应用的几点问题_耕读传家远的博客-****博客_opencv ptr视觉SLAM ch5代码总结(二)https://blog.****.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的专栏-****博客视觉SLAM ch5代码总结(二)https://blog.****.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)视觉SLAM ch5代码总结(二)http://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-****博客_glvertex2f视觉SLAM ch5代码总结(二)https://blog.****.net/u013642494/article/details/418113178. usleep()函数

sleep()和usleep()的使用和区别 - 简书 (jianshu.com)视觉SLAM ch5代码总结(二)https://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下

上一篇:SilverlightOA源代码(可用于企业级Silverlight项目的二次开发,长年有效)


下一篇:机器人环境感知算法之鲁棒感知阶段