深蓝学院-视觉SLAM理论与实践-第十二期-第4章作业

最新推荐文章于 2022-11-15 14:54:38 发布

小宋今天要早睡

最新推荐文章于 2022-11-15 14:54:38 发布

阅读量2.4k

点赞数

分类专栏： SLAM 文章标签： slam

本文链接：https://blog.csdn.net/weixin_40918019/article/details/123038678

版权

SLAM 专栏收录该内容

11 篇文章 10 订阅

订阅专栏

图像去畸变

程序思路

根据相机内参，将无畸变的像素坐标反向投影到归一化平面
通过畸变模型计算无畸变归一化坐标对应的含有畸变的归一化坐标
利用相机内参，将含有畸变的归一化坐标投影到相机坐标系中

源代码

//
// Created by 高翔 on 2017/12/15.
//

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "/home/sld/workspace/vslamLessonHomeWork/L4/code/img/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++)  //v=y u=x
        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;
            double 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 = x_distorted*fx + cx;
            v_distorted = y_distorted*fy+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::imwrite("./image undistorted.png", image_undistort);
    //cv::waitKey();

    return 0;
}

CMake

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(undistort_image undistort_image.cpp)
target_link_libraries(undistort_image ${OpenCV_LIBRARIES})

运行结果

去畸变前

去畸变后

鱼眼模型与去畸变

问题1：请说明鱼眼相机相比于普通针孔相机在 SLAM 方面的优势

相机拍摄的角度比较广，可以获取更加广阔的视野，观测到更多信息。

问题2：请整理并描述 OpenCV 中使用的鱼眼畸变模型（等距投影）是如何定义的，它与上题的畸变模型有何不同

等距投影模型

设相机坐标系下有一点p，它与相机光轴之间的夹角为 $\theta$ ，相机焦距为 $f$ ，在归一化坐标系中，该点到相机光心之间的距离为 $z_{鱼眼}$ ，那么等距投影如下图所示：

投影模型可以表示为： $z_{鱼眼} = f\theta$

OpenCV中的鱼眼模型

考虑到实际情况和理论模型的偏差，OpenCV中的畸变模型将投影模型中的 $\theta$ 替换成了一个关于 $\theta$ 的奇函数，并用泰勒展开进行拟合，即 $z_{OpenCV} = f\theta_d, \theta_d=k_0\theta+k_1\theta^3+k_2\theta^5+k_3\theta^7+k_4\theta^9$

完整的OpenCV鱼眼相机畸变模型如下：

其中

$X_c，Y_c，Z_c]$ 为相机坐标系下该点坐标
$x_c，y_c]$ 为不存在畸变的情况下归一化平面下的坐标
$x_d，y_d]$ 为存在畸变的情况下归一化平面下的坐标
$[u, v]$ 为鱼眼相机的真实像素坐标

等距投影模型与畸变模型的区别

畸变模型可以表示为： $z_{畸变} = r*(1+k_1r^2++k_2r^4)$ ，其中 $ftan(\theta)$

由于 $t a n$ 函数的特性，传统的畸变模型当 $\theta$ 接近90度时， $z_{畸变}$ 依然趋近于无穷，而鱼眼相机(等距投影模型)则趋近于 $f$ 。因此，等距投影模型可以获取角度更加广阔的视野。

问题3：完成 fisheye.cpp 文件中的内容。针对给定的图像，实现它的畸变校正。要求：通过手写方式实现，不允许调用 OpenCV 的 API

主要思路

将无畸变的像素坐标反向投影到归一化平面
利用问题1得到的畸变模型计算归一化平面上的畸变坐标
利用相机内参，将含有畸变的归一化坐标投影到像素坐标系上

核心源代码

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++)    // u = x, v = y
    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;
      double y = (v - cy)/fy;

      //引入畸变
      double r = sqrt(x*x + y*y);
      double theta = atan(r);
      double theta_d = theta + k1*pow(theta, 3) + k2*pow(theta, 5) + k3*pow(theta, 7) + k4*pow(theta, 9);
      double x_distorted = theta_d*x/r;
      double y_distorted = theta_d*y/r;
      
      //重新投影
      u_distorted = x_distorted*fx + cx;
      v_distorted = y_distorted*fy + cy;

      // end your code here
}

CMake

find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(${EIGEN3_INCLUDE_DIRS})
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(undistort_image undistort_image.cpp)
target_link_libraries(undistort_image ${OpenCV_LIBRARIES})

add_executable(gaussnewton gaussnewton.cpp)
target_link_libraries(gaussnewton ${OpenCV_LIBRARIES})

add_executable(fisheye fisheye.cpp)
target_link_libraries(fisheye ${OpenCV_LIBRARIES})

运行结果

去畸变前

去畸变后

问题4：为什么在这张图像中，我们令畸变参数 k1, . . . , k4 = 0，依然可以产生去畸变的效果？

由OpenCV中的畸变模型可以看出，当 $k_1,k_2,k_3,k_4=0$ 时，$\theta_d =\theta $此时，整个畸变模型就转化成了理想的等距投影模型，依然具有去除畸变的效果。

问题5：在鱼眼模型中，去畸变是否带来了图像内容的损失？如何避免这种图像内容上的损失呢？

鱼眼图一般为圆形，边缘的信息被压缩的很密，经过去除畸变后原图中间的部分会被保留的很好，而边缘位置一般都会被拉伸的很严重、视觉效果差，所以通常会进行切除，因此肯定会带来图像内容的损失。增大去畸变时图像的尺寸，或者使用单目相机和鱼眼相机图像进行融合，补全丢失的信息。

双目视差的使用

问题1：推导双目相机模型下，视差与 $X Y Z$ 坐标的关系式。请给出由像素坐标加视差 $u, v, d$ 推导 $X Y Z$ 与已知 $X Y Z$ 推导 $u, v, d$ 两个关系

假设两个相机的光轴平行，且视差只在相机的x轴产生。由目标点P，两个相机的光心以及归一画平面可组成如下所示的图像：

已知 $u 、 v 、 d$ 推导 $X Y Z$

上图中， $z_1 z_2$ 分别代表两个相机的光轴。 $x_L、x_R$ 代表目标点在两个相机归一化平面中的x轴坐标。

设点P在两个相机图像中的像素坐标分别为 $u_L,v_L]、[u_R,v_R]$ ，归一化坐标为 $x_L,y_L]、[x_R,y_R]$ ，相机坐标系下的坐标为 $X_L,Y_L,Z_L]、[X_R,Y_R,Z_R]$ ，基线长度为b

由题目条件可知，

$u_L = u， v_L = v，u_R = u_L - d，v_R = v$

转化到归一化平面为

$x_L = (u-c_x)/f_x，y_L = (v-c_y)/f_y，x_R = (u_L - d-c_x)/f_x，y_R=(v-c_y)/f_y$

由上图中的相似三角形可知，

$Z_L/(Z_L - 1) = b/(b-(x_L-x_R))$

化简后可得：

$Z_L = bf_x/d$

因此

$X_L = x_L*Z_L = ((u-c_x)/f_x)bf_x/d=(u-c_x)b/d$

$Y_L = y_L*Z_L = ((v-c_y)/f_y)bf_x/d=(v-c_y)bf_x/(df_y)$

$Z_L = bf_x/d$

已知 $X Y Z$ 推导 $u 、 v 、 d$

由上图中的相似三角形可知，

$Z_L/(Z_L - 1) = b/(b-(x_L-x_R))$

化简后可得

$d = bf_x/Z_L$

由正向投影可知

$u = u_L = f_x(X_L/Z_L)+c_x$

$v = v_L = f_y(Y_L/Z_L)+c_y$

$d = bf_x/Z_L$

问题2：推导在右目相机下该模型将发生什么改变。

当在右目相机下时，即由 $u_R = u，v_R = v$ ，

因此， $u_L = u+d， v_L = v$

转化到归一化平面为

$x_L = (u+d-c_x)/f_x，y_L = (v-c_y)/f_y，x_R = (u-c_x)/f_x，y_R=(v-c_y)/f_y$

此时 $Z_R/(Z_R - 1) = b/(b-(x_L-x_R))$ 依然成立。

化简可得：

$Z_R = -bf_x/d$

由结果可以看出，当以右相机为模型进行推导时，可以当作基线时负数的左相机进行计算。

问题3：深度计算与点云显示

整体思路

根据问题1推导的 $u 、 v 、 d$ 和 $X 、 Y 、 Z$ 的关系，计算点云的深度

核心源代码

    // TODO 根据双目模型计算点云
    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v+=2)
        for (int u = 0; u < left.cols; u+=2) {

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // start your code here (~6 lines)
            double disparity_pix = disparity.at<uchar>(v, u);
            double X = (u-cx)*d/disparity_pix;
            double Y = (v-cy)*d/disparity_pix*fx/fy;
            double Z = d*fx/disparity_pix;
            point[0] = X;
            point[1] = Y;
            point[2] = Z;
            pointcloud.push_back(point);
            // end your code here

CMake

find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(${Pangolin_INCLUDE_DIRS})
include_directories(${EIGEN3_INCLUDE_DIRS})
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(undistort_image undistort_image.cpp)
target_link_libraries(undistort_image ${OpenCV_LIBRARIES})

add_executable(gaussnewton gaussnewton.cpp)
target_link_libraries(gaussnewton ${OpenCV_LIBRARIES})

add_executable(fisheye fisheye.cpp)
target_link_libraries(fisheye ${OpenCV_LIBRARIES})

add_executable(disparity disparity.cpp)
target_link_libraries(disparity ${OpenCV_LIBRARIES})
target_link_libraries(disparity ${Pangolin_LIBRARIES})

运行结果

矩阵运算微分

问题1：矩阵 $A ∈ R^{N×N}$ ，那么 $d (A x) / d x$ 是什么1？

$d (A x) / d x = A$

问题2：矩阵 $A ∈ R^{N×N}$ ，那么 $d(x^TAx)/dx$ 是什么？

$d(x^TAx)/dx = Ax + A^Tx$

问题3：证明 $x^TAx = tr(Axx^T)$

高斯牛顿法的曲线拟合实验

整体思路

误差项: $exp(ax^2+bx+c) - y$
参数a偏导: $exp(ax^2+bx+c)x^2$
参数b偏导: $exp(ax^2+bx+c)x$
参数c偏导: $exp(ax^2+bx+c)$
方程求解部分，由于H矩阵为正定矩阵，因此可以用cholesky分解的方法求解，提高求解速度

核心源代码

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 = 0; // 填写计算error的表达式
    Vector3d J; // 雅可比矩阵
    J[0] = 0;  // de/da
    J[1] = 0;  // de/db
    J[2] = 0;  // de/dc

    //计算误差
    double yi_est = exp(ae*xi*xi + be*xi + ce);
    error = yi_est - yi;

    //计算雅可比
    J[0] = yi_est*xi*xi;
    J[1] = yi_est*xi;
    J[2] = yi_est;

    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;
dx = H.ldlt().solve(b);
// end your code here

CMake

find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(${EIGEN3_INCLUDE_DIRS})
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(undistort_image undistort_image.cpp)
target_link_libraries(undistort_image ${OpenCV_LIBRARIES})

add_executable(gaussnewton gaussnewton.cpp)
target_link_libraries(gaussnewton ${OpenCV_LIBRARIES})

运行结果

批量最大似然估计

问题1：可以定义矩阵 $H$ ，使得批量误差为 $e = z - H x$ 。请给出此处 $H$ 的具体形式

问题2：信息矩阵W的具体数值

由高斯概率密度函数可得，W如下

问题3：该MAP问题是否存在唯一解

将上述式子展开可得

$z − Hx)^TW^{−1} (z − Hx) = z^TW^{-1}z-x^TH^TW^{-1}Hx+2z^TW^{-1}Hx$

上述式子如果取得极数值，则在极值点出， $x$ 导数必为0，因此有

$2x^TH^TW^{-1}H-2z^TW^{-1}H=0$

化简可得

$x = (H^TW^{-1}H)^{-1}H^TW^{-1}z$

因此，当 $H^TW^{-1}H$ 可逆时， $x$ 存在唯一解，且 $x = (H^TW^{-1}H)^{-1}H^TW^{-1}z$

对于 $H^TW^{-1}H$ 是否可逆，可从机器人能观性角度进行判定， $r a n k (C) = r a n k (1) = 1$ 。因此，系统可观，故 $H^TW^{-1}H$ 可逆。

参考链接

小宋今天要早睡

关注

0
点赞
踩
9

收藏

觉得还不错? 一键收藏
0
评论
深蓝学院-视觉SLAM理论与实践-第十二期-第4章作业

图像去畸变程序思路根据相机内参，将无畸变的像素坐标反向投影到归一化平面通过畸变模型计算无畸变归一化坐标对应的含有畸变的归一化坐标利用相机内参，将含有畸变的归一化坐标投影到相机坐标系中源代码//// Created by 高翔 on 2017/12/15.//#include <opencv2/opencv.hpp>#include <string>using namespace std;string image_file = "/home/sld/w
复制链接

扫一扫