旋转矩阵的自验证c++

As you known,旋转矩阵R,表示的是绕x、y、z轴三个方向的旋转角度,即,我们常说的:俯仰角、偏航角、横滚角。

                                                               

根据座标轴定义的不同及旋转顺序的不同,以先绕z轴再绕y轴再绕x轴旋转,那么,最终的选择矩阵R=Rx*Ry*Rz。

在使用该旋转矩阵之前,先要根据旋转矩阵的性质对它进行检查。(数学知识)

比如:

  1. 正交矩阵每一列都是单位矩阵,并且两两正交。
  2. 正交矩阵的逆(inverse)等于正交矩阵的转置(transpose)。
  3. and so on……

下面,鉴于我验证的时候没有百度到具体的公式(只有类似以上两行文字)以及相关代码,在此贴出几行验证的c++代码,供需要的人copy,免得再去翻线性代数的书了~

如果求得的值,与cout的“if  = 1”或者“if = 0"相等,则说明求得的R满足本身的性质。可以进行下一步的验证。

cout << R.t() << endl;
cout << R.inv() << endl;
cout << "if = 1 ? " << R.t()*(R) << endl;
r[0] = R.at<double>(0, 0);
r[1] = R.at<double>(0, 1);
r[2] = R.at<double>(0, 2);

r[3] = R.at<double>(1, 0);
r[4] = R.at<double>(1, 1);
r[5] = R.at<double>(1, 2);

r[6] = R.at<double>(2, 0);
r[7] = R.at<double>(2, 1);
r[8] = R.at<double>(2, 2);

cout << "if = 1 ? " << (r[2] * r[2] + r[0] * r[0] + r[1] * r[1]) << endl;
cout << "if = 1 ? " << (r[3] * r[3] + r[4] * r[4] + r[5] * r[5]) << endl;
cout << "if = 1 ? " << (r[6] * r[6] + r[7] * r[7] + r[8] * r[8]) << endl;
cout << "if = 0 ? " << (r[0] * r[3] + r[1] * r[4] + r[2] * r[5]) << endl;
cout << "if = 0 ? " << (r[0] * r[6] + r[1] * r[7] + r[2] * r[8]) << endl;
cout << "if = 0 ? " << (r[3] * r[6] + r[4] * r[7] + r[5] * r[8]) << endl;

 

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章