图像去畸变
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,] , (坐标都是列向量的形式，懒得打转置符号了)
程序中遍历的是像素坐标[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 = 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 ptrhttps://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>> pointcloudvector<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数据的通道数。
视差图是指将左右相机观测同一点计算的视差值作为像素的图像
深度图像也叫距离影像，是指将从图像采集器到场景中各点的距离（深度）值作为像素值的图像
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 循环。
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-****博客_glvertex2fhttps://blog.****.net/u013642494/article/details/418113178. usleep()函数
sleep()和usleep()的使用和区别 - 简书 (jianshu.com)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下