i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 09:47:46 +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/nodelets/icp_odometry.cpp
+75 -33
View File
@@ -58,8 +58,9 @@ public:
ICPOdometry() :
OdometryROS(false, false, true),
scanCloudMaxPoints_(0),
scanCloudNormalK_(0),
scanCloudNormalRadius_(0.0f)
scanVoxelSize_(0.0f),
scanNormalK_(0),
scanNormalRadius_(0.0f)
{
}
@@ -75,14 +76,20 @@ private:
ros::NodeHandle & pnh = getPrivateNodeHandle();
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
}
pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
NODELET_INFO("IcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("IcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
NODELET_INFO("IcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("IcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
scan_sub_ = nh.subscribe("scan", 1, &ICPOdometry::callbackScan, this);
cloud_sub_ = nh.subscribe("scan_cloud", 1, &ICPOdometry::callbackCloud, this);
@@ -117,24 +124,44 @@ private:
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true;
cv::Mat scan;
if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
int maxLaserScans = (int)scanMsg->ranges.size();
if(pclScan->size())
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
maxLaserScans = int(float(maxLaserScans) * ratio);
}
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(scanVoxelSize_ > 0.0f)
{
normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
}
}
rtabmap::SensorData data(
scan,
LaserScanInfo((int)scanMsg->ranges.size(), scanMsg->range_max, localScanTransform),
LaserScanInfo(maxLaserScans, scanMsg->range_max, localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
@@ -148,12 +175,15 @@ private:
{
cv::Mat scan;
bool containNormals = false;
for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
if(scanVoxelSize_ == 0.0f)
{
if(cloudMsg->fields[i].name.compare("normal_x") == 0)
for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
{
containNormals = true;
break;
if(cloudMsg->fields[i].name.compare("normal_x") == 0)
{
containNormals = true;
break;
}
}
}
@@ -164,6 +194,7 @@ private:
return;
}
int maxLaserScans = scanCloudMaxPoints_;
if(containNormals)
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
@@ -183,23 +214,33 @@ private:
pclScan = util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
if(pclScan->size())
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
maxLaserScans = int(float(maxLaserScans) * ratio);
}
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
}
}
}
rtabmap::SensorData data(
scan,
LaserScanInfo(scanCloudMaxPoints_, 0, localScanTransform),
LaserScanInfo(maxLaserScans, 0, localScanTransform),
cv::Mat(),
cv::Mat(),
CameraModel(),
@@ -219,8 +260,9 @@ private:
ros::Subscriber scan_sub_;
ros::Subscriber cloud_sub_;
int scanCloudMaxPoints_;
int scanCloudNormalK_;
float scanCloudNormalRadius_;
float scanVoxelSize_;
int scanNormalK_;
float scanNormalRadius_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ICPOdometry, nodelet::Nodelet);
src/nodelets/rgbd_odometry.cpp
+5
View File
@@ -117,6 +117,11 @@ private:
NODELET_FATAL("Only 2 cameras maximum supported yet.");
}
NODELET_INFO("RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
NODELET_INFO("RGBDOdometry: queue_size = %d", queueSize_);
NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
src/nodelets/rgbdicp_odometry.cpp
+78 -42
View File
@@ -74,8 +74,9 @@ public:
exactCloudSync_(0),
queueSize_(5),
scanCloudMaxPoints_(0),
scanCloudNormalK_(0),
scanCloudNormalRadius_(0.0f)
scanVoxelSize_(0.0f),
scanNormalK_(0),
scanNormalRadius_(0.0f)
{
}
@@ -112,14 +113,23 @@ private:
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
pnh.param("scan_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
{
ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
"The value is still used. Use \"scan_normal_k\" to avoid this warning.");
pnh.param("scan_cloud_normal_k", scanCloudNormalK_, scanCloudNormalK_);
pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
}
pnh.param("scan_normal_radius", scanCloudNormalRadius_, scanCloudNormalRadius_);
pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_);
NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
NODELET_INFO("RGBDIcpOdometry: scan_normal_k = %d", scanNormalK_);
NODELET_INFO("RGBDIcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
@@ -193,16 +203,6 @@ private:
uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
}
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
sensor_msgs::LaserScanConstPtr scanMsg;
sensor_msgs::PointCloud2ConstPtr cloudMsg;
callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
}
void callbackScan(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
@@ -282,6 +282,7 @@ private:
cv::Mat scan;
Transform localScanTransform = Transform::getIdentity();
int maxLaserScans = 0;
if(scanMsg.get() != 0)
{
// make sure the frame of the laser is updated too
@@ -300,29 +301,52 @@ private:
projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true;
if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
maxLaserScans = (int)scanMsg->ranges.size();
if(pclScan->size())
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeFastOrganizedNormals2D(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
maxLaserScans = int(float(maxLaserScans) * ratio);
}
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals;
if(scanVoxelSize_ > 0.0f)
{
normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
else
{
normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScan2dFromPointCloud(*pclScan);
}
}
}
else if(cloudMsg.get() != 0)
{
bool containNormals = false;
for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
if(scanVoxelSize_ == 0.0f)
{
if(cloudMsg->fields[i].name.compare("normal_x") == 0)
for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
{
containNormals = true;
break;
if(cloudMsg->fields[i].name.compare("normal_x") == 0)
{
containNormals = true;
break;
}
}
}
localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
@@ -332,6 +356,7 @@ private:
return;
}
maxLaserScans = scanCloudMaxPoints_;
if(containNormals)
{
pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
@@ -351,17 +376,27 @@ private:
pclScan = util3d::removeNaNFromPointCloud(pclScan);
}
if(scanCloudNormalK_ > 0 || scanCloudNormalRadius_>0.0f)
if(pclScan->size())
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_, scanCloudNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
if(scanVoxelSize_ > 0.0f)
{
float pointsBeforeFiltering = (float)pclScan->size();
pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
float ratio = float(pclScan->size()) / pointsBeforeFiltering;
maxLaserScans = int(float(maxLaserScans) * ratio);
}
if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
{
//compute normals
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
}
else
{
scan = util3d::laserScanFromPointCloud(*pclScan);
}
}
}
}
@@ -369,7 +404,7 @@ private:
rtabmap::SensorData data(
scan,
LaserScanInfo(
scanMsg.get() != 0?(int)scanMsg->ranges.size():cloudMsg.get() != 0?scanCloudMaxPoints_:0,
scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
scanMsg.get() != 0?scanMsg->range_max:0,
localScanTransform),
ptrImage->image,
@@ -429,8 +464,9 @@ private:
message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
int queueSize_;
int scanCloudMaxPoints_;
int scanCloudNormalK_;
float scanCloudNormalRadius_;
float scanVoxelSize_;
int scanNormalK_;
float scanNormalRadius_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet);
src/nodelets/stereo_odometry.cpp
+3
View File
@@ -88,6 +88,9 @@ private:
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_);
NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");