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 );
}