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

rtabmap: scan_cloud_normal_k and scan_normal_radius removed. odometry: added guess_min_translation, guess_min_rotation and scan_voxel_size parameters. data_recorder.launch: updated to support rgbd_image and scan_cloud inputs.

Browse Source
This commit is contained in:
matlabbe
2017-09-19 14:27:41 -04:00
parent ea6f39eb73
commit a0e2596cd2
12 changed files with 237 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+6 -43
View File
@@ -1360,9 +1360,7 @@ bool convertScanMsg(
cv::Mat & scan,
rtabmap::Transform & scanLocalTransform,
tf::TransformListener & listener,
double waitForTransform,
int scanCloudNormalK,
float scanCloudNormalRadius)
double waitForTransform)
{
// make sure the frame of the laser is updated too
rtabmap::Transform tmpT = getTransform(
@@ -1393,6 +1391,7 @@ bool convertScanMsg(
projection.transformLaserScanToPointCloud(odomFrameId.empty()?frameId:odomFrameId, *scan2dMsg, scanOut, listener);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true;
//transform back in laser frame
rtabmap::Transform laserToOdom = getTransform(
@@ -1428,19 +1427,7 @@ bool convertScanMsg(
}
}
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK, scanCloudNormalRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScanNormal, laserToOdom); // put back in laser frame
}
else
{
scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
}
scan = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame
return true;
}
@@ -1453,9 +1440,7 @@ bool convertScan3dMsg(
cv::Mat & scan,
rtabmap::Transform & scanLocalTransform,
tf::TransformListener & listener,
double waitForTransform,
int scanCloudNormalK,
float scanCloudNormalRadius)
double waitForTransform)
{
bool containNormals = false;
bool containColors = false;
@@ -1533,18 +1518,7 @@ bool convertScan3dMsg(
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
}
else
{
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
else
{
@@ -1555,18 +1529,7 @@ bool convertScan3dMsg(
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK > 0 || scanCloudNormalRadius>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclScan, scanCloudNormalK, scanCloudNormalRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = rtabmap::util3d::laserScanFromPointCloud(*rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScanNormal));
}
else
{
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
scan = rtabmap::util3d::laserScanFromPointCloud(*pclScan);
}
}
return true;