自动驾驶-机器人-slam-定位面经和面试知识系列05之常考公式推导（02）

这个博客系列会分为C++ STL-面经、常考公式推导和SLAM面经面试题等三个系列进行更新，基本涵盖了自己秋招历程被问过的面试内容（除了实习和学校项目相关的具体细节）。在知乎和牛客（牛客上某些文章上会附上内推码）也会同步更新，全网同号（lonely-stone或者lonely_stone）。
关于高频面试题和C++ STL面经，每次我会更新10个问题左右，每次更新过多，害怕大家可能看了就只记住其中几个点。（在个人秋招面试过程中，面试到后面，发现除了个人项目和实习经历外，个人所记录的内容基本能涵盖面试官能问到的）
（另外个人才疏学浅，如果所分享知识中出现错误，请大家指出，避免误导其他人）

1. 对极约束+H-E-F

如果求职岗位与定位、SLAM这些相关，那对极约束相关的这三个矩阵肯定是必会的，并且推导流程也是要烂熟于心才行。

以上是我个人习惯的推导示意，大家可以自己手动推一下，然后最后记下来，这样印象要深刻一些。

其中基础矩阵秩只有2，因此F的自由度为7。它自由度比本质矩阵多的原因是多了两个内参矩阵
下面是单应矩阵：p2 = H * p1

先说结论，基础矩阵F有7个自由度，本质矩阵E有5个自由度，单应性矩阵H有8个自由度。

• 本质矩阵Ｅ＝t^R：6-1=5，尺度等价性。怎么理解这里的尺度等价性？本质矩阵是由对极约束定义的。由于对极约束是等式为零的约束，所以对Ｅ乘以任意非零常数后，对极约束依然满足。我们把这件事情称为Ｅ在不同尺度下是等价的。此外，本质矩阵E的奇异值是[σ，σ，0]T的形式，这是本质矩阵的内在性质。
• 单应性矩阵Ｈ：9-1=8，单应矩阵Homography，通常我们说它都是说二维平面到二维平面的映射，其实Homography是n维射影空间之间最通常的变换的统称，这类变换的一个特征是在齐次坐标的基础上进行，因此它的维数是(n+1)^{2，它是最通常的，所以我们能想到自由度就是(n+1)}2，而齐次坐标存在尺度模糊，所以要在此基础上减掉1，放在2维空间的话就是(2+1)^2 - 1 = 8。
• 基础矩阵Ｆ：9-2=7，尺度等价性 + 秩为2（不满秩），
  

  其中p是像素点坐标。一般解释F是有7个自由度，是说它有个约束秩为2即行列式为0，所以在单应基础上再减1。

2. 多层优化模式的求导

求导这些也可以推一推写一写，看网上其实关于这些比较细节的还不是很多。下面这个推导过程自己可以多熟悉熟悉，具体的代码实现可以看看ORBSLAM3里面的无imu版本实现。

3. 坐标转换基础

接下来是在坐标系变换里面很重要也是很基础的部分，自己在刚开始实习或者做项目时，总是对坐标转换有些懵圈，有时候感觉可简单了，有时候又感觉不太对。后来就好好理了一下，发现如下图所示的（用向量的方式理解）方法会特别清楚。在后续关于坐标系和位姿的变换时都比较得心应手。（这些可能对有些人来说比较基础简单，之所以把这些也写了出来，是因为自己当初刚开始的时候会有这些麻烦，所以如果能对一两个人有用也可以）

4. 手写高斯牛顿迭代优化

如果说上面的两点只是有助于自己基础SLAM能力的话（可能面试中不会问的多），那关于手写高斯牛顿迭代优化的基本很多都会问了。

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <chrono>

using namespace std;
using namespace Eigen;
using namespace cv;

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值
    double inv_sigma = 1.0 / w_sigma; 
    cv::RNG rng;      // opencv随机数产生器

    // step1.产生数据
    vector<double> x_data, y_data;
    for(int i = 0; i < N; ++i){
        double x = i / 100.0;
        x_data.push_back(x);
        double y = exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma);
        y_data.push_back(y);
    }

    // step2.开始Gauss-Newton迭代
    int iteration = 100;
    double cost = 0, lastCost = 0;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(int iter = 0; iter < iteration; ++iter){
        Matrix3d H = Matrix3d::Zero();    // H = J^T * W^(-1) * J
        Vector3d b = Vector3d::Zero();    // bias
        cost  = 0;

        for(int i = 0; i < N; ++i){
            double xi = x_data[i], yi = y_data[i]; // 第i个数据点
            double error = yi - exp(ae * xi * xi + be * xi + ce);
            Vector3d J;
            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);

            H += inv_sigma * inv_sigma * J * J.transpose();
            b += - inv_sigma * inv_sigma * J * error;

            cost += error * error;
        }

        // 求解线性方程组 Hx = b
        Vector3d dx = H.ldlt().solve(b);
        if(isnan(dx[0])){
            cout << "result is nan!" <<endl;
            break;
        }

        if(iter > 0 && cost >= lastCost){
            cout << "cost:" << cost << ">= last cost" << lastCost << ", break." <<endl;
            break;
        }

        ae += dx[0];
        be += dx[1];
        ce += dx[2];
        
        lastCost = cost;

        cout << "total cost:" << cost << ", \t\t update:" << dx.transpose() << "\t\t estimated params:" << ae << " " << be << " " << ce <<endl;
    }

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout << "solve time cost = " << time_used.count() << " seconds." << endl;
    cout << "estimated abc = " << ae << " " <<  be << " " << ce <<endl;


    return 0;
}

5. 手写扩展卡尔曼滤波进行参数估计

利用扩展卡尔曼滤波进行参数估计的问题当时被问的还不多，可能是自己更多是做BA的原因。记得是在面试滴滴提前批的时候就被问了这个题，结果不出意料，答得马马虎虎。后续我补全了一下。

问题：估计方程y=ax+b中的a和b参数
解答：在这个例子中，我们创建了一个 KalmanFilter 类，其中 a_ 和 b_ 是我们要估计的参数，p_ 是估计的协方差，processVariance_ 和 measurementVariance_ 是过程噪声和测量噪声的方差。
我们使用了一些模拟数据（可以替换为实际观测数据），并在每个数据点上调用 update 方法来更新我们对参数 a 和 b 的估计。

#include <iostream>
#include <vector>

class KalmanFilter {
public:
    KalmanFilter(double a, double b, double processVariance, double measurementVariance)
        : a_(a), b_(b), p_(1.0), processVariance_(processVariance), measurementVariance_(measurementVariance) {}

    void update(double x, double y) {
        // Prediction
        double prediction = a_ * x + b_;
        double predictionError = y - prediction;

        // Update
        double gain = p_ * x / (x * x * p_ + measurementVariance_);
        a_ += gain * predictionError;
        b_ += gain * (y - prediction - a_ * x);
        p_ = (1 - gain * x) * p_ + processVariance_;
    }

    double getA() const { return a_; }
    double getB() const { return b_; }

private:
    double a_, b_, p_;
    double processVariance_, measurementVariance_;
};

int main() {
    // Simulate some data
    std::vector<std::pair<double, double>> data = {
        {1, 2},
        {2, 4},
        {3, 6},
        {4, 8},
        // Add more data points as needed
    };

    KalmanFilter kf(0, 0, 0.01, 0.1); // Initial guesses for a, b and variances

    for (const auto& point : data) {
        double x = point.first;
        double y = point.second;

        kf.update(x, y);

        std::cout << "Estimated a: " << kf.getA() << ", Estimated b: " << kf.getB() << std::endl;
    }

    return 0;
}

这个题仅做参考，个人水平有限。