C/C++ 实现矩阵运算（罗德里格斯，逆，行列式，转置）

C/C++ 实现矩阵运算（罗德里格斯，逆，行列式，转置）

由于工程需要实现一些矩阵运算功能，在这里贴一下

/* 3X3矩阵求逆 */
	temp = PR[0][0] * PR[1][1] * PR[2][2] + PR[0][1] * PR[1][2] * PR[2][0] + PR[0][2] * PR[1][0] * PR[2][1] -
		PR[0][2] * PR[1][1] * PR[2][0] - PR[0][1] * PR[1][0] * PR[2][2] - PR[0][0] * PR[1][2] * PR[2][1];
	PRI[0] = (PR[1][1] * PR[2][2] - PR[1][2] * PR[2][1]) / temp;
	PRI[1] = -(PR[0][1] * PR[2][2] - PR[0][2] * PR[2][1]) / temp;
	PRI[2] = (PR[0][1] * PR[1][2] - PR[0][2] * PR[1][1]) / temp;
	PRI[3] = -(PR[1][0] * PR[2][2] - PR[1][2] * PR[2][0]) / temp;
	PRI[4] = (PR[0][0] * PR[2][2] - PR[0][2] * PR[2][0]) / temp;
	PRI[5] = -(PR[0][0] * PR[1][2] - PR[0][2] * PR[1][0]) / temp;
	PRI[6] = (PR[1][0] * PR[2][1] - PR[1][1] * PR[2][0]) / temp;
	PRI[7] = -(PR[0][0] * PR[2][1] - PR[0][1] * PR[2][0]) / temp;
	PRI[8] = (PR[0][0] * PR[1][1] - PR[0][1] * PR[1][0]) / temp;

//求行列式
double det(int n, double *Mat)
{
	if (n == 1)
		return Mat[0];
	double *subMat = new double[(n - 1)*(n - 1)];//创建n-1阶的代数余子式阵subMat    
	int mov = 0;//判断行是否移动   
	double sum = 0.0;//sum为行列式的值  
	for (int Matrow = 0; Matrow < n; Matrow++) // Mat的行数把矩阵Mat(nn)赋值到subMat(n-1)  
	{
		for (int subMatrow = 0; subMatrow < n - 1; subMatrow++)//把Mat阵第一列各元素的代数余子式存到subMat  
		{
			mov = Matrow > subMatrow ? 0 : 1; //subMat中小于Matrow的行，同行赋值，等于的错过，大于的加一  
			for (int j = 0; j < n - 1; j++)  //从Mat的第二列赋值到第n列  
			{
				subMat[subMatrow*(n - 1) + j] = Mat[(subMatrow + mov)*n + j + 1];
			}
		}
		int flag = (Matrow % 2 == 0 ? 1 : -1);//因为列数为0，所以行数是偶数时候，代数余子式为1.  
		sum += flag * Mat[Matrow*n] * det(n - 1, subMat);//Mat第一列各元素与其代数余子式积的和即为行列式
	}
	delete[]subMat;
	return sum;
}

/* 函数定义 */
//矩阵转置
double *MatT(int row, int col, double *Mat)
{
	double *result = new double[row*col];//结果矩阵
	for (int i = 0; i < row*col; i++)//遍历Mat
	{
		//第x个元素对应row*col阶矩阵的第rowMat行第colMat列
		int x = i + 1;//i从0开始 x从1开始
		int rowMat = 1;
		int colMat = 0;
		while (1)
		{
			if (x - col > 0)
			{
				x = x - col;
				rowMat++;
			}
			else
			{
				colMat = x;
				break;
			}
		}
		//上述将一维数组下标i转化成了自然矩阵行rowMat列colMat
		//转置后为colMat行rowMat列
		//在转化为一维坐标即可j=(colMat-1)*row+rowMat-1
		result[(colMat - 1)*row + rowMat - 1] = Mat[i];
	}
	return result;
}

// opencv
// int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian )
/*rodrigues Transform*/
void rodriguesTransform(Matrix_Type src, Matrix_Type& dst)
{
	if ((src.rows() == 1 && src.cols() == 3) || (src.cols() == 1 && src.rows() == 3))
	{
		if (src.rows() == 1)
			src = src.transpose();

		Data_Type theta = src.norm();

		if (theta < 1.0e-6)
		{
			dst = Matrix_Type::Identity(3, 3);
		}
		else
		{
			src = src / theta;

			Matrix_Type temp = Matrix_Type::Zero(3, 3);
			temp(0, 1) = -src(2, 0); temp(0, 2) = src(1, 0);
			temp(1, 0) = src(2, 0); temp(1, 2) = -src(0, 0);
			temp(2, 0) = -src(1, 0); temp(2, 1) = src(0, 0);

			dst = Matrix_Type::Identity(3, 3) + temp * sin(theta) + temp * temp * (1.0 - cos(theta));
		}
	}
	else if (src.cols() == 3 && src.rows() == 3)
	{
		Eigen::JacobiSVD<Matrix_Type> svd(src, Eigen::ComputeFullU | Eigen::ComputeFullV);
		Matrix_Type R = svd.matrixU() * svd.matrixV().transpose();
		Matrix_Type r(1, 3);
		r << R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1);
		Data_Type s = std::sqrt((r(0, 0) * r(0, 0) + r(0, 1) * r(0, 1) + r(0, 2) * r(0, 2)) * 0.25);
		Data_Type c = (R(0, 0) + R(1, 1) + R(2, 2) - 1) * 0.5;
		c = c > 1. ? 1. : c < -1. ? -1. : c;  // 1 <c> -1
		Data_Type theta = std::acos(c);
		if (s < 1.0e-5)
		{
			Data_Type t;
			if (c > 0)
			{
				r = Matrix_Type::Zero(1, 3);
			}
			else
			{
				t = (R(0, 0) + 1) * 0.5;
				r(0, 0) = std::sqrt(std::max(t, Data_Type(0.)));
				t = (R(1, 1) + 1) * 0.5;
				r(0, 1) = std::sqrt(std::max(t, Data_Type(0.))) * (R(0, 1) < 0 ? -1. : 1.);
				t = (R(2, 2) + 1) * 0.5;
				r(0, 2) = std::sqrt(std::max(t, Data_Type(0.))) * (R(0, 2) < 0 ? -1. : 1.);
				if (fabs(r(0, 0)) < fabs(r(0, 1)) && fabs(r(0, 0)) < fabs(r(0, 2)) && (R(1, 2) > 0) != (r(0, 1) * r(0, 2) > 0))
					r(0, 2) = -r(0, 2);
				theta /= r.norm();
				r *= theta;
			}
		}
		else
		{
			Data_Type vth = 1 / (2 * s);
			vth *= theta;
			r *= vth;
		}
		dst = r.transpose();
	}
}