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-1result[(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> -1Data_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();}}
