博客

计算伪逆1

vector<vector<double>> vec_jac{ {ImgJaco(0, 0), ImgJaco(0, 1), ImgJaco(0, 2), ImgJaco(0, 3), ImgJaco(0, 4), ImgJaco(0, 5)},
								  {ImgJaco(1, 0), ImgJaco(1, 1), ImgJaco(1, 2), ImgJaco(1, 3), ImgJaco(1, 4), ImgJaco(1, 5)},
								  {ImgJaco(2, 0), ImgJaco(2, 1), ImgJaco(2, 2), ImgJaco(2, 3), ImgJaco(2, 4), ImgJaco(2, 5)},
								  {ImgJaco(3, 0), ImgJaco(3, 1), ImgJaco(3, 2), ImgJaco(3, 3), ImgJaco(3, 4), ImgJaco(3, 5)},
								  {ImgJaco(4, 0), ImgJaco(4, 1), ImgJaco(4, 2), ImgJaco(4, 3), ImgJaco(4, 4), ImgJaco(4, 5)},
								  {ImgJaco(5, 0), ImgJaco(5, 1), ImgJaco(5, 2), ImgJaco(5, 3), ImgJaco(5, 4), ImgJaco(5, 5)},
								  {ImgJaco(6, 0), ImgJaco(6, 1), ImgJaco(6, 2), ImgJaco(6, 3), ImgJaco(6, 4), ImgJaco(6, 5)},
								  {ImgJaco(7, 0), ImgJaco(7, 1), ImgJaco(7, 2), ImgJaco(7, 3), ImgJaco(7, 4), ImgJaco(7, 5)}, };

	Mat mat(8, 6, CV_32FC1);
	for (int y = 0; y < 8; ++y) {
		for (int x = 0; x < 6; ++x) {
			mat.at<float>(y, x) =  20*vec_jac[y][x];
		}
	}

	Mat vec_jac_inv;
	invert(mat, vec_jac_inv, cv::DECOMP_SVD);

	MatrixXd ImgJaco_inv(6, 8);
	for (int x = 0; x < 6; x++)
	{
		for (int y = 0; y < 8; y++)
		{
			ImgJaco_inv(x, y) = vec_jac_inv.at<float>(x, y);
		}
	}

	cout << "jacobian矩阵的伪逆" << endl << ImgJaco_inv << endl;

计算伪逆2

MatrixXd pinv(MatrixXd  A)
{
	JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
	double  pinvtoler = 1.e-8; //tolerance
	int row = A.rows();
	int col = A.cols();
	int k = min(row, col);
	MatrixXd X = MatrixXd::Zero(col, row);
	MatrixXd singularValues_inv = svd.singularValues();//奇异值
	MatrixXd singularValues_inv_mat = MatrixXd::Zero(col, row);
	for (long i = 0; i < k; ++i) {
		if (singularValues_inv(i) > pinvtoler)
			singularValues_inv(i) = 1.0 / singularValues_inv(i);
		else singularValues_inv(i) = 0;
	}
	for (long i = 0; i < k; ++i)
	{
		singularValues_inv_mat(i, i) = singularValues_inv(i);
	}
	X = (svd.matrixV()) * (singularValues_inv_mat) * (svd.matrixU().transpose());

	return X;
}