ICP变换矩阵平移和旋转

我有一辆带漂亮IMU和声纳的车。我正在用点云库线性ICP对声纳点云进行精细注册。我想将ICP变换的结果与IMU数据进行比较，但我不明白如何从最终的齐次4x4变换矩阵中提取平移。

我发现的一个类似的问题和其他来源都说，翻译只是表格中的第4栏。

我遇到的问题是，我得到的翻译值是不可能的，似乎旋转分量越大，得到的值就越荒谬，这让我相信我不能简单地提取最后一列。滚转、俯仰和偏航值都在合理范围内，而且是合理的，但在任何方向上都不可能有超过1米的偏移。矩阵在应用时确实按预期执行，所以我知道矩阵是正确的，我只是不明白如何解释或提取x，y，z线性平移。

测量原始云的质心和最终云之间的距离会得到更合理的结果，但我不知道这是否是一种可以接受的方法。这看起来有点古怪。

代码：

myCloud::Ptr      target, source, output;  // PCL clouds
myPoint           cInit, cRough, cFinal;   // centroid points
Eigen::Matrix4f   estimation, icpResult, finalTransform;   // transforms
// load vectors of sonar data into point clouds
target = pointVector_to_pointCloud(verbose, tgtPoints);
source = pointVector_to_pointCloud(verbose, srcPoints);
pcl::computeCentroid(*source, cInit);
// x, y, z offsets come from a previous rough alignment
Eigen::Affine3f fromIMU(Eigen::Translation3f(x, y, z));
estimation = fromIMU.matrix();
pcl::transformPointCloud(*cloud, *cloud, estimation);
pcl::computeCentroid(*source, cRough);
// create new empty cloud in the output pointer, set up ICP
output.reset(new myCloud);
icp.setInputSource(source);
icp.setInputTarget(target);
/**** Set ICP parameters, omitted ****/
icp.align(*output);
icpResult = icp.getFinalTransformation();
finalTransform = estimation * icpResult;
pcl::computeCentroid(*source, cFinal);
// Output Results
Eigen::Affine3f roughT(estimation);
Eigen::Affine3f fineT(icpResult);
float tx, ty, tz, rx, ry, rz;
pcl::getTranslationAndEulerAngles(roughT, tx, ty, tz, rx, ry, rz);
std::cerr << "********* ICP RESULTS **********n";
std::cerr << "Rough Transform Matrix:n" << transform << endl;
std::cerr << "Translation (x, y, z)       : " << tx << ", " << ty << ", " << tz << endl;
std::cerr << "Rotation (roll, pitch, yaw) : " << rx << ", " << ry << ", " << rz << endl;
pcl::getTranslationAndEulerAngles(fineT, tx, ty, tz, rx, ry, rz);
std::cerr << "nFine Transform Matrix:n" << icpResult << endl;
std::cerr << "Translation (x, y, z)       : " << tx << ", " << ty << ", " << tz << endl;
std::cerr << "Rotation (roll, pitch, yaw) : " << rx << ", " << ry << ", " << rz << endl << endl;
std::cerr << "nFinal Transformation Matrix:n" << finalTransform << endl;
std::cerr << "ntCentroid after Rough Alignment: " << cRough << " ... Distance From Start: " << pcl::geometry::distance(cInit, cRough) << endl;
std::cerr << "tCentroid after ICP: " << cFinal << " ... Distance From Start: " << pcl::geometry::distance(cInit, cFinal) << endl;

其输出(对于一个示例数据集(：

********* INSIDE ICP TRANSFORM STATS **********
Rough Transform Matrix:
1         0         0  0.612095
0         1         0 -0.211855
0         0         1         0
0         0         0         1
Translation (x, y, z)       : 0.612095, -0.211855, 0
Rotation (roll, pitch, yaw) : 0, -0, 0
Fine Transform Matrix:
0.999992 -0.00257317  0.00361636     2.92558
0.00256172    0.999995  0.00328003     2.66182
-0.00362478 -0.00327113    0.999988   0.0578782
0           0           0           1
Translation (x, y, z)       : 2.92558, 2.66182, 0.0578782
Rotation (roll, pitch, yaw) : -0.00327116, 0.00362479, 0.00256174
Final Transformation Matrix: 
0.999992 -0.00257317  0.00361636     3.53767
0.00256172    0.999995  0.00328003     2.44996
-0.00362478 -0.00327113    0.999988   0.0578782
0           0           0           1
Centroid after Rough Alignment: (8.8218,9.12704,-807.301 - 0,126,255) ... Distance From Start: 0.647709
Centroid after ICP: (8.8068,9.1658,-807.3 - 0,126,255) ... Distance From Start: 0.621667