ICP变换矩阵平移和旋转
ICP变换矩阵平移和旋转
提问于 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回答 1
Stack Overflow用户
回答已采纳
发布于 2019-07-13 17:06:25
这个问题的答案与数据的来源有关。水深测量数据表示为正数，海水水表高于一个点。但是收集数据的车辆是在离海底10-30米的高空飞行。
要正确转换数据：
- 转换成车辆参考架
- 执行ICP并获得矩阵
- 如果需要，将数据转换回测深框架
这可能需要额外的车辆数据，以了解如何转换为车辆框架，例如传感器位置和车辆本身的偏移。
转换成车辆参考架，
页面原文内容由Stack Overflow提供。腾讯云小微IT领域专用引擎提供翻译支持
原文链接：
https://stackoverflow.com/questions/51623436
复制相关文章
相似问题
腾讯云开发者
