【SLAM】视觉SLAM十四讲（四：相机模型与非线性优化）
图像去畸变
对一张桶形失真的图片进行去畸变
畸变模型有两种：
- 径向畸变：由镜头形状导致
- 切向畸变：透镜与成像平面未严格平行
去畸变代码
int main(int argc, char **argv) {
// 畸变参数
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); // 去畸变以后的图
double x,y,r,xd,yd;
// 计算去畸变后图像的内容
for (int v = 0; v < rows; v++)
for (int u = 0; u < cols; u++) {
double u_distorted = 0, v_distorted = 0;
//利用内参将像素点转化为实际点
x = (u-cx)/fx;
y = (v-cy)/fy;
//计算出该实际点距离原点半径
r = sqrt(pow(x,2)+pow(x,2));
//根据畸变变换公式求原始图像的坐标
xd = x*(1 + k1*pow(r,2) + k2*pow(r,4)) + 2*p1*x*y + p2*(pow(r,2) + 2*pow(x,2));
yd = 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*xd + cx;
v_distorted = fy*yd + cy;
// 赋值 (最近邻插值)
if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
image_undistort.at(v, u) = image.at((int) v_distorted, (int) u_distorted);
} else {
image_undistort.at(v, u) = 0;
}
}
// 画图去畸变后图像
cv::imshow("image undistorted", image_undistort);
cv::waitKey();
return 0;
}
双目视差的使用
假设双目计算的视差已给定，根据双目模型，画出点云。
使用双目模型画出点云的代码（视觉SLAM十四讲P101）
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> pointcloud;
for (int v = 0; v < left.rows; v++)
for (int u = 0; u < left.cols; u++) {
Vector4d point(0, 0, 0, left.at(v, u) / 255.0); // 前三维为xyz,第四维为颜色
// 根据双目模型计算 point 的位置
unsigned int depth = disparity.ptr(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 = [ "< 
矩阵微分
参考https://blog.csdn.net/pipisorry/article/details/68961388
x为N阶列向量，A为N*N阶矩阵
1.d(Ax)/dx是什么？A的转置
2.d(x’Ax)/dx是什么？(A‘+A)x
3.证明xA’x=tr(Axx’)
暂时不会
高斯牛顿法的曲线拟合实验
思想：
对于不方便直接求解的最小二乘问题，用迭代的方法更新增量，使目标函数下降。确定增量的常用方法有两种，泰勒展开不同的阶数
- 一阶梯度（最速下降法）：增量= - J’
- 二阶梯度（牛顿法）：增量= -J’/H
J是雅克比矩阵，H是海塞矩阵
高斯牛顿法在泰勒展开一阶后，将展开式的最小二乘对增量求导，令增量的导数为0后，得到J’J近似H。
- 优点：免去计算复杂的H
- 缺点：H是可逆且正定的，J’J是半正定的，求出的增量可能不稳定，导致算法无法收敛
gaossnewton.cpp参考https://github.com/YCJin9/sparse_BA/blob/master/GN.cpp
#include
#include
#include
using namespace std;
using namespace Eigen;
int main ( int argc, char** argv )
{
double a=1.0, b=2.0, c=1.0; // 真实参数值
int N=100; // 数据点
double w_sigma=1.0; // 噪声Sigma值
cv::RNG rng; // OpenCV随机数产生器
double ae=2.0, be=-1.0, ce=5.0; // abc参数的估计值
vector x_data, y_data; // 数据
cout<<"generating data…… "< 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;
}
