slam面试题最小二成问题，wifi热点问题

1. 考虑线性方程组Ax=b,其中A∈Rm×n且rank(A)=min{m,n}.

1)当m>n, 推导求解最小二乘解.xlsq=min||Ax-b||2，需要写出细致的每一步的推导过程

推导步骤：

2. 假设有三个WIFI热点，位置分别在(x1,y1), (x2,y2), (x3,y3), 移动端测量到每一个热点的距离L1,L2和L3，要求解移动端的位置. 

需要写一个C/C++/JAVA的程序，（基于第一题的结论自己实现求解过程，可以调用基础的数学矩阵库，但是不允许调用Eigen等优化库），最终的程序需要可以运行，输入3个热点坐标和测量距离，可以得到正确的移动端位置。

下面是优化的问题解法。 其实还有更简单的解法，因为只有三个wifi热点，列三个方程，1，2，3，消掉其中的二次项，有点像高中的几何题，但这种方法只适合有三个wifi热点的情况。优化问题的解法能适合有更多wifi观测的情况。

#include <iostream>

#include <vector>

#include <opencv2/opencv.hpp>

using namespace std;

/**

 * @brief 计算误差向量

 * @param wifi_positions wifi的位置

 * @param term_loc 终端的位置

 * @param distances 观测的终端到wifi的位置

 * @param error_vector 存储误差向量（输出参数）

 * @param error_mean 存储平均误差用于迭代结束条件判断(输出参数）

 */

void CalErrorVector(const vector<cv::Point2f>& wifi_positions, const cv::Point2f& term_loc, const vector<float>& distances, cv::Mat& error_vector, float& error_mean)

{

    if (wifi_positions.size() != error_vector.rows||wifi_positions.size()!=distances.size()||error_vector.rows!=distances.size())

    {

        std::cout << "Please chack the dims of data" << std::endl;

        return;

    }

    cv::Point2f diff;

    error_mean = 0;

    for (int i = 0; i < wifi_positions.size(); i++)

    {

        diff = term_loc - wifi_positions[i];

        float e = diff.x * diff.x + diff.y * diff.y - distances[i] * distances[i];

        error_vector.at<float>(i, 0) = e;

        error_mean += std::fabs(e);

    }

    error_mean /= int(wifi_positions.size());

}

/**

 * @brief 计算雅可比矩阵

 * @param wifi_positions wifi的位置

 * @param term_loc 终端的位置

 * @param jac_matrix 存储雅可比矩阵（输出参数）

 */

void CalJacMatrix(const vector<cv::Point2f>& wifi_positions, const cv::Point2f& term_loc, cv::Mat& jac_matrix)

{

    if (wifi_positions.size() != jac_matrix.rows)

    {

        std::cout << "Please chack the dims of data" << std::endl;

        return;

    }

   

    cv::Point2f diff;

    for (int i = 0; i < wifi_positions.size(); i++)

    {

        diff = term_loc - wifi_positions[i];

        jac_matrix.at<float>(i, 0) = 2*diff.x;

        jac_matrix.at<float>(i, 1) = 2*diff.y;

    }

}

 

/**

 * @brief 优化函数

 * @param wifi_positions wifi的位置

 * @param distances 观测的终端到wifi的位置

 * @param term_loc 终端的位置（输出参数）

 */

void OptimizePosition(const vector<cv::Point2f>& wifi_potisions, const vector<float>& distances, cv::Point2f& term_loc)

{

    if (wifi_potisions.size() < 3)

    {

        std::cout << " The number of points should be greater than 3" << std::endl;

        return;

    }

    cv::Point2f init_loc(0.0, 0.0);

    for (int i = 0; i < wifi_potisions.size(); i++)

    {

        init_loc += wifi_potisions[i];

    }

    init_loc /= int(wifi_potisions.size());

    //定义误差向量

    cv::Mat error_vector;

    float error_mean;

    error_vector.create(wifi_potisions.size(), 1, CV_32F);

    CalErrorVector(wifi_potisions, init_loc, distances, error_vector, error_mean);

    //定义雅可比矩阵

    cv::Mat jac_mat;

    jac_mat.create(wifi_potisions.size(), 2, CV_32F);

 

    cv::Mat update;

    //当平均误差>0.0001执行while循环

    while (std::fabs(error_mean) > 0.0001)

    {

       

        //计算雅可比矩阵

        CalJacMatrix(wifi_potisions, init_loc, jac_mat);

        cv::Mat jac_mat_t;

        cv::transpose(jac_mat, jac_mat_t);

        cv::Mat H = jac_mat_t * jac_mat;

        cv::Mat H_inv;

        cv::invert(H, H_inv);

        //std::cout << H * H_inv << std::endl;

        cv::Mat g = - jac_mat_t * error_vector;

        update = H_inv * g;

        init_loc.x += update.at<float>(0, 0);

        init_loc.y += update.at<float>(1, 0);

        CalErrorVector(wifi_potisions, init_loc, distances, error_vector, error_mean);

        //std::cout << "loc: " << init_loc << std::endl;

    }

    term_loc = init_loc;

}

 

int main()

{

    //构造测试数据

    vector<cv::Point2f> wifi_positions = { cv::Point2f(1.0,0),cv::Point2f(0.0,1.0),cv::Point2f(1.0,1.0) };

    vector<float> distances = { 1.0,1.0,1.41421 };

    cv::Point2f term_loc_gt(0.0, 0.0), term_loc(0.0, 0.0);

    OptimizePosition(wifi_positions, distances, term_loc);

    //结果检验，如果diff的x和y成员接近0，则说明上面的代码是OK的

    auto diff = term_loc_gt - term_loc;

    std::cout << "diff: " << diff << std::endl;

    return 0;

}