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