cmake_minimum_required(VERSION 3.10)
project(UndistortImg)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
# 寻找OpenCV库
find_package(OpenCV REQUIRED)
# 添加头文件
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(UndistortImg undistort_image.cpp)
target_link_libraries(UndistortImg ${OpenCV_LIBS})
#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;
// TODO 按照公式,计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)
// start your code here
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;
u_distorted = fx * x_distorted + cx;
v_distorted = fy * y_distorted + cy;
// end your code here
// 赋值 (最近邻插值)
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();
cv::imwrite("undistorted.jpg", image_undistort);
return 0;
}
运行结果:
cmake_minimum_required(VERSION 3.10)
project(Disparity)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
find_package(OpenCV REQUIRED)
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3" ${OpenCV_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
add_executable(Disparity disparity.cpp)
target_link_libraries(Disparity ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
#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::imshow("left", left);
// cv::waitKey(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,第四维为颜色
// start your code here (~6 lines)
// 根据双目模型计算 point 的位置
double x = (u - cx) / fx;
double y = (v - cy) / fy;
double depth = fx * d / (disparity.at<char>(v, u));
point[0] = x * depth;
point[1] = y * depth;
point[2] = depth;
pointcloud.push_back(point);
// end your code here
}
// 画出点云
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;
}
运行结果:
4.1
d
(
A
x
)
d
x
T
=
∂
(
A
x
)
∂
x
T
d
x
d
x
T
=
∂
(
A
x
)
∂
x
T
⋅
I
=
A
\frac {d(\mathbf A \mathbf x)}{d\mathbf x^T} =\frac{\frac {\partial(\mathbf A \mathbf x)}{\partial \mathbf x^T}d\mathbf x}{d\mathbf x^T} = \frac {\partial(\mathbf A \mathbf x)}{\partial \mathbf x^T} \cdot \mathbf I = \mathbf A
dxTd(Ax)=dxT∂xT∂(Ax)dx=∂xT∂(Ax)⋅I=A
d
(
A
x
)
d
x
=
A
T
\frac {d(\mathbf A \mathbf x)}{d\mathbf x} = \mathbf A^T
dxd(Ax)=AT
4.2
d
x
T
A
x
d
x
T
=
d
x
T
A
x
+
x
T
A
d
x
d
x
T
=
x
T
A
T
d
x
+
x
T
A
d
x
d
x
T
=
x
T
(
A
+
A
T
)
\frac{d \mathbf{x}^T\mathbf{A}\mathbf{x}}{d \mathbf{x}^T} = \frac{ d\mathbf x^{T}\mathbf A\mathbf x + \mathbf x^{T}\mathbf Ad\mathbf x}{d \mathbf{x}^T} = \frac{ \mathbf x^{T}\mathbf A^{T} d \mathbf x + \mathbf x^{T}\mathbf Ad\mathbf x}{d \mathbf{x}^T} =\mathbf{x}^T(\mathbf{A}+ \mathbf{A}^T)
dxTdxTAx=dxTdxTAx+xTAdx=dxTxTATdx+xTAdx=xT(A+AT)
d
x
T
A
x
d
x
=
d
x
T
A
x
+
x
T
A
d
x
d
x
=
d
x
T
A
x
+
d
x
T
A
T
x
d
x
=
(
A
+
A
T
)
x
\frac{d \mathbf{x}^T\mathbf{A}\mathbf{x}}{d \mathbf{x}}= \frac{ d\mathbf x^{T}\mathbf A\mathbf x + \mathbf x^{T}\mathbf Ad\mathbf x}{d \mathbf{x}} = \frac{ d\mathbf x^{T}\mathbf A\mathbf x + d\mathbf x^{T} \mathbf A^T \mathbf x}{d \mathbf{x}} = (\mathbf{A}+ \mathbf{A}^T)\mathbf{x}
dxdxTAx=dxdxTAx+xTAdx=dxdxTAx+dxTATx=(A+AT)x
4.3
t r ( A x x T ) = t r ( [ a 11 a 12 … a 1 n a 21 a 22 … a 2 n ⋮ ⋮ ⋮ a n 1 a n 2 … a n n ] ⋅ [ x 1 x 2 ⋮ x n ] ⋅ [ x 1 , x 2 , … x n ] ) tr(\mathbf A \mathbf x\mathbf x^T)=tr\left(\begin{bmatrix} a_{11} & a_{12} & \dots & a_{1n} \\ a_{21} & a_{22} & \dots & a_{2n} \\ \vdots & \vdots & & \vdots \\ a_{n1} & a_{n2} & \dots & a_{nn} \\ \end{bmatrix} \cdot \begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_n \end{bmatrix} \cdot[x_1,\ x_2,\ \dots x_n] \right) tr(AxxT)=tr⎝⎜⎜⎜⎛⎣⎢⎢⎢⎡a11a21⋮an1a12a22⋮an2………a1na2n⋮ann⎦⎥⎥⎥⎤⋅⎣⎢⎢⎢⎡x1x2⋮xn⎦⎥⎥⎥⎤⋅[x1, x2, …xn]⎠⎟⎟⎟⎞
= t r ( [ a 11 a 12 … a 1 n a 21 a 22 … a 2 n ⋮ ⋮ ⋮ a n 1 a n 2 … a n n ] ⋅ [ x 1 x 1 x 1 x 2 … x 1 x n x 2 x 1 x 2 x 2 … x n x n ⋮ ⋮ ⋮ x n x 1 x n x 2 … x n x n ] ) =tr\left(\begin{bmatrix} a_{11} & a_{12} & \dots & a_{1n} \\ a_{21} & a_{22} & \dots & a_{2n} \\ \vdots & \vdots & & \vdots \\ a_{n1} & a_{n2} & \dots & a_{nn} \\ \end{bmatrix}\cdot \begin{bmatrix} x_1x_1 & x_1x_2 & \dots & x_1x_n \\ x_2x_1 & x_2x_2 & \dots & x_nx_n \\ \vdots & \vdots & & \vdots \\ x_nx_1 & x_nx_2 & \dots & x_nx_n \\ \end{bmatrix}\right) =tr⎝⎜⎜⎜⎛⎣⎢⎢⎢⎡a11a21⋮an1a12a22⋮an2………a1na2n⋮ann⎦⎥⎥⎥⎤⋅⎣⎢⎢⎢⎡x1x1x2x1⋮xnx1x1x2x2x2⋮xnx2………x1xnxnxn⋮xnxn⎦⎥⎥⎥⎤⎠⎟⎟⎟⎞
= ∑ i = 1 n a 1 i x i x 1 + ∑ i = 1 n a 2 i x i x 2 + ⋯ + ∑ i = 1 n a n i x i x n =\sum_{i=1}^n a_{1i}x_ix_1+\sum_{i=1}^n a_{2i}x_ix_2+\cdots +\sum_{i=1}^n a_{ni}x_ix_n =i=1∑na1ixix1+i=1∑na2ixix2+⋯+i=1∑nanixixn
= x 1 ∑ i = 1 n a 1 i x i + x 2 ∑ i = 1 n a 2 i x i + ⋯ + x n ∑ i = 1 n a n i x i =x_1\sum_{i=1}^n a_{1i}x_i+x_2\sum_{i=1}^n a_{2i}x_i+\cdots +x_n\sum_{i=1}^n a_{ni}x_i =x1i=1∑na1ixi+x2i=1∑na2ixi+⋯+xni=1∑nanixi
x T A x = [ x 1 , x 2 , … x n ] ⋅ [ a 11 a 12 … a 1 n a 21 a 22 … a 2 n ⋮ ⋮ ⋮ a n 1 a n 2 … a n n ] ⋅ [ x 1 x 2 ⋮ x n ] = [ x 1 , x 2 , … x n ] ⋅ [ ∑ i = 1 n a 1 i x i ∑ i = 1 n a 2 i x i ⋮ ∑ i = 1 n a n i x i ] = x 1 ∑ i = 1 n a 1 i x i + x 2 ∑ i = 1 n a 2 i x i + ⋯ + x n ∑ i = 1 n a n i x i \mathbf x^T\mathbf A\mathbf x= [x_1,\ x_2,\ \dots x_n] \cdot \begin{bmatrix} a_{11} & a_{12} & \dots & a_{1n} \\ a_{21} & a_{22} & \dots & a_{2n} \\ \vdots & \vdots & & \vdots \\ a_{n1} & a_{n2} & \dots & a_{nn} \\ \end{bmatrix} \cdot \begin{bmatrix} x_1 \\ x_2 \\ \vdots \\ x_n \end{bmatrix} =\\ [x_1,\ x_2,\ \dots x_n] \cdot \begin{bmatrix} \sum_{i=1}^n a_{1i}x_i \\ \sum_{i=1}^n a_{2i}x_i \\ \vdots \\ \sum_{i=1}^n a_{ni}x_i \end{bmatrix}=x_1\sum_{i=1}^n a_{1i}x_i+x_2\sum_{i=1}^n a_{2i}x_i+\cdots +x_n\sum_{i=1}^n a_{ni}x_i xTAx=[x1, x2, …xn]⋅⎣⎢⎢⎢⎡a11a21⋮an1a12a22⋮an2………a1na2n⋮ann⎦⎥⎥⎥⎤⋅⎣⎢⎢⎢⎡x1x2⋮xn⎦⎥⎥⎥⎤=[x1, x2, …xn]⋅⎣⎢⎢⎢⎡∑i=1na1ixi∑i=1na2ixi⋮∑i=1nanixi⎦⎥⎥⎥⎤=x1i=1∑na1ixi+x2i=1∑na2ixi+⋯+xni=1∑nanixi
所以得到 x T A x = t r ( A x x T ) \mathbf x^T\mathbf A\mathbf x=tr(\mathbf A\mathbf x\mathbf x^T) xTAx=tr(AxxT)
cmake_minimum_required(VERSION 3.10)
project(GaussNewton)
set(CMAKE_BUILD_TYPE "Release")#Release
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
find_package(OpenCV REQUIRED)
include_directories("/usr/include/eigen3" ${OpenCV_INCLUDE_DIRS})
add_executable(GaussNewton gaussnewton.cpp)
target_link_libraries(GaussNewton ${OpenCV_LIBS} )
#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随机数产生器
cout << "(x_data,y_data):" ;
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));
cout << "(" << x_data[i] << "," << y_data[i] << ") " ;
}
cout << endl ;
// 开始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个数据点
// start your code here
double error = 0; // 第i个数据点的计算误差
error = yi - exp(ae * xi * xi + be * xi + ce); // 填写计算error的表达式
Vector3d J; // 雅可比矩阵
J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce); // de/da
J[1] = -xi * exp(ae * xi * xi + be * xi + ce); // de/db
J[2] = -exp(ae * xi * xi + be * xi + ce); // de/dc
H += J * J.transpose(); // GN近似的H
b += -error * J;
// end your code here
cost += error * error;
}
// 求解线性方程 Hx=b,建议用ldlt
// start your code here
Vector3d dx = H.ldlt().solve(b);
// end your code here
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;
}
运行结果:
6.1
H = [ 1 − 1 0 0 0 1 − 1 0 0 0 1 − 1 0 1 0 0 0 0 1 0 0 0 0 1 ] \bm{H} = \left[ {\begin{array}{cccc} 1&{ - 1}&0&0\\ 0&1&{ - 1}&0\\ 0&0&1&{ - 1}\\ \hline 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1 \end{array}} \right] H=⎣⎢⎢⎢⎢⎢⎢⎡100000−1101000−1101000−1001⎦⎥⎥⎥⎥⎥⎥⎤
6.2
W = d i a g ( Q 1 , Q 2 , Q 3 , R 1 , R 2 , R 3 ) \bm{W}=\mathrm{diag}(Q_1, Q_2, Q_3, R_1, R_2, R_3) W=diag(Q1,Q2,Q3,R1,R2,R3)
6.3
有唯一解:
x
m
a
p
∗
=
(
H
T
W
−
1
H
)
−
1
H
T
W
−
1
z
\bm{x}^*_{\mathrm{map}} = (\bm{H}^\mathrm{T} \boldsymbol{W}^{-1} \bm{H})^{-1} \bm{H}^\mathrm{T} \boldsymbol{W}^{-1} \bm{z}
xmap∗=(HTW−1H)−1HTW−1z