旋轉矩陣的自驗證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;