第二讲
本讲主要是cmake的使用以及一些库的链接方式
#声明cmake的最低版本
cmake_minimum_required(VERSION 2.8)
#声明一个cmake工程
project(HelloSLAM)
#设置编译模式
set(CMAKE_BUILD_TYPE"Debug")
#添加可执行程序
add_executable(helloSLAM helloSLAM.cpp)
#下一部分
#添加库文件
add_library(hello STATIC libHelloSLAM.cpp) #静态链接库,会生成.a文件,可默认不加STATIC或者SHARED
# add_library(hello_shared SHARED libHelloSLAM.cpp) #动态链接库，会生成.so文件
add_executable(useHello useHello.cpp)
target_link_libraries(useHello hello)
补充
添加头文件include_directories()可使用具体路径，例如：
#添加Eigen头文件
include_directories(“/usr/include/eigen3”)
静态库
• 原理:在编译时将源代码复制到程序中，运行时不用库文件依旧可以运行。
• 优点:运行已有代码，运行时不用再用库;无需加载库，运行更快
• 缺点:占用更多的空间和磁盘;静态库升级，需要重新编译程序
共享库(常用)
• 原理:编译时仅仅是记录用哪一个库里面的哪一个符号，不复制相关代码
• 优点:不复制代码，占用空间小;多个程序可以同时调用一个库;升级方便，无需重新编译 • 缺点:程序运行需要加载库，耗费一定时间
| 操作系统 | 静态库 | 共享库 |
|---|---|---|
| Windows | lib | .dll |
| Linux | .a | .so |
| Mac OS | .a | dylib |
第三讲
第三讲主要是三维空间刚体运动
Eigen库的使用
ch3/useEigen/eigenMatrix.cpp中的需要记住的
using namespace Eigen;
Vector3d v_3d; //实际上是MAtrix<double,3,1>
Matrix3d matrix_33=Matrix3d::Zero();// Eigen::Matrix<double, 3, 3>
Matrix<float,2,3> matrix_23;
//可以直接这样输入数据
matrix_23<<1,2,3,4,5,6;
//相乘不能混用，需要显式转换
Matrix<double,2,1> result =matrix_23.cast<double>()*v_3d;
matrix_33.transpose()；//转置
matrix_33.trace()//迹
matrix_33.inverse() ;//求逆
matrix_33.determinant();//求行列式
//实对称矩阵一定可以对角化
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose()*matrix_33);
eigen_solver.eigenvalues()；//特征值
eigen_solver.eigenvectors()；//特征向量
//求解 matrix_NN * x = v_Nd 这个方程.QR分解
x=matrix_NN.colPivHouseholderQr().solve(v_Nd);
ch3/useGeometry/eigenGeometry.cpp中
Matrix3d rotation_matrix=Matrix3d::Identity();//单位阵,旋转矩阵
AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1));//旋转向量，使用的AngleAxisd
//旋转角以及旋转轴
cout << "rotation_vector" << "angle is: " << rotation_vector.angle() * (180 / M_PI)
<< " axis is: " << rotation_vector.axis().transpose() << endl;
//旋转向量变旋转矩阵
rotation_matrix = rotation_vector.toRotationMatrix();
//旋转矩阵变旋转向量
rotation_vector.fromRotationMatrix(rotation_matrix);
// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即yaw-pitch-roll顺序
//欧式变换使用Isometry,这里注意需要使用头文件 #include<Eigen/Geometry>
Isometry3d T = Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵
T.rotate(rotation_vector); // 按照rotation_vector进行旋转
T.pretranslate(Vector3d(1, 3, 4)); // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() << endl;
//四元数:拥有一个实部，3个虚部，Quaterniond是（s(q0),q1,q2,q3）
Quaterniond q=Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose()<< endl;
// 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部补充：pretranslate 是在旋转之前的坐标轴上进行的平移操作，而 translate 是在旋转之后，pretanslate 相当于左乘，translate 相当于右乘。
Eigen 中 pretanslate 和 translate 的区别
###ch3/examples/plotTrajectory.cpp中需要注意的
使用ifstream流来读取文件
说明：
1.ifstream类的对象创建成功的时候会返回非空值，借此判断是否创建文件对象成功
2.ifstream有个函数eof()用来判断文件是否读到尾部,没读到尾部返回false，否则返回true。
若尾部有回车，那么最后一条记录会读取两次。
若尾部没有回车，那么最后一条记录只会读取一次
3.iftream的对象假设为fin，fin在读取数据的时候会根据你的输出对象来选择输出的方式。
例程
#include<iostream>
#include<fstream>
using namespace std;
int main()
{
ifstream fin("abc.txt");//读取文件的名字，可以相对或绝对
if(!fin)
{
cout<<"open fail."<<endl;
exit(1);
}
else{
while(!fin.eof())
{
char a[20],b[20],c[20];
fin>>a>>b>>c;//读取的时候遇见空格才会跳跃。
cout<<a<<" "<<b<<" "<<c<<" "<<endl;
}
fin.close();
}
}如果STL容器中的元素是Eigen库数据结构，例如这里定义一个vector容器，元素是Matrix4d ，如下所示：
vectorEigen::Matrix4d
这个错误也是和上述一样的提示，编译不会出错，只有在运行的时候出错。解决的方法很简单，定义改成下面的方式：
vector<Eigen::Matrix4d,Eigen::aligned_allocatorEigen::Matrix4d>;
其实上述的这段代码才是标准的定义容器方法，只是我们一般情况下定义容器的元素都是C++中的类型，所以可以省略，这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。
对于轨迹文件的处理过程如下：
while (!fin.eof()) {
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
Isometry3d Twr(Quaterniond(qw, qx, qy, qz)); // 对比旋转向量的T.rotate(rotation_vector); // 按照rotation_vector进行旋转
Twr.pretranslate(Vector3d(tx, ty, tz));
poses.push_back(Twr);
}
第四讲
主要是李群以及李代数，Sophus的使用
ch4/useSophus.cpp中
//李群的表示
// 沿Z轴转90度的旋转矩阵
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
// 或者四元数
Quaterniond q(R);
Sophus::SO3d SO3_R(R); // Sophus::SO3d可以直接从旋转矩阵构造
Sophus::SO3d SO3_q(q); // 也可以通过四元数构造
// 对SE(3)操作大同小异
Vector3d t(1, 0, 0); // 沿X轴平移1
Sophus::SE3d SE3_Rt(R, t); // 从R,t构造SE(3)
Sophus::SE3d SE3_qt(q, t); // 从q,t构造SE(3)
//通过.matrix()查看李群的矩阵
//通过对数映射转化为李代数，这里的log直接把李群转化为李代数，不是反对称矩阵
Vector3d so3 = SO3_R.log();
cout << "so3 = " << so3.transpose() << endl;
// hat 为向量到反对称矩阵
cout << "so3 hat=\n" << Sophus::SO3d::hat(so3) << endl;
// 李代数se(3) 是一个六维向量，方便起见先typedef一下
typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = SE3_Rt.log();
cout << "se3 = " << se3.transpose() << endl;
// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后.需要注意
// 同样的，有hat和vee两个算符
cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
// 增量扰动模型的更新，李代数求导
Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;
Vector6d update_se3; //更新量,这里不是太懂
update_se3.setZero();
update_se3(0, 0) = 1e-4;
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
ch4/example/trajectoryError.cpp中比较真实轨迹与估计轨迹之间的误差
//vector（动态数组）类型的TrajectoryType存储位姿
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
//通过下面这样读取至李群中
while (!fin.eof()) {
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
trajectory.push_back(p1);
}
//轨迹误差的计算方法如下
double rmse = 0;
for (size_t i = 0; i < estimated.size(); i++) {
Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i];
double error = (p2.inverse() * p1).log().norm();
rmse += error * error;
}
rmse = rmse / double(estimated.size());
rmse = sqrt(rmse);
cout << "RMSE = " << rmse << endl;
第五讲
本讲主要是相机与图像,内参外参畸变参数等，Opencv使用以及摄像头标定等
ch5/imageBasics/imageBasics.cpp中操作Opencv图像中需要注意的
image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像
// 文件顺利读取, 首先输出一些基本信息
cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
cv::imshow("image", image); // 用cv::imshow显示图像
cv::waitKey(0); // 暂停程序,等待一个按键输入
// 使用 std::chrono 来给算法计时
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
sleep(1);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;
//time_point表示一个时间点，用来获取1970.1.1以来的秒数和当前的时间。time_point必须要clock来计时，time_point有一个函数time_since_epoch()用来获得1970年1月1日到time_point时间经过的duration
//时钟间隔Duration
// 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
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++) {
// 访问位于 x,y 处的像素
unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
// 输出该像素的每个通道,如果是灰度图就只有一个通道
for (int c = 0; c != image.channels(); c++) {
unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
}
}
}
// 关于 cv::Mat 的拷贝
// 直接赋值并不会拷贝数据
cv::Mat image_another = image;
// 修改 image_another 会导致 image 发生变化
image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零
cv::imshow("image", image);
cv::waitKey(0);
// 使用clone函数来拷贝数据
cv::Mat image_clone = image.clone();
image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);
cv::imshow("image", image);
cv::imshow("image_clone", image_clone);
cv::waitKey(0);
补充
CMakeLists.txt文件代码
cmake_minimum_required( VERSION 2.8 )
project(imageBasics)
set( CMAKE_CXX_FLAGS "-std=c++11 -O3")
find_package(OpenCV)
include_directories(${OpenCV})
add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})
add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})数组名与指针的关系
a;//代表数组首行地址，一般用a[0][0]的地址表示
&a;//代表整个数组的地址，一般用a[0][0]地址表示
a[i];代表了第i行起始元素的地址(网上说是代表了第i行的地址，但我觉得不是，在讲数组与指针的关系时我会验证给大家看)
&a[i];代表了第i行的地址，一般用a[i][0]的地址表示
a[i]+j;//代表了第i行第j个元素地址,a[i]就是j==0的情况
a[i][j];//代表了第i行第j个元素
&a[i][j];//代表了第i行第j个元素的地址
*a;//代表数组a首元素地址也就是a[0]或者&a[0][0]
*(a+i);//代表了第i行首元素的地址,*a是i=0的情况
*(a+i)+j;//代表了第i行j个元素的地址
**a;//代表a的首元素的值也就是a[0][0]
*(*(a+i)+j);//代表了第i行第j个元素
几种及时函数如下：
//方法1,标准库
#include <sys/time.h>
struct timeval tv;
gettimeofday(&tv,NULL);
auto b1=(unsigned long long)tv.tv_sec*1000+(unsigned long long)tv.tv_usec/1000;
sleep(1);
gettimeofday(&tv,NULL);
auto b2=(unsigned long long)tv.tv_sec*1000+(unsigned long long)tv.tv_usec/1000;
cout<<"method 1, cost time is:"<<(b2-b1)<<endl;
//方法2，chrono
#include<chrono>
using namespace std::chrono;
auto t1= duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
sleep(1);
auto t2=duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
cout<<"method 2, cost time is:"<<(t2-t1)<<endl;
//方法2副本，该代码只能求两段时间差，不能得到打印出当前时刻。
auto begin=system_clock::now();
sleep(1);
auto end=system_clock::now();
cout<<"method 2-1, cost time is:"<<duration_cast<milliseconds>(end - begin).count()<<endl;
//方法3
#include <unistd.h>
clock_t c1=clock();
sleep(1);
clock_t c2=clock();
cout<<"method 3, cost time is:"<<(c2-c1)<<endl;
ch5/imageBasics/undistortImage.cpp中图像去畸变
//主要知道内参以及去畸变参数
// 畸变参数
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;
// 计算去畸变后图像的内容
for (int v = 0; v < rows; v++) {
for (int u = 0; u < cols; u++) {
// 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
double x = (u - cx) / fx, y = (v - cy) / fy;
double r = sqrt(x * x + y * y);
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;
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 {
image_undistort.at<uchar>(v, u) = 0;
}
}
}
Opencv提供了去畸变函数cv::Undistort().
函数功能：直接对图像进行畸变矫正。
其内部调用了initUndistortRectifyMap和remap函数。
CV_EXPORTS_W void undistort( InputArray src, //原始图像
OutputArray dst,//矫正图像
InputArray cameraMatrix,//原相机的内参矩阵
InputArray distCoeffs,//相机矫正参数
InputArray newCameraMatrix = noArray() );//新相机内参矩阵直接使用Opencv中的函数主要是对参数写成矩阵形式加进去就可以了
#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
int main(int argc, char **argv) {
const cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 458.654, 0.0, 367.215, 0.0, 457.296, 248.375, 0.0, 0.0, 1.0 ); // 相机内参矩阵
//k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
const cv::Mat D = (cv::Mat_<double> ( 5,1 ) << -0.28340811, 0.07395907, 0.0, 0.00019359, 1.76187114e-05);
//double k1 = -0.28340811, k2 = 0.07395907, k3=0,p1 = 0.00019359, p2 = 1.76187114e-05;
const string str = "/home/chy/slambook2/ch5/imageBasics/distorted.png";
cv::Mat image = cv::imread(str, 0); // 图像是灰度图，CV_8UC1
int rows = image.rows, cols = image.cols;
cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸变以后的图
cv::undistort(image,image_undistort,K,D,K);
// 画图去畸变后图像
cv::imshow("distorted", image);
cv::imshow("undistorted", image_undistort);
cv::waitKey();
return 0;
}
使用 getOptimalNewCameraMatrix + initUndistortRectifyMap + remap 矫正图像
ch5/stereo/stereoVision.cpp中双目视觉
使用SGBM算法计算左右图像的视差
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);//输入前面两张图，第三个参数是输出
disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f); //将disparity_sgbm变成32F类型的disparity，这里的disparity才是视差图。 如果Mat类型数据的深度不满足上面的要求，则需要使用convertTo()函数来进行转换。convertTo()函数负责转换数据类型不同的Mat
for (int v = 0; v < left.rows; v++)
for (int u = 0; u < left.cols; u++) {
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;
double depth = fx * b / (disparity.at<float>(v, u));
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;
pointcloud.push_back(point);
}sgbm参数解释：
cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32); // sgbm经典参数配置
convertTo函数
用于计算距离的视差图（CV_32F）和用于肉眼看的视差图（CV_8U）使用的格式不同，并且用于计算的视差图无需进行裁剪和归一化，这些只是为了显示的可读性和美观。所以，在对sgbm进行compute之后得到视差图disparity_sgbm，除以16得到用于计算的视差图disparity（除以16是因为每个像素值由一个16bit表示，其中低位的4位存储的是视差值得小数部分，所以真实视差值应该是该值除以16
这部分Pangolin的讲解也可以参考上面的链接
ch5/rgbd/joinMap.cpp中
已知相机内外参，五张RGB图以及他们的深度信息计算任何一个像素的世界坐标系下的位置从而建立一个点云地图
//起个别名，方便后面使用
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
//通过fmt占位符读取图像，具体看下面fmt讲解链接，要先include <boost/format.hpp>这个头文件。
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读取原始图像
// for 在C++11的新特性,具体看下面链接讲解
double data[7] = {0};
for (auto &d:data)
fin >> d;
// 计算点云并拼接
// 相机内参
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;
// reserve为容器预先分配内存空间，并未初始化空间元素
pointcloud.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++) {
/*通过用Mat中的ptr模板函数 返回一个unsigned short类型的指针。v表示行 根据内部计算返回data头指针 + 偏移量来计算v行的头指针
* 图像为单通道的 depth.ptr<unsigned short> ( v ) 来获取行指针*/
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;
Eigen::Vector3d pointWorld = T * point;//将相机坐标系转换为世界坐标系
Vector6d p;
//head<n>()函数是对于Eigen库中的向量类型而言的，表示提取前n个元素
//方法一
p.head<3>() = pointWorld;
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
//方法二：
//程序上方读取了一张图片color
p[5]=color.at<cv::Vec3b>(v, u)[0]; //B
p[4]=color.at<cv::Vec3b>(v, u)[1]; //G
p[3]=color.at<cv::Vec3b>(v, u)[2]; //R
pointcloud.push_back(p);
}
}
vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
pointcloud.reserve(1000000);
/* 备注： 3通道的图像的遍历方式总结
* 对于单通道来说 每个像素占8位 3通道则是每个矩阵元素是一个Vec3b 即一个三维的向量 向量内部元素为8位数的unsigned char类型
* 1、使用at遍历图像
* for(v)row
* for(u)col
* image.at<Vec3b>（v,u）[0] 表示第一个通道的像素的值
* image.at<Vec3b>(v,u)[1]
* image.at<Vec3b>(v,u)[2]
* 2、使用迭代器方式 (实际上就是一个指针指向了 cv::Mat矩阵元素)
* cv::MatIterator_<Vec3b>begin,end;
* for( begin = image.begin<Vec3b>(), end = image.end<Vec3b>() ; begin != end; )
* (*begin)[0] = ...
* (*begin)[1] = ...
* (*begin)[2] = ...
*
* 3、用指针的方式操作
* for(v)
* for(u)
* image.ptr<Vec3b>(v)[u][0] 表示第一个通道
* image.ptr<Vec3b>(v)[u][0] 表示第二通道
* .
* .
* .
* */
