最小二乘拟合平面

原创 2016年07月26日 16:07:36

最小二乘拟合平面


#include <iostream>  
#include <windows.h>  
#include "opencv.hpp"  
#include <string>
#include <stdlib.h>
#include <stdio.h>


using namespace std;
using namespace cv;

//Ax+by+cz=D  
void cvFitPlane(const CvMat* points, float* plane){
    // Estimate geometric centroid.  
    int nrows = points->rows;
    int ncols = points->cols;
    int type = points->type;
    CvMat* centroid = cvCreateMat(1, ncols, type);
    cvSet(centroid, cvScalar(0));
    for (int c = 0; c<ncols; c++){
        for (int r = 0; r < nrows; r++)
        {
            centroid->data.fl[c] += points->data.fl[ncols*r + c];
        }
        centroid->data.fl[c] /= nrows;
    }
    // Subtract geometric centroid from each point.  
    CvMat* points2 = cvCreateMat(nrows, ncols, type);
    for (int r = 0; r<nrows; r++)
    for (int c = 0; c<ncols; c++)
        points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
    // Evaluate SVD of covariance matrix.  
    CvMat* A = cvCreateMat(ncols, ncols, type);
    CvMat* W = cvCreateMat(ncols, ncols, type);
    CvMat* V = cvCreateMat(ncols, ncols, type);
    cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
    cvSVD(A, W, NULL, V, CV_SVD_V_T);
    // Assign plane coefficients by singular vector corresponding to smallest singular value.  
    plane[ncols] = 0;
    for (int c = 0; c<ncols; c++){
        plane[c] = V->data.fl[ncols*(ncols - 1) + c];
        plane[ncols] += plane[c] * centroid->data.fl[c];
    }
    // Release allocated resources.  
    //cvReleaseMat(¢roid);
    cvReleaseMat(&points2);
    cvReleaseMat(&A);
    cvReleaseMat(&W);
    cvReleaseMat(&V);
}



int main()
{

    //source img 8192*14000
    //Mat im = imread("D:\\XXX02.bmp", 0);
    Mat im = imread("D:\\XXX02.bmp", 0);


    double t = (double)getTickCount();

    Mat im_resize;
    resize(im, im_resize, Size(0.1*im.cols, 0.1*im.rows));


    //================================ using least squares to fit the plane==================================//
    vector<int> X_vector;
    vector<int> Y_vector;
    vector<int> Z_vector;
    //初始值为左上角点,
    for (int i = 0; i < im_resize.rows; i++){
        //uchar*rowptr = im.ptr<uchar>(i);
        for (int j = 0; j < im_resize.cols; j++){
            X_vector.push_back(j * 10);
            Y_vector.push_back(i * 10);
            Z_vector.push_back(im_resize.at<uchar>(i, j));
            //cout << rowptr[j] << endl;
        }
    }

    /*//测试用
    double x_point[] = { 1, 2, 1, 4, 2, 6, 7, 3, 9 };
    double y_point[] = { 1, 1, 3, 4, 5, 2, 7, 8, 2 };
    double z_point[] = { 91, 102, 103, 104, 105, 106, 107, 108, 109 };

    vector<double> X_vector(x_point, x_point + 9);
    vector<double> Y_vector(y_point, y_point + 9);
    vector<double> Z_vector(z_point, z_point + 9);*/

    CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵   
    for (int i = 0; i < X_vector.size(); ++i)
    {
        points_mat->data.fl[i * 3 + 0] = X_vector[i];//矩阵的值进行初始化   X的坐标值  
        points_mat->data.fl[i * 3 + 1] = Y_vector[i];//  Y的坐标值  
        points_mat->data.fl[i * 3 + 2] = Z_vector[i]; //<span style = "font-family: Arial, Helvetica, sans-serif;">//  Z的坐标值</span>  

    }
    float plane12[4] = { 0 };//定义用来储存平面参数的数组   
    cvFitPlane(points_mat, plane12);//调用方程   

    //Ax+By+Cz=D
    //其中 A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3]
    float A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3];
    cout << "A=" << plane12[0] << ",B=" << plane12[1] << ",C=" << plane12[2] << ",D=" << plane12[3] << endl;

    t = double(getTickCount() - t) * 1000 / getTickFrequency();
    cout << "the least squares time = " << t << " ms" << endl;

    //=============================== END using least squares to fit the plane================================//

    //test the result plane 
    Mat resultImg(im.size(), CV_8U);
    for (int i = 0; i < im.rows; i++){
        //uchar*rowptr = im.ptr<uchar>(i);
        uchar*resultptr = resultImg.ptr<uchar>(i);
        for (int j = 0; j < im.cols; j++){
            //Ax+By+Cz=D, z=(D-Ax-By)/C
            resultptr[j] = (D - A*j - B*i) / C;
        }
    }

    //resultImg -srcimg
    Mat diff = resultImg - im;
    namedWindow("diff img", 0);
    imshow("diff img", diff);

    Mat diffThre;
    threshold(diff, diffThre, 10, 255, THRESH_BINARY);

    namedWindow("diffThre img", 0);
    imshow("diffThre img", diffThre);

    waitKey();

    //Sleep(5000);
    return 0;

}

参考了这个http://blog.csdn.net/zhouyelihua/article/details/46122977

版权声明:本文为博主原创文章,未经博主允许不得转载。

最小二乘法拟合平面。

本文采用了opencv的一些函数来对平面进行拟合。 //Ax+by+cz=D void cvFitPlane(const CvMat* points, float* plane){ // ...
  • zhouyelihua
  • zhouyelihua
  • 2015年05月28日 15:00
  • 13785

最小二乘法平面方程拟合计算, 点云法向量估算

利用最小二乘法求解点云/点集/多点的拟合平面,求解点云的法向量。
  • z444_579
  • z444_579
  • 2015年11月25日 17:22
  • 3425

opencv最小二乘法拟合平面

//Ax+by+cz=D   void cvFitPlane(const CvMat* points, float* plane){       // Estimate geometric cen...
  • u014260892
  • u014260892
  • 2016年07月30日 08:18
  • 2452

opencv 最小二乘拟合平面

//Ax+by+cz=D   void cvFitPlane(const CvMat* points, float* plane){       // Estimate geometric centr...
  • laobai1015
  • laobai1015
  • 2017年06月22日 13:31
  • 893

平面方程拟合计算

其程序代码如下: #include "stdafx.h" #include #include #include #define MAX 10 void Inverse(double ...
  • zang141588761
  • zang141588761
  • 2016年01月15日 13:31
  • 2141

最小二乘法 拟合平面直线

前言:     最近要实现一个算法,“对一系列点拟合出一条线,且区分出不属于改线的点”。在网上找了许多资料,用数学公式解释原理以及用matlab实现的居多,本文章主要解释用最小二乘法的进行点拟合成线,...
  • ouyangying123
  • ouyangying123
  • 2017年01月03日 19:15
  • 3109

程序员数学——最小二乘法,线性拟合(一)

1.在工程中经常会遇到这种问题,通过AD采集获得一组电压(电流)数据x0, x1, x2, …, xn,这些电压数据所对应的物理数据为y0, y1, y2, …, yn,即有如下对应关...
  • linxing927
  • linxing927
  • 2016年07月07日 14:28
  • 390

opencv最小二乘拟合平面

//平面拟合(Ax+By+Cz+1=0) void cvFitPlane() { double array[4][3],Y[3];//待拟合的点 Zer...
  • qq_15805507
  • qq_15805507
  • 2017年03月17日 15:34
  • 129

最小二乘拟合空间平面

题目的意思是求一个平面P,使得 ∑(Distance(pi,P)^2)最小化, pi是给定点集里的点。 Right? 对于a*x+b*y+c*z+m=0,m!=0的情况,写成a*x+b*y+c*z+...
  • msh1216
  • msh1216
  • 2012年03月04日 09:31
  • 2256

最小二乘曲线拟合matlab实现

曲线拟合[1]是一种常用的模型拟合方法,经常用在相机标定或其它传感器标定上。对于加性高斯噪声,最小二乘通常是一种最优的估计方法(PS:当外点存在时,通常可以采用其它robust estimator 或...
  • ranchlai
  • ranchlai
  • 2013年09月02日 14:10
  • 5444
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:最小二乘拟合平面
举报原因:
原因补充:

(最多只允许输入30个字)