《SLAM十四讲》第四讲讲后作业

1、图像去畸变

结果：
Code：
undistort_image.cpp

 			// start your code here 将图像像素坐标转换成相机坐标
            double x = (u -cx)/fx;
            double y = (v -cy)/fy;
            //半径r
            double r_2 = x*x+y*y;
            double x_distorted = x*(1+k1*r_2+k2*r_2*r_2);
            double y_distorted = y*(1+k1*r_2+k2*r_2*r_2);
            //将相机坐标转化为像素坐标
            u_distorted = x_distorted*fx+cx;
            v_distorted = y_distorted*fy+cy;
            // end your code here

CMakeLists.txt

cmake_minimum_required(VERSION 3.5.1)
project(undistort_image)

set(CMAKE_CXX_STANDARD 11)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(undistort_image undistort_image.cpp)

target_link_libraries(undistort_image ${OpenCV_LIBRARIES})

2、双目视差的使用

结果：
disparity.cpp

 //start code
   unsigned int d=disparity.at<uchar>(v,u);
   //视差图disparty中(v,u)点，带有视差信息d，其视差d用像素值来表达0-255之间
   if(d ==0)
    continue;  //视差d=0表示该点没有深度不用估计
   cout<<d<<endl;
  //基线长度b=0.573  Z=fb/d  f是内参--焦距，b是基线长度，d是视差
  double z=(f*b*1000)/d;

  point[2]=z;
   point[0]=(u-cx)*z/fx;
  point[1]=(v-cy)*z/fy;
  pointcloud.push_back(point);
       
  // end your code here

CMakeLIsts.txt

cmake_minimum_required(VERSION 3.5.1)
project(disparity)

set(CMAKE_CXX_STANDARD 11)

find_package(OpenCV REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(${Pangolin_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")

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

结果展示

3、矩阵微分

解答：
(1）
将AX对X的每一个分量求偏导，构成一个超向量。注意该向量的每一个元素都是一个矩阵。
此处：
矩阵Y对标量x求导：
相当于每个元素求导数后转置一下，注意M×N矩阵求导后变成N×M
Y = [y(ij)] --> dY/dx = [dy(ji)/dx]

（2）

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

接答：
gaussnewton.cpp

  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的表达式
            error=yi - (exp(ae * xi * xi + be * xi + ce));
            Vector3d J; // 雅可比矩阵
            //J[0] = 0;  // de/da
            //J[1] = 0;  // de/db
            //J[2] = 0;  // de/dc
            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);
            cout<<"j: "<<J<<endl;
            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;
        cout<<"H:"<< H<<endl;
        cout<<"B:"<< b<<endl;
        dx = H.ldlt().solve(b);
	// end your code here

结果：

estimated abc = 0.890912, 2.1719, 0.943629