C++opencv实现最小二乘法拟合直线和平面

使用opencv中的Mat实现用矩阵的方式根据最小二乘法拟合直线和平面方程，但是好像不能实现拟合斜率无穷大的直线和平面方程，后续再改进吧。
有关于原理部分，有时间再详细写一下。

#include "stdafx.h"
#include <opencv2/opencv.hpp>  
#include <vector>  
#include <iostream>  
#include <fstream>  

#pragma warning(disable:4244)

using namespace cv;
using namespace std;

/*输入一组坐标值，根据最小二乘法计算直线方程 y = kx + b 
先返回斜率 k ,再返回截距 b*/
Mat OLS_Line(vector<Point> point)
{
    //Ax = b的形式，将A b 写成矩阵的形式
    Mat A((int)point.size(), 2, CV_32F);
    Mat b((int)point.size(), 1, CV_32F);

    //初始化矩阵A
    for (size_t i = 0; i<point.size(); i++)
        A.at<float>((int)i, 1) = 1;
    for (size_t i = 0; i< point.size(); i++)
        A.at<float>((int)i, 0) = point[i].x;
//  cout <<矩阵A：<< endl << A << endl;

    //初始化矩阵b 
    for (size_t i = 0; i< point.size(); i++)
        b.at<float>((int)i, 0) = point[i].y;
//  cout <<"矩阵b"<< endl << b << endl;

    //根据线性代数知识，A'* A * x = A' * b 求得的矩阵 x 即为最优解
    //解 x = (A' * A)^-1 * A' * b

    Mat x = (A.t()*A).inv()*A.t()*b;

    return x;
}


/*输入一组坐标值，根据最小二乘法计算平面方程
分别返回 a ,b, c 的值
aX + bY - Z + c = 0 */
Mat OLS_Plane(vector<Point3f> point)
{
    //Ax = 0的形式，将A, b 写成矩阵的形式
    Mat A((int)point.size(), 3, CV_32F);
    Mat b((int)point.size(), 1, CV_32F);

//  cout <<"原始点为:"<< point << endl;

    //初始化矩阵A
    for (size_t i = 0; i< point.size(); i++)
        A.at<float>((int)i, 0) = (point[i].x);

    for (size_t i = 0; i< point.size(); i++)
        A.at<float>((int)i, 1) = (point[i].y);

    for (size_t i = 0; i<point.size(); i++)
        A.at<float>((int)i, 2) = 1;

//  cout << "矩阵A:" << endl << A << endl;

    //初始化矩阵b 
    for (size_t i = 0; i< point.size(); i++)
        b.at<float>((int)i, 0) = -(point[i].z);

//  cout << "矩阵b:" << endl << b << endl;

    //根据线性代数知识，A'* A * x = A' * b 求得的矩阵 x 即为最优解
    //解 x = (A' * A)^-1 * A' * b

    Mat x = -((A.t()*A).inv()*A.t()*b);

    return x;
}


int main()
{
    vector<Point> point;
    point.push_back(Point(1, 1));
    point.push_back(Point(2, 2));
    point.push_back(Point(3, 2));
    //point.push_back(Point(4, 10));

    cout << OLS_Line(point) << endl;;

    vector<Point3f> point3;
    point3.push_back(Point3f(2, -1,4));
    point3.push_back(Point3f(-1, 3, -2));
    point3.push_back(Point3f(0, 2, 5));
    point3.push_back(Point3f(0, 0, -14));
    //cout << point3 << endl;
    cout << OLS_Plane(point3) << endl;


    vector<Point3f> point33;
    point33.push_back(Point3f(-1, -1, -5));
    point33.push_back(Point3f(-8, 1, 0));
    point33.push_back(Point3f(3, 2, -12));
    point33.push_back(Point3f(0, 0, -8));
    //cout << point33 << endl;
    cout << OLS_Plane(point33) << endl;

    waitKey(0);
    system("pause");
    return 0;
}