SLAM十四讲第4章编程题

文件存放

.cpp/CMakeLists.txt/png/build都放在主文件夹里，所以执行文件和cmake ../make都在build里

---

2.图像去畸变

undistort_image.cpp

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

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);   // 图像是灰度图，flags=0
    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;
            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 = 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("distorted", image);
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( image_undistort )
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加OpenCV头文件
find_package(OpenCV 3.0 REQUIRED)
include_directories(${OpenCV_INCLDUE_DIRES})

add_executable( undistort undistort_image.cpp )
target_link_libraries(undistort ${OpenCV_LIBS})

---

3.鱼眼模型与去畸变

 fisheye.cpp

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

// 文件路径，如果不对，请调整
std::string input_file = "../fisheye.jpg";

int main(int argc, char **argv) {
  // 本程序实现鱼眼的等距投影去畸变模型
  // 畸变参数（本例设为零）
  double k1 = 0, k2 = 0, k3 = 0, k4 = 0;

  // 内参
  double fx = 689.21, fy = 690.48, cx = 1295.56, cy = 942.17;

  //这里没有0
  cv::Mat image = cv::imread(input_file);
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC3); // 去畸变以后的图

  // 计算去畸变后图像的内容
  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 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,y_distorted = theta_d*y/r;
      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<cv::Vec3b>(v, u) =
            image.at<cv::Vec3b>((int)v_distorted, (int)u_distorted);
      } else {
        image_undistort.at<cv::Vec3b>(v, u) = 0;
      }
    }

  // 画图去畸变后图像
  cv::imshow("image undistorted", image_undistort);
  cv::imwrite("fisheye_undist.jpg", image_undistort);
  cv::waitKey();

  return 0;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( fisheye )
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加OpenCV头文件
find_package(OpenCV 3.0 REQUIRED)
include_directories(${OpenCV_INCLDUE_DIRES})

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

---

4.双目视差的使用

 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;
    // 注意！间距是d，PPT上是b
    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,第四维为颜色

            // start your code here (~6 lines)
            // 根据双目模型计算 point 的位置
            double disparity_pix = disparity.at<uchar>(v,u); // 视差
            double X = (u-cx)*d/disparity_pix;
            double Y = (u-cy)*d/disparity_pix*fx/fy;
            double Z = d/disparity_pix*fx;
            point[0]=X;
            point[1]=Y;
            point[2]=Z;
            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;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( disparity )
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加Eigen/OpenCV头文件
include_directories( "/usr/include" )
include_directories( "/usr/include/eigen3" )
find_package(OpenCV 3.0 REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(${OpenCV_INCLDUE_DIRES} ${Pangolin_INCLDUE_DIRES})

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

---

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

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个数据点
            // start your code here
            double error = 0;   // 第i个数据点的计算误差
            error = exp(ae*xi*xi+be*xi+ce)-yi; // 填写计算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;
        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;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( gaussnewton )
set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加Eigen/OpenCV头文件
include_directories( "/usr/include" )
include_directories( "/usr/include/eigen3" )
find_package(OpenCV 3.0 REQUIRED)

include_directories(${OpenCV_INCLDUE_DIRES})

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