i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

OdomF2M: added support to laser scan

Browse Source
This commit is contained in:
matlabbe
2016-03-06 15:11:09 -05:00
parent eefd557ab4
commit 7a1cf84b08
22 changed files with 696 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_transforms.cpp
+17
View File
@@ -90,6 +90,23 @@ pcl::PointXYZRGB transformPoint(
{
return pcl::transformPoint(pt, transform.toEigen3f());
}
pcl::PointNormal transformPoint(
const pcl::PointNormal & point,
const Transform & transform)
{
pcl::PointNormal ret;
Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
// Rotate normals
Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
return ret;
}
}