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

Updated Info msg: changed localLoopClosureId for proximityDetectionId. point_cloud_xyzrgb nodelet: handling bgr and rgb encoding. Updated demo_find_object.launch with 0.11. MapsManager: Fixed grids generated with only one point.

Browse Source
This commit is contained in:
matlabbe
2016-03-11 20:16:52 -05:00
parent f0d91c5eac
commit f26c8b55eb
7 changed files with 75 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
launch/demo/demo_find_object.launch
+35 -17
View File
@@ -1,6 +1,10 @@
<launch> <launch>
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<param name="use_sim_time" type="bool" value="True"/> <param name="use_sim_time" type="bool" value="True"/>
<!-- SLAM (robot side) --> <!-- SLAM (robot side) -->
@@ -10,7 +14,7 @@
<param name="frame_id" type="string" value="base_footprint"/> <param name="frame_id" type="string" value="base_footprint"/>
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="true"/> <param name="subscribe_scan" type="bool" value="true"/>
<remap from="odom" to="/base_controller/odom"/> <remap from="odom" to="/base_controller/odom"/>
<remap from="scan" to="/base_scan"/> <remap from="scan" to="/base_scan"/>
@@ -25,24 +29,39 @@
<param name="queue_size" type="int" value="10"/> <param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. --> <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/PoseScanMatching" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans --> <param name="RGBD/NeighborLinkRefining" type="string" value="true"/> <!-- Do odometry correction with consecutive laser scans -->
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/> <!-- Local loop closure detection (using estimated position) with locations in WM --> <param name="RGBD/ProximityBySpace" type="string" value="true"/> <!-- Proximity detection (using estimated position) with locations in WM -->
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/> <!-- Local loop closure detection with locations in STM --> <param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) -->
<param name="Mem/BadSignaturesIgnored" type="string" value="false"/> <!-- Don't ignore bad images for 3D node creation (e.g. white walls) --> <param name="Reg/Strategy" type="string" value="1"/> <!-- Registration strategy: 0=visual, 1=ICP, 2=visual+ICP -->
<param name="LccIcp/Type" type="string" value="2"/> <!-- Loop closure transformation refining with ICP: 0=No ICP, 1=ICP 3D, 2=ICP 2D --> <param name="Icp/CorrespondenceRatio" type="string" value="0.3"/>
<param name="LccIcp2/CorrespondenceRatio" type="string" value="0.9"/> <param name="Icp/Iterations" type="string" value="30"/>
<param name="LccIcp2/MaxFitness" type="string" value="0.1"/> <param name="Icp/VoxelSize" type="string" value="0.025"/>
<param name="LccIcp2/Iterations" type="string" value="100"/> <param name="Vis/MinInliers" type="string" value="10"/> <!-- 3D visual words minimum inliers to accept loop closure -->
<param name="LccIcp2/VoxelSize" type="string" value="0"/> <param name="Vis/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/MinInliers" type="string" value="5"/> <!-- 3D visual words minimum inliers to accept loop closure --> <param name="Vis/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="LccBow/MaxDepth" type="string" value="4.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving --> <param name="RGBD/AngularUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving --> <param name="RGBD/LinearUpdate" type="string" value="0.01"/> <!-- Update map only if the robot is moving -->
<param name="Rtabmap/TimeThr" type="string" value="700"/> <param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/> <param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/> <param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
</node> </node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_footprint"/>
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
<remap from="scan" to="/base_scan"/>
<remap from="odom" to="/base_controller/odom"/>
<param name="rgb/image_transport" type="string" value="compressed"/>
<param name="depth/image_transport" type="string" value="compressedDepth"/>
</node>
</group> </group>
<!-- send AZIMUT 3 urdf to param server --> <!-- send AZIMUT 3 urdf to param server -->
@@ -51,10 +70,9 @@
--> -->
<!-- Visualisation --> <!-- Visualisation -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/> <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/> <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="/camera/data_throttled_image"/> <remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/> <remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/> <remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
msg/Info.msg
+1 -1
View File
@@ -7,7 +7,7 @@ Header header
int32 refId int32 refId
int32 loopClosureId int32 loopClosureId
int32 localLoopClosureId int32 proximityDetectionId
geometry_msgs/Transform loopClosureTransform geometry_msgs/Transform loopClosureTransform
src/MapsManager.cpp
+13 -1
View File
@@ -39,6 +39,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
cloudFrustumCulling_(false), cloudFrustumCulling_(false),
cloudNoiseFilteringRadius_(0.0), cloudNoiseFilteringRadius_(0.0),
cloudNoiseFilteringMinNeighbors_(5), cloudNoiseFilteringMinNeighbors_(5),
scanDecimation_(0),
scanVoxelSize_(0.0), scanVoxelSize_(0.0),
scanOutputVoxelized_(false), scanOutputVoxelized_(false),
projMaxGroundAngle_(45.0), // degrees projMaxGroundAngle_(45.0), // degrees
@@ -48,6 +49,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
gridSize_(0), // meters gridSize_(0), // meters
gridEroded_(false), gridEroded_(false),
gridUnknownSpaceFilled_(false), gridUnknownSpaceFilled_(false),
gridMaxUnknownSpaceFilledRange_(6.0),
mapFilterRadius_(0.5), mapFilterRadius_(0.5),
mapFilterAngle_(30.0), // degrees mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true) mapCacheCleanup_(true)
@@ -89,6 +91,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
pnh.param("grid_size", gridSize_, gridSize_); // m pnh.param("grid_size", gridSize_, gridSize_); // m
pnh.param("grid_eroded", gridEroded_, gridEroded_); pnh.param("grid_eroded", gridEroded_, gridEroded_);
pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_); pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
// common map stuff // common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
@@ -358,6 +361,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
{ {
scan = util3d::downsample(scan, scanDecimation_); scan = util3d::downsample(scan, scanDecimation_);
} }
if(scanRequired || scanVoxelSize_ > 0.0) if(scanRequired || scanVoxelSize_ > 0.0)
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan); pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
@@ -369,16 +373,24 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
scan = util3d::laserScan2dFromPointCloud(*scanCloud); scan = util3d::laserScan2dFromPointCloud(*scanCloud);
} }
} }
if(scanRequired) if(scanRequired)
{ {
uInsert(scans_, std::make_pair(iter->first, scanCloud)); uInsert(scans_, std::make_pair(iter->first, scanCloud));
} }
} }
} }
if(gridRequired && scan.type() == CV_32FC2) if(gridRequired && scan.type() == CV_32FC2)
{ {
cv::Mat ground, obstacles; cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange()); util3d::occupancy2DFromLaserScan(
scan,
ground,
obstacles,
gridCellSize_,
data.id() < 0 || gridUnknownSpaceFilled_,
data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange());
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles))); uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
} }
} }
src/MapsManager.h
+1
View File
@@ -85,6 +85,7 @@ private:
double gridSize_; double gridSize_;
bool gridEroded_; bool gridEroded_;
bool gridUnknownSpaceFilled_; bool gridUnknownSpaceFilled_;
double gridMaxUnknownSpaceFilledRange_;
double mapFilterRadius_; double mapFilterRadius_;
double mapFilterAngle_; double mapFilterAngle_;
bool mapCacheCleanup_; bool mapCacheCleanup_;
src/MsgConversion.cpp
+2 -2
View File
@@ -146,7 +146,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
// rtabmap_ros::Info // rtabmap_ros::Info
stat.setRefImageId(info.refId); stat.setRefImageId(info.refId);
stat.setLoopClosureId(info.loopClosureId); stat.setLoopClosureId(info.loopClosureId);
stat.setLocalLoopClosureId(info.localLoopClosureId); stat.setProximityDetectionId(info.proximityDetectionId);
stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform)); stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
@@ -190,7 +190,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
{ {
info.refId = stats.refImageId(); info.refId = stats.refImageId();
info.loopClosureId = stats.loopClosureId(); info.loopClosureId = stats.loopClosureId();
info.localLoopClosureId = stats.localLoopClosureId(); info.proximityDetectionId = stats.proximityDetectionId();
rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform); rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
src/nodelets/point_cloud_xyzrgb.cpp
+15 -1
View File
@@ -173,7 +173,21 @@ private:
if(cloudPub_.getNumSubscribers()) if(cloudPub_.getNumSubscribers())
{ {
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image); cv_bridge::CvImageConstPtr imagePtr;
if(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
imagePtr = cv_bridge::toCvShare(image);
}
else if(image->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
imagePtr = cv_bridge::toCvShare(image, "mono8");
}
else
{
imagePtr = cv_bridge::toCvShare(image, "bgr8");
}
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model; image_geometry::PinholeCameraModel model;
src/rviz/InfoDisplay.cpp
+7 -7
View File
@@ -51,8 +51,8 @@ void InfoDisplay::onInitialize()
this->setStatusStd(rviz::StatusProperty::Ok, "Info", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Info", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", ""); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", "");
this->setStatusStd(rviz::StatusProperty::Ok, "Global", "0"); this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", "0");
this->setStatusStd(rviz::StatusProperty::Ok, "Local", "0"); this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", "0");
spinner_.start(); spinner_.start();
} }
@@ -63,12 +63,12 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
boost::mutex::scoped_lock lock(info_mutex_); boost::mutex::scoped_lock lock(info_mutex_);
if(msg->loopClosureId) if(msg->loopClosureId)
{ {
info_ = QString("%1->%2 [Global]").arg(msg->refId).arg(msg->loopClosureId); info_ = QString("%1->%2").arg(msg->refId).arg(msg->loopClosureId);
globalCount_ += 1; globalCount_ += 1;
} }
else if(msg->localLoopClosureId) else if(msg->proximityDetectionId)
{ {
info_ = QString("%1->%2 [Local]").arg(msg->refId).arg(msg->localLoopClosureId); info_ = QString("%1->%2 [Proximity]").arg(msg->refId).arg(msg->proximityDetectionId);
localCount_ += 1; localCount_ += 1;
} }
else else
@@ -103,8 +103,8 @@ void InfoDisplay::update( float wall_dt, float ros_dt )
this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString()); this->setStatusStd(rviz::StatusProperty::Ok, "Position (XYZ)", tr("%1;%2;%3").arg(x).arg(y).arg(z).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString()); this->setStatusStd(rviz::StatusProperty::Ok, "Orientation (RPY)", tr("%1;%2;%3").arg(roll).arg(pitch).arg(yaw).toStdString());
} }
this->setStatusStd(rviz::StatusProperty::Ok, "Global", tr("%1").arg(globalCount_).toStdString()); this->setStatusStd(rviz::StatusProperty::Ok, "Loop closures", tr("%1").arg(globalCount_).toStdString());
this->setStatusStd(rviz::StatusProperty::Ok, "Local", tr("%1").arg(localCount_).toStdString()); this->setStatusStd(rviz::StatusProperty::Ok, "Proximity detections", tr("%1").arg(localCount_).toStdString());
for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter) for(std::map<std::string, float>::const_iterator iter=statistics_.begin(); iter!=statistics_.end(); ++iter)
{ {