ICP变换矩阵平移和旋转答案

【问题标题】：ICP transformation matrix translation and rotationICP变换矩阵平移和旋转
【发布时间】：2018-07-31 23:30:26
【问题描述】：

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

类似的question和other sources我发现都说翻译只是表格中的第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 << "\n\tCentroid 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

【问题讨论】：

可以接受什么？你不是在制定规则吗？平移将取决于点云的原点在哪里。如果它在外面很远，旋转部分已经将点云移动了很多。然后，翻译只会添加到这个动作中。使用质心听起来很合理。

标签： c++ math matrix point-cloud-library point-clouds

【解决方案1】：

这个问题的答案与数据的来源有关。测深数据以正数表示，即高于某一点的海水米。但收集数据的车辆在海床以上 10-30 米处飞行。

正确转换数据：

转换成车辆参考系
执行 ICP 并获取矩阵
如果需要，将数据转换回测深框架

这可能需要额外的车辆数据来了解如何转换为车架，例如传感器位置和车辆本身的偏移量。

转化为车辆参考系，

【讨论】：