PCL - ICP代碼研讀(八 ) - transformCloud函數
前言
如PCL - ICP代碼研讀(六 ) - IterativeClosestPoint架構中所介紹的,transformCloud
函數用於對輸入點雲的XYZ坐標及法向量做剛體變換。
本篇中介紹的是IterativeClosestPoint::transformCloud
,icp.hpp
中還另外定義了一個IterativeClosestPointWithNormals::transformCloud
函數,在此略過不述。
transformCloud
transformCloud
函數會對輸入點雲input
使用transform
的4*4矩陣做轉換,然後輸出到output
點雲。這個函數在computeTransform
函數中多次被用到。
template <typename PointSource, typename PointTarget, typename Scalar>
void
IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud(
const PointCloudSource& input, PointCloudSource& output, const Matrix4& transform)
{
pt
表示一個三維點,pt_t
表示轉換後的三維點,兩者都是使用齊次表示法。
Eigen::Vector4f pt(0.0f, 0.0f, 0.0f, 1.0f), pt_t;
4*4變換矩陣:
// 4*4的變換矩陣
Eigen::Matrix4f tr = transform.template cast<float>();
關於template cast
可參考.template cast<>中template的作用。
在source點雲帶有法向量的情況下,執行if分支:
//?
// XYZ is ALWAYS present due to the templatization, so we only have to check for
// normals
if (source_has_normals_) {
事先宣告nt
為法向量,nt_t
為轉換後的法向量:
Eigen::Vector3f nt, nt_t;
變換矩陣的左上角為旋轉矩陣:
Eigen::Matrix3f rot = tr.block<3, 3>(0, 0);
看不太懂這邊轉換為std::uint8_t
的用意:
for (std::size_t i = 0; i < input.size(); ++i) {
// 把input點雲的第i個元素的"位址"轉成std::uint8_t*的data_in
// 這裡為什麼cast成uint8_t?
// std::uint8_t為8bit
const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
std::uint8_t* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
把data_in
(也就是input[i]
)中的XYZ坐標複製到pt
:
// 然後再把data_in複製到pt -> 為什麼要用memcpy不直接用=?
/**
* 點雲底層的資料型別一定是float?
* 查看Registration類別的模板:
* template <typename PointSource, typename PointTarget, typename Scalar = float>
* 底層資料型別可以是float或double,
* 感覺這邊要改成sizeof(Scalar)然後轉成float才對?
**/
memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
continue;
使用變換矩陣tr
對pt
做旋轉及平移,得到旋轉平移後的法向量pt_t
:
pt_t = tr * pt;
把pt_t
的內容複製到(也就是output[i]
)的XYZ坐標欄位:
// 由pt_t複製到data_out
memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
把data_in
(也就是input[i]
)中的法向量複製到nt
:
// 將法向量由data_in複製到nt
memcpy(&nt[0], data_in + nx_idx_offset_, sizeof(float));
memcpy(&nt[1], data_in + ny_idx_offset_, sizeof(float));
memcpy(&nt[2], data_in + nz_idx_offset_, sizeof(float));
if (!std::isfinite(nt[0]) || !std::isfinite(nt[1]) || !std::isfinite(nt[2]))
continue;
使用旋轉矩陣rot
對nt
做旋轉,得到旋轉後的法向量nt_t
(注意:不需要對法向量做平移):
// 對法向量只做旋轉,不做平移(是用rot左乘而非tr)
nt_t = rot * nt;
把nt_t
的內容複製到data_out
(也就是output[i]
)的法向量欄位:
// 將法向量由nt_t複製到data_out
memcpy(data_out + nx_idx_offset_, &nt_t[0], sizeof(float));
memcpy(data_out + ny_idx_offset_, &nt_t[1], sizeof(float));
memcpy(data_out + nz_idx_offset_, &nt_t[2], sizeof(float));
}
}
else分支的代碼與if分支的代碼大同小異:
else {
// 這段代碼與上面的重複了,應該可以只寫一次
for (std::size_t i = 0; i < input.size(); ++i) {
const std::uint8_t* data_in = reinterpret_cast<const std::uint8_t*>(&input[i]);
std::uint8_t* data_out = reinterpret_cast<std::uint8_t*>(&output[i]);
memcpy(&pt[0], data_in + x_idx_offset_, sizeof(float));
memcpy(&pt[1], data_in + y_idx_offset_, sizeof(float));
memcpy(&pt[2], data_in + z_idx_offset_, sizeof(float));
if (!std::isfinite(pt[0]) || !std::isfinite(pt[1]) || !std::isfinite(pt[2]))
continue;
pt_t = tr * pt;
memcpy(data_out + x_idx_offset_, &pt_t[0], sizeof(float));
memcpy(data_out + y_idx_offset_, &pt_t[1], sizeof(float));
memcpy(data_out + z_idx_offset_, &pt_t[2], sizeof(float));
}
}
}