SLAM相关手撕代码题

参考：

• Eigen求最小二乘解

1.最小二乘拟合直线

• 单纯使用C++：

std::vector<double> LineFit(const std::vector<double>& x, const std::vector<double>& y)
{
    double k = 1, b = 1;
    const double eps = 1e-5;

    while(true)
    {
        double h11 = 0, h12 = 0, h21 = 0, h22 = 0;
        double je1 = 0, je2 = 0;
        for(int i = 0; i < x.size(); i++)
        {
            double res = x[i] * k + b - y[i];
            double dk = x[i];
            double db = 1;
            h11 += dk * dk;
            h12 += dk * db;
            h21 += db * dk;
            h22 += db * db;
            je1 += dk * res;
            je2 += db * res;
        }
        double detH = h11 * h22 - h12 * h21;
        double delta_k = - (h22 * je1 - h12 * je2) / detH;
        double delta_b = - (h11 * je2 - h21 * je1) / detH;
        k += delta_k;
        b += delta_b;
        if(abs(delta_k) < eps && abs(delta_b) < eps)
            break;
    }

    return {k, b};
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

• 使用 Eigen：

std::vector<double> LineFit(const std::vector<double>& x, const std::vector<double>& y)
{
    Eigen::Vector2d X = Eigen::Vector2d::Zero();

    const double eps = 1e-5;

    while(true)
    {
        Eigen::Vector2d b = Eigen::Vector2d::Zero();
        Eigen::Matrix2d H = Eigen::Matrix2d::Zero();

        for(int i = 0; i < x.size(); i++)
        {
            double res = x[i] * X(0) + X(1) - y[i];
            Eigen::Vector2d J;
            J << x[i], 1;
            H += J * J.transpose();
            b += res * J;
        }
        Eigen::Vector2d delta_X = -H.inverse() * b;
        X += delta_X;
        if(delta_X.norm() < eps)
            break;
    }

    return {X(0), X(1)};
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

2.RANSAC 最小二乘拟合直线

cv::Mat dataFit(const vector<double> &x, const vector<double> &y)
{
    int dataSize = x.size();
    cout << "x.size = " << x.size() << ", y.size = " << y.size() << endl;
    cv::Mat A = cv::Mat::zeros(dataSize, 2, CV_64FC1);
    cv::Mat b = cv::Mat::zeros(dataSize, 1, CV_64FC1);

    // 构造矩阵A和b
    for(int i = 0; i < dataSize; i++)
    {
        A.at<double>(i, 0) = 1;
        A.at<double>(i, 1) = x[i];
        b.at<double>(i, 0) = y[i];      // 注意这个只有第0列
    }
    cv::Mat X;
    // SVD分解求解 Ax = b线性最小二乘
    solve(A, b, X, DECOMP_SVD);
//    cout << "A = " << endl;
//    cout << A << endl;
//    cout << "b = " << endl;
//    cout << b << endl;
//    cout << endl << "estimate result : " << endl;
//    cout << "ae = " << X << endl << endl;

    return X;
}

/**
 * 1.随机选择两个点，拟合一条直线
 * 2.计算其他所有点到这个点的距离，距离小于一定的值认为是内点，统计所有内点的个数
 * 3.重复上述过程M次，寻找内点数最大的模型
 * 4.利用所有的内点重新进行直线拟合
 * @return  估计得到的直线参数矩阵
 */
 // threshold：判断为内点的距离阈值；  M: 重复寻找模型的次数
cv::Mat RansacFit(double threshold, int M, const vector<double> &x_data, const vector<double> &y_data)
{
    int dataSize = x_data.size();
    int maxInlineNum = 0;
    vector<vector<bool>> inlineIndexes;   // 所有模型的内点索引
    int bestModelIndex  = 0;             // 最好模型的索引
    double best_k, best_b;

    for(int i = 0; i < M; i++)
    {
        int num1 = rand() % dataSize;       // 第1个点
        int num2 = rand() % dataSize;       // 第2个点
        double x1 = x_data[num1];
        double x2 = x_data[num2];
        double y1 = y_data[num1];
        double y2 = y_data[num2];
        double k = (y2 - y1) / (x2 - x1);
        double b = y1 - k * x1;
        double inlineNum = 0;       // 内点个数
        vector<bool> inlineIndex;    // 当前模型的内点索引

        cout << "第 " << i << " 次模型寻找，" << "num1 = " << num1 << ", num2 = " << num2 << endl;
        cout << "k = " << k << ", b = " << b << endl;

        // 遍历所有的数据点，寻找内点
        for(int j = 0; j < dataSize; j++)
        {
            double error = k * x_data[j] + b - y_data[j];
            cout << " error " << j << " = " << error << ", ";
            if(myabs(error) < threshold)     // 内点
            {
                inlineNum++;
                inlineIndex.push_back(true);  // 当前模型下是否属于内点
            }
            else
            {
                inlineIndex.push_back(false);
            }
        }
        inlineIndexes.push_back(inlineIndex);      // 存储当前模型的内点
        cout << "内点个数 : " << inlineNum << endl;
        if(inlineNum > maxInlineNum)
        {
            maxInlineNum = inlineNum;
            bestModelIndex = i;
            best_k = k;
            best_b = b;
        }
    }
    cout << "最佳模型参数：" << "第 " << bestModelIndex << " 次寻找到" << endl;
    cout << "best_k = " << best_k << ", best_b = " << best_b << ", 最大内点数：" << maxInlineNum << endl;

    vector<bool> bestIndex = inlineIndexes[bestModelIndex];    // 最佳模型的索引
    vector<double> inlineX_data, inlineY_data;
    for(int i = 0; i < dataSize; i++)   // 遍历最好的模型的所有内点，组成新的数据集
    {
        if(bestIndex[i])  // 这个点属于内点
        {
            inlineX_data.push_back(x_data[i]);
            inlineY_data.push_back(y_data[i]);
        }
    }

    return dataFit(inlineX_data, inlineY_data);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

3.开根号

3.1.高斯牛顿法

double MySqrt(double a)
{
    const double eps = 1e-5;
    double x = a;
    while(true)
    {
        // 开2次根号
        // double J = 2 * x;      
        // double b = x * x - a;
        
        // 开3次根号
        double J = 3 * x * x;
        double b = x * x * x - a;
        
        double JTJ = J * J;
        double JTb = J * b;
        double delta_x = -JTb / JTJ;
        if(abs(delta_x) < eps)
            break;
        x += delta_x;
    }

    return x;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

3.2.二分法

double MySqrt(double a)
{
    const double eps = 1e-5;
    double left = 0;
    double right = a;
    double x = 0;
    while(left <= right)
    {
        if(abs(left - right) < eps)
            break;
        x = (left + right) / 2;
        // 开二次根号
        double res = x * x;
        if(res > a)
            right = x;
        else
            left = x;
    }

    return x;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

4.高斯牛顿法拟合曲线