i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added util3d::laserScan2dFromPointCloud()

Browse Source
This commit is contained in:
matlabbe
2015-11-26 14:34:06 -05:00
parent ba6733411a
commit 991a603648
2 changed files with 26 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
corelib/src/util3d.cpp
View File

@@ -815,6 +815,29 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
return laserScan;
}
cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
{
cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(unsigned int i=0; i<cloud.size(); ++i)
{
if(!nullTransform)
{
pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
laserScan.at<cv::Vec2f>(i)[0] = pt.x;
laserScan.at<cv::Vec2f>(i)[1] = pt.y;
}
else
{
laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
}
}
return laserScan;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
{
UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3);