Example #1
0
void lstsq(const Eigen::Matrix<ScalarT, Eigen::Dynamic, Eigen::Dynamic> &A,
           const Eigen::Matrix<ScalarT, Eigen::Dynamic, 1> &b,
           Eigen::Matrix<ScalarT, Eigen::Dynamic, 1> &x)
{

    if (A.rows() == A.cols()) {
        // solve via pivoting
        x = A.colPivHouseholderQr().solve(b);
    } else if (A.rows() > A.cols()) {
        // solving via SVD
        x = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
    } else {
        x.fill(std::numeric_limits<ScalarT>::quiet_NaN());
        std::cout << "Error solving linear system" << std::endl;
        return;
    }
}
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDLS( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
    //ds A matrix for system: A*X=0
    Eigen::Matrix< double, 4 , 4 > matAHomogeneous;

    //ds fill the matrix
    matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
    matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
    matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
    matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);

    //ds inhomogeneous solution
    const Eigen::Matrix< double, 4, 3 > matAInhomogeneous( matAHomogeneous.block< 4, 3 >( 0, 0 ) );
    const Eigen::Vector4d vecRHS( -matAHomogeneous.col( 3 ) );

    //ds solve the system and return
    return matAInhomogeneous.jacobiSvd( Eigen::ComputeFullU | Eigen::ComputeFullV ).solve( vecRHS );
}