视觉SLAM理论与实践4
视觉SLAM理论与实践4-相机模型 非线性优化
-
- 一、单目相机模型
- 二、图像畸变
- 二、双目视差的使用
- 三、矩阵运算微分
- 四、高斯牛顿法拟合实验
- 五、批量最大似然估计
一、单目相机模型
1）针孔相机模型简介：
针孔相机模型如上图所示，三维路标点（landmark）P，通过相机光心投射到像素平面P’。
小孔成像模型
成像平面到像素坐标
综上所述真实的相机投影过程一般需要经历3次变换：世界——>相机——>归一化平面——>像素平面。
而在单目相机中同一直线的投影点仍是同一个，如下图所示，
除了内参，相机坐标系与世界坐标系还相差一个变换：
这里R,t或T称为外参（注：右侧式子隐含了一次非齐次到齐次的变换）
外参就是SLAM估计的目标
二、图像畸变
图像畸变是由于透镜制造精度以及组装工艺的偏差会引入畸变，导致原始图像失真。
常见的畸变分为径向畸变和切向畸变
1）其中径向畸变是图像像素点以畸变中心为中心点,沿着径向产生的位置偏差,从而导致图像中所成的像发生形变，常见的两种径向畸变类型如下:
用归一化坐标变换来定量描述如下：
2）切向畸变是由于摄像机制造上的缺陷使得透镜本身与图像平面不平行而产生的，示意图如下所示：
用归一化坐标变换来定量描述如下：
将上述两项畸变叠加起来得到：（注：实际当中可灵活保留各项系数，稍后的实践过程中便取r到4阶）
欧克，基础知识介绍完毕，下面来实践一哈！！！
环境ubuntu16.04，编程语言C++
1.现实生活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但是当我们使用广角和鱼眼镜头时，由于畸变的原因，直线在图像里看起来是扭曲的。尝试将如下图像去畸变，得到畸变前的图像。
显然上图红色标记部分出现明显畸变。根据以上基础知识的介绍，畸变前后的坐标变化为：（注：r取4阶）
其中x; y 为去畸变后的坐标，xdistorted; ydistroted 为去畸变前的坐标。现给定参数：
以及相机内参：
undistort_image.cpp:
#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
string image_file = "../test.png"; // 请确保路径正确
int main(int argc, char **argv) {
// 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
// 畸变参数
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;
cv::Mat image = cv::imread(image_file,0); // 图像是灰度图，CV_8UC1
int rows = image.rows, cols = image.cols;
cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸变以后的图
// 计算去畸变后图像的内容
for (int v = 0; v < rows; v++)
for (int u = 0; u < cols; u++) {
double u_distorted = 0, v_distorted = 0;
//利用内参将像素点转化为实际点
double x=(u-cx)/fx; double y=(v-cy)/fy;
//计算出该点实际点距离原点半径
double r=sqrt(pow(x,2)+pow(y,2));
//根据畸变公式求原始图像的坐标
double x_distorted=x*(1+k1*pow(r,2)+k2*pow(r,4))+2*p1*x*y+p2*(pow(r,2)+2*pow(x,2));
double y_distorted=y*(1+k1*pow(r,2)+k2*pow(r,4))+p1*(pow(r,2)+2*pow(y,2))+2*p2*x*y;
u_distorted=fx*x_distorted+cx; 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;
}
}
// 画图去畸变后图像
cv::imshow("image undistorted", image_undistort);
cv::waitKey();
return 0;
}
这里需要修改自己图片的路径，否则会报“核心已转储”的错误，由于本人的图片放在可执行文件的上级菜单中，所以我修改的路径如下：（返回到上级目录中）
string image_file = "../test.png";
CMakeLists.txt:
cmake_minimum_required(VERSION 2.8)
PROJECT(undistort_image)
IF(NOT CMAKE_BUILD_TYPE) #（可选）如果没有指定cmake编译模式，就选择Relealse模式，必须写成三行
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE("Build type: " ${
CMAKE_BUILD_TYPE}) #终端打印cmake编译模式的信息
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") #添加c标准支持库
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") #添加c++标准支持库
# Check C++11 or C++0x support #检查c++11或c++0x标准支持库
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
find_package(OpenCV 3.0 QUIET) #find_package(<Name>)命令首先会在模块路径中寻找 Find<name>.cmake
if(NOT OpenCV_FOUND)
find_package(OpenCV 2.4.3 QUIET)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
endif()
endif()
include_directories(${
OpenCV_INCLUDE_DIRS})
add_executable(image undistort_image.cpp)
#链接OpenCV库
target_link_libraries(image ${
OpenCV_LIBS})
去畸变后的图像如下：
显然去图像畸变部分得到了校正。
二、双目视差的使用
1）双目模型
2）双目视觉实践
相比于单目，双目相机的一大好处是可以通过左右目的视差来恢复深度。以上我们介绍了由视差计算深度的过程。现在根据视差计算深度，进而生成点云数据。实践开始！
这里我们假设双目计算的视差已经给定，请你根据双目模型，画出图像对应的点云，并显示到Pangolin 中。
给定的左右图见code/left.png 和code/right.png，视差图亦给定，见code/right.png。双目的参
数如下：
且双目左右间距（即基线）为：
disparity.cpp:
#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>
using namespace std;
using namespace Eigen;
// 文件路径，如果不对，请调整
string left_file = "../left.png";
string right_file = "../right.png";
string disparity_file = "../disparity.png";
// 在panglin中画图，已写好，无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);
int main(int argc, char **argv) {
// 内参
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
// 间距
double d = 0.573;
// 读取图像
cv::Mat left = cv::imread(left_file, 0);
cv::Mat right = cv::imread(right_file, 0);
cv::Mat disparity = cv::imread(disparity_file, 0); // disparty 为CV_8U,单位为像素
// 生成点云
vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
// TODO 根据双目模型计算点云
// 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < left.rows; v++)
for (int u = 0; u < left.cols; u++) {
Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
unsigned int depth = disparity.ptr<unsigned short>(v)[u]; //深度值
if(depth == 0)continue; //深度为0表示未测量到
point[2]=(fx*d*1000)/depth;
point[1]=(v-cy)*point[2]/fy;
point[0]=(u-cx)*point[2]/fx;
cout<<"point = [ "<<point[0]<<" "<<point[1]<<" "<<point[2]<<" "<<point[3]<<" ]"<<endl;
pointcloud.push_back(point);
}
// 画出点云
showPointCloud(pointcloud);
return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {
if (pointcloud.empty()) {
cerr << "Point cloud is empty!" << endl;
return;
}
pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_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);
glPointSize(2);
glBegin(GL_POINTS);
for (auto &p: pointcloud) {
glColor3f(p[3], p[3], p[3]);
glVertex3d(p[0], p[1], p[2]);
}
glEnd();
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
return;
}
此外还需注意图片文件的路径：
string left_file = "../left.png";
string right_file = "../right.png";
string disparity_file = "../disparity.png";
注：返回上级目录提取图片
CMakeLists.txt:
cmake_minimum_required(VERSION 2.8)
PROJECT(disparity)
IF(NOT CMAKE_BUILD_TYPE) #（可选）如果没有指定cmake编译模式，就选择Relealse模式，必须写成三行
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE("Build type: " ${
CMAKE_BUILD_TYPE}) #终端打印cmake编译模式的信息
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") #添加c标准支持库
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") #添加c++标准支持库
# Check C++11 or C++0x support #检查c++11或c++0x标准支持库
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
find_package(OpenCV 3.0 QUIET) #find_package(<Name>)命令首先会在模块路径中寻找 Find<name>.cmake
if(NOT OpenCV_FOUND)
find_package(OpenCV 2.4.3 QUIET)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
endif()
endif()
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3")
include_directories(
${
OpenCV_INCLUDE_DIRS}
${
Pangolin_INCLUDE_DIRS}
)
add_executable(disparity disparity.cpp)
#链接OpenCV库
target_link_libraries(disparity
${
OpenCV_LIBS}
${
Pangolin_LIBRARIES}
)
运行结果如下：
三、矩阵运算微分
1)矩阵 A ∈ R N ×N ,那么 d(Ax)/dx 是什么 ?
答：A的转置
2) 矩阵 A ∈ R N ×N ,那么 d(x T Ax)/dx 是什么?
答：(A+AT)x
3) 证明：
证明过程如下：
四、高斯牛顿法拟合实验
gaussnewton.cpp:
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
double ar = 1.0, br = 2.0, cr = 1.0; // 真实参数值
double ae = 2.0, be = -1.0, ce = 5.0; // 估计参数值
int N = 100; // 数据点
double w_sigma = 1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
vector<double> x_data, y_data; // 数据
for (int i = 0; i < N; i++) {
double x = i / 100.0;
x_data.push_back(x);
y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
}
// 开始Gauss-Newton迭代
int iterations = 100; // 迭代次数
double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost
for (int iter = 0; iter < iterations; iter++) {
Matrix3d H = Matrix3d::Zero(); // Hessian = J^T J in Gauss-Newton
Vector3d b = Vector3d::Zero(); // bias
cost = 0;
for (int i = 0; i < N; i++) {
double xi = x_data[i], yi = y_data[i]; // 第i个数据点
double error = 0; // 第i个数据点的计算误差
error = yi-exp(ae*xi*xi+be*xi+ce);
Vector3d J;
J[0] = -xi*xi*exp(ae*xi*xi+be*xi+ce);
J[1] = -xi*exp(ae*xi*xi+be*xi+ce);
J[2] = -exp(ae*xi*xi+be*xi+ce);
H += J * J.transpose(); // GN近似的H
b += -error * J;
cost += error * error;
}
// 求解线性方程 Hx=b，建议用ldlt
Vector3d dx;
//dx=H.inverse()*b; //直接求逆方法求解增量
dx = H.ldlt().solve(b); // ldlt方法
if (isnan(dx[0])) {
cout << "result is nan!" << endl;
break;
}
if (iter > 0 && cost > lastCost) {
// 误差增长了，说明近似的不够好
cout << "cost: " << cost << ", last cost: " << lastCost << endl;
break;
}
// 更新abc估计值
ae += dx[0];
be += dx[1];
ce += dx[2];
lastCost = cost;
cout << "total cost: " << cost << endl;
}
cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
return 0;
}
CMakeLists.txt:
cmake_minimum_required(VERSION 2.8)
PROJECT(gaussnewton)
IF(NOT CMAKE_BUILD_TYPE) #（可选）如果没有指定cmake编译模式，就选择Relealse模式，必须写成三行
SET(CMAKE_BUILD_TYPE Release)
ENDIF()
MESSAGE("Build type: " ${
CMAKE_BUILD_TYPE}) #终端打印cmake编译模式的信息
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ") #添加c标准支持库
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native") #添加c++标准支持库
# Check C++11 or C++0x support #检查c++11或c++0x标准支持库
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X)
message(STATUS "Using flag -std=c++0x.")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
find_package(OpenCV 3.0 QUIET) #find_package(<Name>)命令首先会在模块路径中寻找 Find<name>.cmake
if(NOT OpenCV_FOUND)
find_package(OpenCV 2.4.3 QUIET)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
endif()
endif()
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3")
include_directories(
${
OpenCV_INCLUDE_DIRS}
)
add_executable(gaussnewton gaussnewton.cpp)
#链接OpenCV库
target_link_libraries(gaussnewton
${
OpenCV_LIBS}
)
结果如下：
显然最后估计结果与题设参考值吻合。
五、批量最大似然估计
1.H矩阵的具体形式如下：
2. 待更新。。。
3.待更新。。。
友情提示：代码下载需要C币，请事先判断是否对您有帮助，谨慎下载哦！！！