齐次方程线性最小二乘的求解

最小二乘问题其形式都为Ax − b = 0,如果问题形式发生改变,变为Ax = 0,那么这样的最小二乘问题应该如何求解呢?

Ax = 0形式的问题经常出现在重建问题(reconstruction)中。我们期望找到方程Ax = 0中 x 不等于零的解。由于该方程的特殊形式我们会发现对于 x 不等于零的解我们乘上任意的尺度因子 k 使解变为 kx 都不会改变最终结果。因为我们可以将问题转化为求解 x 使得||Ax||值最小并且||x||=1。

现在对矩阵 A 进行 SVD 分解有:

通过 SVD 分解中 D 矩阵的性质我们能够发现,D 为一个对角矩阵,且它的对角线元素呈降序排列。因此方程的解应该为:

该解在最后的位置有一个值为 1 的非零项。依据此结果,再根据方程:

我们可以发现,x 的值就为矩阵 V 的最后一列。

平面拟合的 例子：

int RanSac::PlaneFittingRansac(std::vector<Point>& pointset, FittingPlane& bestplane) {

    if (pointset.size()<3)
    {
        return -1;
    }

    std::vector<FittingPlane> fittingPlaneVect;
    int t = 0, num = 15;     //num 为迭代次数
    while (t < num)
    {
        //步骤1：从数据中随机选择3组数据
        std::set<int> threepoints;
        while (true) {
            int r = rand() % (pointset.size());
            threepoints.insert(r);
            if (threepoints.size() == 3)
                break;
        }

        PlaneEquation planeParameters;
        size_t index[3];
        int j = 0;
        for (std::set<int>::iterator iElement = threepoints.begin(); iElement != threepoints.end(); iElement++)
        {
            index[j] = *iElement;
            j++;
        }

        //步骤2：从选择3组数据计算平面方程
        getPlaneParameters(pointset, index, planeParameters);

        std::vector<size_t> inside;
        //步骤3：计算内点个数
        for (size_t i = 0; i < pointset.size(); i++)
        {
            if (distPointToPlane(pointset[i], planeParameters) < distanThreshold)
            {
                inside.push_back(i);
            }
        }

        FittingPlane fittingPlaneTemp;
        fittingPlaneTemp.insideNum = inside.size();
        fittingPlaneTemp.inside = inside;
        fittingPlaneTemp.planeParameters = planeParameters;
        fittingPlaneVect.push_back(fittingPlaneTemp);

        t++;
    }

    int insideNumTemp = 0, pos = 0;

    for (size_t j = 0; j < fittingPlaneVect.size(); j++)
    {
        if (insideNumTemp < fittingPlaneVect[j].insideNum) {

            insideNumTemp = fittingPlaneVect[j].insideNum;
            pos = j;
        }
        else
        {
            continue;
        }
    }
    
    if (insideNumTemp>insideNumThreshold)
    {
        bestplane = fittingPlaneVect[pos];
        //解最小二乘解问题
        Eigen::MatrixXf A(insideNumTemp,4);   //(行，列)
        
        for (size_t i = 0; i < insideNumTemp; i++)
        {
            A(i, 0) = pointset[fittingPlaneVect[pos].inside[i]].x;
            A(i, 1) = pointset[fittingPlaneVect[pos].inside[i]].y;
            A(i, 2) = pointset[fittingPlaneVect[pos].inside[i]].z;
            A(i, 3) = 1;
        }

        Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::MatrixXf V = svd.matrixV(), U = svd.matrixU();

        //Eigen::Matrix3f S = U.inverse() * A * V.transpose().inverse(); // S = U^-1 * A * VT * -1

        //std::cout << "A :
" << A << std::endl;
        //std::cout << "U :
" << U << std::endl;
        //std::cout << "S :
" << S << std::endl;
        //std::cout << "V :
" << V << std::endl;
        bestplane.planeParameters.A = V(0, 3);
        bestplane.planeParameters.B = V(1, 3);
        bestplane.planeParameters.C = V(2, 3);
        bestplane.planeParameters.D = V(3, 3);
    }
    
    //测试SVD分解结果
    //std::vector<size_t> insideTemp;
    //for (size_t i = 0; i < pointset.size(); i++)
    //{
    //    if (distPointToPlane(pointset[i], bestplane.planeParameters) < distanThreshold)
    //    {
    //        insideTemp.push_back(i);
    //    }
    //}

    return 0;
}