接下来是运行视觉SLAM十四讲第二版中我进行代码修改的部分:
ch3
useGeometry文件夹的CMakeLists.txt:
将add_executable( eigenGeometry eigenGeometry.cpp …/examples/coordinateTransform.cpp)
更改为:
add_executable( eigenGeometry eigenGeometry.cpp)
ch4
CMakeLists.txt中添加如下代码,添加对c++11的支持:
SET(CMAKE_CXX_FLAGS "-std=c++0x")
或者
set(CMAKE_CXX_FLAGS "-std=c++11")
ch5
imageBasics文件夹:
CMakeLists.txt更改为如下内容:
project(imageBasics)
#加入对c++11的支持
set(CMAKE_CXX_FLAGS "-std=c++11")
#ubuntu内置了openCV2的版本,这里的语句是选择使用openCV3
#并加入openCV3的路径
set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)
#find_package(OpenCV REQUIRED)
#include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})
add_executable(undistortImage undistortImage.cpp)
cmake make之后,利用如下语句执行:
./imageBasics ubuntu.png
stereo:
CMakeLists.txt添加如下内容:
set(CMAKE_CXX_FLAGS “-std=c++11”)
set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)
rgbd:
CMakeLists.txt添加如下内容:
set(CMAKE_CXX_FLAGS “-std=c++11”)
et(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)
并将pose.txt和depth、color文件夹都放进build中去,这样运行joinMap即可,参考了:参考链接。
ch6
CMakeLists.txt添加如下内容:
set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)
ch7
如果报如下错误,则代表需要添加Opencv3的路径。
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(3.4.6) /home/fuyouao/opencv-3.4.6/opencv-3.4.6/modules/imgproc/src/color.cpp:182: error: (-215:Assertion failed) !_src.empty() in function 'cvtColor'
这本书的代码有关opencv的部分,都需要在每个CMakeLists.txt添加如下内容:
set(OpenCV_DIR /usr/local/opencv3/share/OpenCV)
find_package(OpenCV 3 REQUIRED)
include_directories( ${OpenCV_DIR} )
include_directories(/usr/local/opencv3/include)
运行程序示例(图片都放到build里去):
./orb_cv 1.png 2.png
错误2:
error: (-215:Assertion failed) size.width>0 && size.height>0 in function 'imshow'
Aborted (core dumped)
表示虚拟机没链接摄像头,在虚拟机-可移动设备里把摄像头连接即可。
之后继续运行和安装还会再进行更新。
ch8 自己敲第一版project代码时遇到的问题
1、sophus相关的错误
第二版和第一版所用的sophus一个是模板类,一个是非模板类。我安装的是第二版的模板类,两个版本代码需要改动很多。
总之第一版的project里,头文件引入库的时候,se3.h和so3.h要修改为:se3.hpp和so3.hpp;using Sophus::SO3和using Sophus::SE3改为using Sophus::SO3d和using Sophus::SE3d,下面代码里有关的变量声明也要加上d; 但这次改了一圈也没解决问题,尤其是这个语句:
T_c_r_estimated_ = SE3(
SO3(rvec.at(0,0), rvec.at(1,0), rvec.at(2,0)),
Vector3d( tvec.at(0,0), tvec.at(1,0), tvec.at(2,0))
);
怎么改都不行,放弃这版本了。
如果是自己安装的sophus,要注意eigen和ceres的版本,可能需要更新,参考链接,又安了个fmt。
2、g2o相关的错误(参考链接,感谢)
报错是说g2o没有cmake_modules,或者是没有g20.cmake这样类似的问题。
因为我是自己照着书一点点敲代码,书上没有提g2o这里有个需要做的事,就是把git g2o时候的文件夹(或者是代码3rdparty的g2o文件夹),总之就是安装g2o的那个文件夹里的cmake_modules文件夹粘贴到项目根目录去(把报错说没找到的那几个cmake文件复制过来也可以)。
ch12 运行参考链接:
1、编译之前需要在dense_mono/CMakeFiles/dense_mapping.dir/dense_mapping.cpp文件,bool update 函数里面补充 上 return true(参考链接)。
2、error while loading shared libraries: libvtkRenderingCore-7.1.so.1: cannot open shared object file: No such file or directory:
(参考链接)
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
3、surfel_mapping.cpp:31:9: error:
‘class pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>’
has no member named ‘setPolynomialFit’;
did you mean ‘setPolynomialOrder’?:
#按提示修改
mls.setPolynomialFit(polynomial_order > 1);
改为
mls.setPolynomialOrder(polynomial_order > 1);
课后习题部分
第三章 三微空间刚体运动
本章CMakeLists.txt皆是如下代码:
cmake_minimum_required(VERSION 2.8)
project(useEigen)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3")
# 添加Eigen头文件
include_directories("/usr/include/eigen3")
add_executable(eigenMatrix eigenMatrix.cpp)
1、提取矩阵左上角3*3大小的块,并赋值为单位矩阵。
重点:MatrixXd中的block函数可直接提取块,block<a,b>(i,j)代表从矩阵的i行j列开始提取一个(a,b)大小的矩阵。索引i和j从0开始,即左上角第一个元素是(0,0)。
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>
using namespace Eigen;
#define MATRIX_SIZE 5 //这里我设成5*5的矩阵
int main(int argc, char **argv) {
//随机初始化,生成5*5大小的矩阵
Eigen::MatrixXd matrix_f = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
//MatrixXd中的block函数可直接提取块,block<a,b>(i,j)代表从矩阵的i行j列开始提取一个(a,b)大小的矩阵。索引i和j从0开始,即左上角第一个元素是(0,0)。
Eigen::MatrixXd matrix_part = matrix_f.block<3,3>(0,0);
cout << "yuan:"<< matrix_f << endl;
cout << "bufen:"<< matrix_part << endl;
matrix_part = Matrix<double,3,3>::Identity();
cout << matrix_part << endl;
return 0;
}
2、小萝卜二号坐标系下的坐标
重点:本题 q 和 t 表达的是Tcw,即从世界坐标系到相机坐标系的变换关系。
题目可理解为:世界坐标系下有一点P,它对应小萝卜一号相机坐标系下的点P1,也对应着小萝卜二号相机坐标系下的点P2。由给出的q1、t1、q2、t2,可以得到欧式变换矩阵T1与T2。
有如下两个等式,等式中P代表同一个点:
P
1
=
T
1
∗
P
P1=T1*P
P1=T1∗P
P
2
=
T
2
∗
P
P2=T2*P
P2=T2∗P
即世界坐标系下的点P通过旋转平移变换在两个相机坐标系下得到了P1和P2。
两个等式可以解出P2:
P
2
=
T
2
∗
T
1
−
1
∗
P
1
P2=T2*T1^{-1}*P1
P2=T2∗T1−1∗P1
代码即是以上思路的实现:
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>
using namespace Eigen;
int main(int argc, char **argv) {
Eigen::Quaterniond q1(0.55,0.3,0.2,0.2),q2(-0.1,0.3,-0.7,0.2);
cout<<q1.coeffs()<<endl;
q1.normalize();
cout<<q1.coeffs()<<endl;
q2.normalize();
Eigen::Vector3d t1(0.7,1.1,0.2),t2(-0.1,0.4,0.8),p1(0.5,-0.1,0.2);
Eigen::Isometry3d T1(q1);
//Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
//T1.rotate(q1.toRotationMatrix());//四元数转换成旋转矩阵
T1.pretranslate(t1);
Eigen::Isometry3d T2=Eigen::Isometry3d::Identity();
T2.rotate(q2.toRotationMatrix());
T2.pretranslate(t2);
Eigen::Vector3d p2=T2*T1.inverse()*p1;
cout<<p2.transpose()<<endl;
return 0;
}
可以看到,欧式变换矩阵的赋值有两种写法。
一是先初始化成单位阵,再单独设置旋转矩阵(这里要先把四元数转换成旋转矩阵,用toRotationMatrix()):
Eigen::Isometry3d T1=Eigen::Isometry3d::Identity();
T1.rotate(q1.toRotationMatrix());
二是直接在初始化时把值传进去,应该是会直接进行四元数和旋转矩阵的转换(网上没搜到,这两种结果一样,故猜测):
Eigen::Isometry3d T1(q1);
3、
A
X
=
b
AX=b
AX=b 的不同求解方法
高博士给出了三种例子,直接求、QR分解和cholesky分解。
#include <iostream>
using namespace std;
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>
using namespace Eigen;
#define MATRIX_SIZE 50
int main(int argc, char **argv) {
// 解方程
// 我们求解 matrix_NN * x = v_Nd 这个方程
// N的大小在前边的宏里定义,它由随机数生成
// 直接求逆自然是最直接的,但是求逆运算量大
Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN
= MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
matrix_NN = matrix_NN * matrix_NN.transpose(); // 保证半正定
Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);
clock_t time_stt = clock(); // 计时
// 直接求逆
Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
cout << "time of normal inverse is "
<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;
// 通常用矩阵分解来求,例如QR分解,速度会快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout << "time of Qr decomposition is "
<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;
// 对于正定矩阵,还可以用cholesky分解来解方程
time_stt = clock();
x = matrix_NN.ldlt().solve(v_Nd);
cout << "time of ldlt decomposition is "
<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;
return 0;
}
第四章 李群与李代数
1、两个轨迹,并求RMSE (注释版本)
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
using namespace Sophus;
using namespace std;
string groundtruth_file = "./groundtruth.txt";
string estimated_file = "./estimated.txt";
//为某种数据类型定义新的名字
//eigen自己定义的内存分配器,即aligned_allocator。
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti);
TrajectoryType ReadTrajectory(const string &path);//读取文本的函数,const引用参数在函数内不可改变
int main(int argc, char **argv) {
TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
TrajectoryType estimated = ReadTrajectory(estimated_file);
assert(!groundtruth.empty() && !estimated.empty());
assert(groundtruth.size() == estimated.size());
// compute rmse
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;
DrawTrajectory(groundtruth, estimated);
return 0;
}
TrajectoryType ReadTrajectory(const string &path) {
ifstream fin(path);
TrajectoryType trajectory;
if (!fin) {
cerr << "trajectory " << path << " not found." << endl;
return trajectory;
}
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(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz)); //q,t
trajectory.push_back(p1);//在结尾插入元素
}
return trajectory;
}
void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti) {
// create pangolin window and plot the trajectory
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);//总是显示颜色深度较小的那个颜色,深度
glEnable(GL_BLEND);//将重叠的图形融合在一起
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//OpenGL会把源颜色和目标颜色各自取出,并乘以一个系数(源颜色乘以的系数称为“源因子”,目标颜色乘以的系数称为“目标因子”)
//然后相加,这样就得到了新的颜色。
//GL_SRC_ALPHA:表示使用源颜色的alpha值来作为因子。
//GL_DST_ALPHA:表示使用目标颜色的alpha值来作为因子。
//GL_ONE_MINUS_SRC_ALPHA:表示用1.0减去源颜色的alpha值来作为因子。
//GL_ONE_MINUS_DST_ALPHA:表示用1.0减去目标颜色的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);
glLineWidth(2);
for (size_t i = 0; i < gt.size() - 1; i++) {
glColor3f(0.0f, 0.0f, 1.0f); // blue for ground truth
glBegin(GL_LINES);
auto p1 = gt[i], p2 = gt[i + 1];
//p1.translation() SE3d p1中的平移向量,平移代表位置
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);//指定坐标(qx, qy, qz)
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
for (size_t i = 0; i < esti.size() - 1; i++) {
glColor3f(1.0f, 0.0f, 0.0f); // red for estimated
glBegin(GL_LINES);
auto p1 = esti[i], p2 = esti[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
第五章 相机与图像
1、图像去畸变
//#include <math.h>``
double x=(u-cx)/fx,y=(v-cy)/fy;//像素点对应的相机平面坐标
double r=sqrt(pow(x,2)+pow(y,2));//坐标点到原点距离
u_distorted=x*(1+k1*pow(r,2)+k2*pow(r,4))+2*p1*x*y+p2*(pow(r,2)+2*pow(x,2));
v_distorted=y*(1+k1*pow(r,2)+k2*pow(r,4))+2*p2*x*y+p1*(pow(r,2)+2*pow(y,2));
u_distorted=fx*u_distorted+cx;
v_distorted=fy*v_distorted+cy;
2、双目摄像机
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * b / (disparity.at<float>(v, u));//disparity求视差
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;
pointcloud.push_back(point);
第六章 非线性优化
数据结构的知识都忘光了:
// 代价函数的计算模型
struct CURVE_FITTING_COST {
//构造函数,最后这里没有分号
//构造函数数组初始化时调用
CURVE_FITTING_COST(double x, double y) :_x(x), _y(y) {}
// 残差的计算
template<typename T>
bool operator()(
const T *const abc, // 模型参数,有3维
T *residual) const { //这个函数不能访问类中所有this所能调用的内存,即这是个只读函数
residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y数据
};
这章非常详细的参考链接
第七章 特征点视觉里程计
课后习题
5、solvePnP里有三种解法:P3P, EPnP,迭代法(默认);opencv2里参数分别为CV_P3P,CV_EPNP,CV_ITERATIVE (opencv3里多了DLS和UPnP解法)。参考链接
主要修改:
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false);