线性方程组

一个m个方程，n个未知数的方程组定义如下

\[ \begin{cases} a_{11} x_1 + a_{12}x_2 + \cdots + a_{1n}x_n = b_1 \\ a_{21} x_1 + a_{22}x_2 + \cdots + a_{2n}x_n = b_2 \\ \vdots \\ a_{m1} x_1 + a_{m2}x_2 + \cdots + a_{mn}x_n = b_m \\ \end{cases} \]

把上式中的系数与一个m*n的阵列联系起来，称这个了阵列为方程组的系数矩阵(coefficient matrix):

\[ \begin{bmatrix} a_{11} & a_{12} & ... & a_{1n} \cr a_{21} & a_{22} & ... & a_{2n} \cr \vdots \cr a_{m1} & a_{m2} & ... & a_{mn} \cr\end{bmatrix} \]

如果在系数矩阵的右侧添加一列方程组的右端项，则得到新的矩阵, 称这个矩阵为方程组的增广矩阵(augmented maxtrix):

\[ \left[\begin{array}{c,c,c,c,|,c} a_{11} & a_{12} & ... & a_{1n} & b_1\cr a_{21} & a_{22} & ... & a_{2n} & b_2\cr \vdots & & & &\vdots\cr a_{m1} & a_{m2} & ... & a_{mn} & b_m\cr \end{array}\right] \]

令：

\[ A=\left[\matrix{ a_{11} & a_{12} & ... & a_{1n} \cr a_{21} & a_{22} & ... & a_{2n} \cr \vdots \cr a_{m1} & a_{m2} & ... & a_{mn} \cr }\right], X=\left[\matrix{ x_1 \cr x_2 \cr \vdots \cr x_n}\right], B=\left[\matrix{ b_1 \cr b_2 \cr \vdots \cr b_m}\right] \]

因此定义一般形式为：

\[ A_{m \times n}X_{n \times 1} = B_{m \times 1} \]

r(A) < r(A|B),方程组无解

r(A) = r(A|B)=n,方程组有唯一解

r(A) = r(A|B) < n,方程组有无穷解

r(A) > r(A|B),这种情况不存在

其中r()代表矩阵的秩，A|B是增广矩阵，n是X未知数个数.

线性最小二乘

以一个简单的直线拟合的例子说明线性最小二乘

假设方程\(y = kx + b\)，给定n组观测数据\((x, y)\)求解系数\(X = [k, b]^T\).

解为：

\[ X = (A^TA)^{-1}A^TB \]

矩阵分解

LU 分解

将系数矩阵A转变成等价两个矩阵L和U的乘积，其中L和U分别是单位下三角矩阵和上三角矩阵。当A的所有顺序主子式都不为0时，矩阵A可以分解为A=LU（所有顺序主子式不为0，矩阵不一定不可以进行LU分解）。其中L是下三角矩阵，U是上三角矩阵。

求解方程组

当系数矩阵A完成了LU分解后，方程组Ax = b就可以化为L(Ux) = b，等价于求解两个方程组Ly = b和Ux = y

Example

/**
******************************************************************************
* @file
* @author  maky <chengwei920412@outlook.com>
* @version
* @date    2018-12-24 23:16:58
* @brief
******************************************************************************
* @attention
*
*
******************************************************************************
*/

#include <iostream>
#include <Eigen/Eigen>

int main(int argc, char * argv[])
{
    Eigen::Matrix4d A;
    A << 6., 2., 1., -1., 
        2., 4., 1., 0., 
        1., 1., 4., -1., 
        -1., 0., -1., 3.;
    auto solver = A.lu();
    {
        Eigen::Vector4d B(1., 0., 0., 0.);
        auto x = solver.solve(B);
        std::cout << x.transpose() << std::endl;
    }
    {
        Eigen::Vector4d B(0., 1., 0., 0.);
        auto x = solver.solve(B);
        std::cout << x.transpose() << std::endl;
    }
    {
        Eigen::Vector4d B(0., 0., 1., 0.);
        auto x = solver.solve(B);
        std::cout << x.transpose() << std::endl;
    }
    {
        Eigen::Vector4d B(0., 0., 0., 1.);
        auto x = solver.solve(B);
        std::cout << x.transpose() << std::endl;
    }
    return 0;
}

QR 分解

QR 分解是把矩阵分解成一个正交矩阵Q(\(Q^TQ = I\))与一个上三角矩阵R的积。QR 分解经常用来解线性最小二乘法问题。QR 分解也是特定特征值算法即QR算法的基础.

其中， Q 是一个标准正交方阵， R 是上三角矩阵。

线性最小二乘

对于矩阵\(A_{m \times n} \, \, (m \ge n)\), 存在一个单位列正交矩阵\(Q_{m \times n}\)和一个上三角矩阵\(R_{n \times n}\)使得：

\[ A = QR \]

我们找一个向量xx使得\(\lVert Ax - b \rVert\)最小，首先把矩阵Q扩充为一个正交矩阵，于是有

\[ \begin{align} \begin{Vmatrix} A\mathbf x-\mathbf b\end{Vmatrix} &=\begin{Vmatrix}[Q,\widetilde Q]^T(A\mathbf x-\mathbf b)\end{Vmatrix}\\ &=\begin{Vmatrix} \begin{bmatrix}Q^T \\ \widetilde Q^T\end{bmatrix}(QR\mathbf x-\mathbf b)\end{Vmatrix}\\ &=\begin{Vmatrix}\begin{bmatrix}R\mathbf x-Q^T\mathbf b \\ -\widetilde Q^T\mathbf b\end{bmatrix}\end{Vmatrix} \end{align} \]

所以\(\lVert Ax - b \rVert\)最小也就是取\(\begin{Vmatrix}R\mathbf x-Q^T\mathbf b\end{Vmatrix}\)最小。所以最小二乘解为 :

\[ \mathbf x=R^{-1}Q^T\mathbf b \]

SVD 分解

LLT 分解（Cholesky 分解）

Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。它要求矩阵的所有特征值必须大于零，故分解的下三角的对角元也是大于零的。Cholesky分解法又称平方根法，是当A为实对称正定矩阵时，LU三角分解法的变形。

定理: 若\(A \in R^{n \times n}\)对称正定，则存在一个对角元为正数的下三角矩阵\(L \in R^{n \times n}\), 使得\(A = LL^{T}\)成立。

\[ A = LL^{T} \]

Cholesky 分解在计算马氏距离时的作用

对于协方差矩阵，是实对称半正定矩阵，如果其对角线元素全部为正，则协方差矩阵是实对称正定矩阵，因此可进行Cholesky分解。

\[ \Sigma = LL^{T} \]

在计算样本X中两个个特征向量的距离时，可以采用马氏距离表示：

\[ D(x, y)=\sqrt{(x-y)^{T} \Sigma^{-1}(x-y)} \]

直接对协方差矩阵求逆计算复杂度较高，因此使用Cholesky分解:

\[ D(x, y)=\sqrt{\left[L^{-1}(x-y)\right]^{T}\left[L^{-1}(x-y)\right) ]} \]

LDLT 分解

Example

/**
******************************************************************************
* @file
* @author  maky <chengwei920412@outlook.com>
* @version
* @date    2018-12-25 23:16:58
* @brief
******************************************************************************
* @attention
*
*
******************************************************************************
*/
#include <iostream>
#include <cmath>
#include <random>
#include <vector>
#include <array>
#include <set>
#include <memory>
#include <algorithm>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>

template <class ModelT, class SampleT, int min_sample>
class RandomSampleConsensus
{
protected:
    typedef ModelT ModelT;
    typedef SampleT SampleT;
    enum { min_sample_ = min_sample };
public:
    class IModel {
    protected:
        enum { min_sample_ = min_sample };
        typedef SampleT SampleT;
    public:
        IModel() {}
        virtual ~IModel() {}
    public:
        virtual bool generate(const std::array<std::shared_ptr<SampleT>, min_sample_> &samples) = 0;
        virtual bool evaluate(const std::set<std::shared_ptr<SampleT>> &samples, float threshold) = 0;
    public:
        const std::set<std::shared_ptr<SampleT>> &getInliers() const { return inliers_; }
        int getWeight() { return inliers_.size(); }
    protected:
        std::set<std::shared_ptr<SampleT>> inliers_;
    };

public:
    RandomSampleConsensus(float threshold, float confidence = 0.995, float probability_of_inliers = 0.5, int iteration_total = 0)
    {
        reset(threshold, confidence, probability_of_inliers, iteration_total);
    }
    ~RandomSampleConsensus() {}
public:
    void reset(float threshold, float confidence = 0.995, float probability_of_inliers = 0.5, int iteration_total = 0)
    {
        threshold_ = threshold;
        iteration_total_ = iteration_total;
        if (iteration_total_ <= 0) {
            iteration_total_ = std::log(1 - confidence) / std::log(1 - std::pow(probability_of_inliers, min_sample_));
        }
        models_.clear();
        for (int pos = 0; pos < iteration_total_; pos++) {
            models_.push_back(std::make_shared<ModelT>());
        }
        std::srand((unsigned int)(std::time(nullptr)));
    }

    bool evaluate(const std::set<std::shared_ptr<SampleT>> &samples)
    {
        if (!samples.size() || samples.size() < min_sample) {
            return false;
        }
        for (int pos = 0; pos < iteration_total_; pos++) {
            auto &model = models_.at(pos);
            std::set<std::shared_ptr<SampleT>> remaining = samples;
            std::array<std::shared_ptr<SampleT>, min_sample_> model_sample;
            randomGenerateMinSample(remaining, model_sample);
            if (!model->generate(model_sample)) {
                return false;
            }
            if (!model->evaluate(remaining, threshold_)) {
                return false;
            }
        }
        // sort models by inliers count.
        std::sort(models_.begin(), models_.end(), [](const auto &left, const auto &right)
        {
            if (left->getWeight() > right->getWeight()) {
                return true;
            }
            return false;
        });
        return true;
    }
    const std::vector<std::shared_ptr<ModelT>> getModels(void) { return models_; }
protected:
    bool randomGenerateMinSample(std::set<std::shared_ptr<SampleT>> samples, std::array<std::shared_ptr<SampleT>, min_sample_> &model_sample)
    {
        int index = std::rand() % samples.size();
        for (auto iter = model_sample.begin(); iter != model_sample.end(); iter++) {
            int index = std::rand() % samples.size();
            auto sample = samples.begin();
            std::advance(sample, index);
            (*iter) = (*sample);
            samples.erase(sample);
        }
        return true;
    }
private:
    int iteration_total_;
    float threshold_;
    std::vector<std::shared_ptr<ModelT>> models_;
};


class LineModel : public RandomSampleConsensus<LineModel, Eigen::Vector2d, 2>::IModel
{
public:
    LineModel() {}
    virtual ~LineModel() {}
public:
    virtual bool generate(const std::array<std::shared_ptr<SampleT>, min_sample_> &samples)
    {
        if (!samples[0] || !samples[1]) {
            return false;
        }
        // y = kx + b;
        k_ = (samples[0]->y() - samples[1]->y()) / (samples[0]->x() - samples[1]->x());
        b_ = samples[0]->y() - k_ * samples[0]->x();
        model_sample_ = samples;
        return true;
    }
    virtual bool evaluate(const std::set<std::shared_ptr<SampleT>> &samples, float threshold)
    {
        for (auto iter : samples) {
            if (!iter) {
                return false;
            }
            if (threshold > distance(iter)) {
                inliers_.insert(iter);
            }
        }
        return true;
    }
    float distance(std::shared_ptr<SampleT> sample)
    {
        // http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
        // kx - y + b = 0
        return std::fabs(k_ * sample->x() + (-1.) * sample->y() + b_) / std::sqrt(k_ * k_ + (-1. * -1.));
    }
    const std::array<std::shared_ptr<SampleT>, min_sample_> getModelSample() const { return model_sample_; }
private:
    float k_, b_;
    std::array<std::shared_ptr<SampleT>, min_sample_> model_sample_;
};

double slope(int x0, int y0, int x1, int y1)
{
    return (double)(y1 - y0) / (x1 - x0);
}
void draw_line(cv::Mat& output, cv::Point a, cv::Point b, cv::Scalar color, int LineWidth)
{
    double s = slope(a.x, a.y, b.x, b.y);

    cv::Point p(0, 0), q(output.cols, output.rows);

    p.y = -(a.x - p.x) * s + a.y;
    q.y = -(b.x - q.x) * s + b.y;

    cv::line(output, p, q, color, LineWidth, cv::LINE_AA, 0);
}

void least_squares(const std::set<std::shared_ptr<Eigen::Vector2d>> &observation, float &k, float &b)
{
    Eigen::MatrixXd A(observation.size(), 2);
    Eigen::VectorXd B(observation.size());
    int pos = 0;
    for (auto iter : observation) {
        A(pos, 0) = iter->x();
        A(pos, 1) = 1.;
        B(pos, 0) = iter->y();
        pos++;
    }

    //Eigen::Vector2d x = (A.transpose() * A).inverse() * A.transpose() * B;
    Eigen::Vector2d x = A.colPivHouseholderQr().solve(B);
    k = x.x();
    b = x.y();
}

int main(int argc, char * argv[])
{
    while (true) {
        int canvas_size = 800;
        int points_size = 500;
        int outlier_size = 50;
        cv::Mat canvas(canvas_size, canvas_size, CV_8UC3, cv::Scalar::all(255));

        std::random_device seed;
        std::mt19937 random = std::mt19937(seed());
        std::uniform_int_distribution<int> range(0, canvas_size - 1);

        std::set<std::shared_ptr<Eigen::Vector2d>> observations;
        {
            std::normal_distribution<double> noise(0, 25);
            for (int pos = 0; pos < points_size; pos++)
            {
                int value = range(random);
                cv::Point point(floor(value + noise(random)), floor(value + noise(random)));
                cv::circle(canvas, point, floor(canvas_size / 100) + 3, cv::Scalar(0, 0, 0), 2, cv::LINE_AA);
                observations.insert(std::make_shared<Eigen::Vector2d>(point.x, point.y));
            }
        }
        {
            std::normal_distribution<double> noise(0, 200);
            for (int pos = 0; pos < outlier_size; pos++) {
                int value = range(random);
                cv::Point point(floor(value + noise(random)), floor(value + noise(random)));
                cv::circle(canvas, point, floor(canvas_size / 100) + 3, cv::Scalar(0, 0, 0), 2, cv::LINE_AA);
                observations.insert(std::make_shared<Eigen::Vector2d>(point.x, point.y));
            }
        }
        RandomSampleConsensus<LineModel, Eigen::Vector2d, 2> line_ransac(20);
        line_ransac.evaluate(observations);

        auto model = line_ransac.getModels().front();
        auto inliers = model->getInliers();
        if (inliers.size())
        {
            for (auto& inlier : inliers)
            {
                cv::Point point(floor(inlier->x()), floor(inlier->y()));
                cv::circle(canvas, point, floor(canvas_size / 100), cv::Scalar(0, 255, 0), -1, cv::LINE_AA);
            }
        }
        auto line = model->getModelSample();
        {
            auto begin = line[0];
            auto end = line[1];
            if (line[0] && line[1])
            {
                cv::Point begin(line[0]->x(), line[0]->y());
                cv::Point end(line[1]->x(), line[1]->y());
                draw_line(canvas, begin, end, cv::Scalar(0, 0, 255), 4);
            }
        }
        {
            float k_e = 0, b_e = 0;
            least_squares(inliers, k_e, b_e);
            auto begin = line[0];
            auto end = line[1];
            if (line[0] && line[1])
            {
                cv::Point begin(line[0]->x(), line[0]->x() * k_e + b_e);
                cv::Point end(line[1]->x(), line[1]->x() * k_e + b_e);
                draw_line(canvas, begin, end, cv::Scalar(255, 0, 0), 2);
            }
        }

        cv::imshow("RANSAC", canvas);
        char key = cv::waitKey(100);
        if (key == 27) {
            return 0;
        }
    }
    return 0;
}

如下图，其中，黑色为观测值，绿色点为inliers，红色线ransac模型，蓝色线为最小二乘拟合的直线。

Reference

http://www-users.math.umn.edu/~lerman/math5467/svd.pdf

https://baike.baidu.com/item/lu%E5%88%86%E8%A7%A3/764245?fr=aladdin

https://baike.baidu.com/item/QR%E5%88%86%E8%A7%A3/8918473?fr=aladdin

https://blog.csdn.net/wangshuailpp/article/details/80209863

https://blog.csdn.net/kokerf/article/details/72437294

https://www.cnblogs.com/liufuqiang/p/5663175.html