00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap_ros/OdometryROS.h>
00029
00030 #include <pluginlib/class_list_macros.h>
00031 #include <nodelet/nodelet.h>
00032
00033 #include <message_filters/subscriber.h>
00034 #include <message_filters/time_synchronizer.h>
00035 #include <message_filters/sync_policies/approximate_time.h>
00036
00037 #include <image_transport/image_transport.h>
00038 #include <image_transport/subscriber_filter.h>
00039
00040 #include <image_geometry/stereo_camera_model.h>
00041 #include <laser_geometry/laser_geometry.h>
00042
00043 #include <sensor_msgs/Image.h>
00044 #include <sensor_msgs/image_encodings.h>
00045 #include <cv_bridge/cv_bridge.h>
00046 #include <sensor_msgs/LaserScan.h>
00047 #include <sensor_msgs/PointCloud2.h>
00048 #include <pcl_conversions/pcl_conversions.h>
00049
00050 #include "rtabmap_ros/MsgConversion.h"
00051
00052 #include <rtabmap/core/util3d.h>
00053 #include <rtabmap/core/util3d_surface.h>
00054 #include <rtabmap/core/util3d_transforms.h>
00055 #include <rtabmap/core/util3d_filtering.h>
00056 #include <rtabmap/core/util2d.h>
00057 #include <rtabmap/utilite/ULogger.h>
00058 #include <rtabmap/utilite/UConversion.h>
00059 #include <rtabmap/utilite/UStl.h>
00060
00061 using namespace rtabmap;
00062
00063 namespace rtabmap_ros
00064 {
00065
00066 class RGBDICPOdometry : public rtabmap_ros::OdometryROS
00067 {
00068 public:
00069 RGBDICPOdometry() :
00070 OdometryROS(false, true, true),
00071 approxScanSync_(0),
00072 exactScanSync_(0),
00073 approxCloudSync_(0),
00074 exactCloudSync_(0),
00075 queueSize_(5),
00076 scanCloudMaxPoints_(0),
00077 scanVoxelSize_(0.0),
00078 scanNormalK_(0),
00079 scanNormalRadius_(0.0)
00080 {
00081 }
00082
00083 virtual ~RGBDICPOdometry()
00084 {
00085 if(approxScanSync_)
00086 {
00087 delete approxScanSync_;
00088 }
00089 if(exactScanSync_)
00090 {
00091 delete exactScanSync_;
00092 }
00093 if(approxCloudSync_)
00094 {
00095 delete approxCloudSync_;
00096 }
00097 if(exactCloudSync_)
00098 {
00099 delete exactCloudSync_;
00100 }
00101 }
00102
00103 private:
00104
00105 virtual void onOdomInit()
00106 {
00107 ros::NodeHandle & nh = getNodeHandle();
00108 ros::NodeHandle & pnh = getPrivateNodeHandle();
00109
00110 bool approxSync = true;
00111 bool subscribeScanCloud = false;
00112 pnh.param("approx_sync", approxSync, approxSync);
00113 pnh.param("queue_size", queueSize_, queueSize_);
00114 pnh.param("subscribe_scan_cloud", subscribeScanCloud, subscribeScanCloud);
00115 pnh.param("scan_cloud_max_points", scanCloudMaxPoints_, scanCloudMaxPoints_);
00116 pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
00117 pnh.param("scan_normal_k", scanNormalK_, scanNormalK_);
00118 if(pnh.hasParam("scan_cloud_normal_k") && !pnh.hasParam("scan_normal_k"))
00119 {
00120 ROS_WARN("rtabmap: Parameter \"scan_cloud_normal_k\" has been renamed to \"scan_normal_k\". "
00121 "The value is still used. Use \"scan_normal_k\" to avoid this warning.");
00122 pnh.param("scan_cloud_normal_k", scanNormalK_, scanNormalK_);
00123 }
00124 pnh.param("scan_normal_radius", scanNormalRadius_, scanNormalRadius_);
00125
00126 NODELET_INFO("RGBDIcpOdometry: approx_sync = %s", approxSync?"true":"false");
00127 NODELET_INFO("RGBDIcpOdometry: queue_size = %d", queueSize_);
00128 NODELET_INFO("RGBDIcpOdometry: subscribe_scan_cloud = %s", subscribeScanCloud?"true":"false");
00129 NODELET_INFO("RGBDIcpOdometry: scan_cloud_max_points = %d", scanCloudMaxPoints_);
00130 NODELET_INFO("RGBDIcpOdometry: scan_voxel_size = %f", scanVoxelSize_);
00131 NODELET_INFO("RGBDIcpOdometry: scan_normal_k = %d", scanNormalK_);
00132 NODELET_INFO("RGBDIcpOdometry: scan_normal_radius = %f", scanNormalRadius_);
00133
00134 ros::NodeHandle rgb_nh(nh, "rgb");
00135 ros::NodeHandle depth_nh(nh, "depth");
00136 ros::NodeHandle rgb_pnh(pnh, "rgb");
00137 ros::NodeHandle depth_pnh(pnh, "depth");
00138 image_transport::ImageTransport rgb_it(rgb_nh);
00139 image_transport::ImageTransport depth_it(depth_nh);
00140 image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00141 image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00142
00143 image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00144 image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00145 info_sub_.subscribe(rgb_nh, "camera_info", 1);
00146
00147 std::string subscribedTopicsMsg;
00148 if(subscribeScanCloud)
00149 {
00150 cloud_sub_.subscribe(nh, "scan_cloud", 1);
00151 if(approxSync)
00152 {
00153 approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
00154 approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
00155 }
00156 else
00157 {
00158 exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
00159 exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
00160 }
00161
00162 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
00163 getName().c_str(),
00164 approxSync?"approx":"exact",
00165 image_mono_sub_.getTopic().c_str(),
00166 image_depth_sub_.getTopic().c_str(),
00167 info_sub_.getTopic().c_str(),
00168 cloud_sub_.getTopic().c_str());
00169 }
00170 else
00171 {
00172 scan_sub_.subscribe(nh, "scan", 1);
00173 if(approxSync)
00174 {
00175 approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
00176 approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
00177 }
00178 else
00179 {
00180 exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
00181 exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
00182 }
00183
00184 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
00185 getName().c_str(),
00186 approxSync?"approx":"exact",
00187 image_mono_sub_.getTopic().c_str(),
00188 image_depth_sub_.getTopic().c_str(),
00189 info_sub_.getTopic().c_str(),
00190 scan_sub_.getTopic().c_str());
00191 }
00192 this->startWarningThread(subscribedTopicsMsg, approxSync);
00193 }
00194
00195 virtual void updateParameters(ParametersMap & parameters)
00196 {
00197
00198 ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
00199 if(iter != parameters.end() && iter->second.compare("0") != 0)
00200 {
00201 ROS_WARN("RGBDICP odometry works only with \"Reg/Strategy\"=2. Ignoring value %s.", iter->second.c_str());
00202 }
00203 uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
00204 }
00205
00206 void callbackScan(
00207 const sensor_msgs::ImageConstPtr& image,
00208 const sensor_msgs::ImageConstPtr& depth,
00209 const sensor_msgs::CameraInfoConstPtr& cameraInfo,
00210 const sensor_msgs::LaserScanConstPtr& scanMsg)
00211 {
00212 sensor_msgs::PointCloud2ConstPtr cloudMsg;
00213 callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
00214 }
00215
00216 void callbackCloud(
00217 const sensor_msgs::ImageConstPtr& image,
00218 const sensor_msgs::ImageConstPtr& depth,
00219 const sensor_msgs::CameraInfoConstPtr& cameraInfo,
00220 const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
00221 {
00222 sensor_msgs::LaserScanConstPtr scanMsg;
00223 callbackCommon(image, depth, cameraInfo, scanMsg, cloudMsg);
00224 }
00225
00226 void callbackCommon(
00227 const sensor_msgs::ImageConstPtr& image,
00228 const sensor_msgs::ImageConstPtr& depth,
00229 const sensor_msgs::CameraInfoConstPtr& cameraInfo,
00230 const sensor_msgs::LaserScanConstPtr& scanMsg,
00231 const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
00232 {
00233 callbackCalled();
00234 if(!this->isPaused())
00235 {
00236 if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
00237 image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00238 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00239 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00240 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
00241 image->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
00242 image->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
00243 image->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
00244 !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
00245 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
00246 depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
00247 {
00248 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
00249 "recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
00250 image->encoding.c_str(), depth->encoding.c_str());
00251 return;
00252 }
00253
00254
00255 ros::Time stamp = image->header.stamp > depth->header.stamp? image->header.stamp : depth->header.stamp;
00256 if(scanMsg.get() != 0)
00257 {
00258 if(stamp < scanMsg->header.stamp)
00259 {
00260 stamp = scanMsg->header.stamp;
00261 }
00262 }
00263 else if(cloudMsg.get() != 0)
00264 {
00265 if(stamp < cloudMsg->header.stamp)
00266 {
00267 stamp = cloudMsg->header.stamp;
00268 }
00269 }
00270
00271 Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
00272 if(localTransform.isNull())
00273 {
00274 return;
00275 }
00276
00277 if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
00278 {
00279 rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
00280 cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
00281 cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
00282
00283 cv::Mat scan;
00284 Transform localScanTransform = Transform::getIdentity();
00285 int maxLaserScans = 0;
00286 if(scanMsg.get() != 0)
00287 {
00288
00289 localScanTransform = getTransform(this->frameId(),
00290 scanMsg->header.frame_id,
00291 scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment));
00292 if(localScanTransform.isNull())
00293 {
00294 ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting odometry update.", scanMsg->header.stamp.toSec());
00295 return;
00296 }
00297
00298
00299 sensor_msgs::PointCloud2 scanOut;
00300 laser_geometry::LaserProjection projection;
00301 projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
00302 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00303 pcl::fromROSMsg(scanOut, *pclScan);
00304 pclScan->is_dense = true;
00305
00306 maxLaserScans = (int)scanMsg->ranges.size();
00307 if(pclScan->size())
00308 {
00309 if(scanVoxelSize_ > 0.0f)
00310 {
00311 float pointsBeforeFiltering = (float)pclScan->size();
00312 pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
00313 float ratio = float(pclScan->size()) / pointsBeforeFiltering;
00314 maxLaserScans = int(float(maxLaserScans) * ratio);
00315 }
00316 if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
00317 {
00318
00319 pcl::PointCloud<pcl::Normal>::Ptr normals;
00320 if(scanVoxelSize_ > 0.0f)
00321 {
00322 normals = util3d::computeNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
00323 }
00324 else
00325 {
00326 normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
00327 }
00328 pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
00329 pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
00330 scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
00331 }
00332 else
00333 {
00334 scan = util3d::laserScan2dFromPointCloud(*pclScan);
00335 }
00336 }
00337 }
00338 else if(cloudMsg.get() != 0)
00339 {
00340 bool containNormals = false;
00341 if(scanVoxelSize_ == 0.0f)
00342 {
00343 for(unsigned int i=0; i<cloudMsg->fields.size(); ++i)
00344 {
00345 if(cloudMsg->fields[i].name.compare("normal_x") == 0)
00346 {
00347 containNormals = true;
00348 break;
00349 }
00350 }
00351 }
00352 localScanTransform = getTransform(this->frameId(), cloudMsg->header.frame_id, cloudMsg->header.stamp);
00353 if(localScanTransform.isNull())
00354 {
00355 ROS_ERROR("TF of received scan cloud at time %fs is not set, aborting rtabmap update.", cloudMsg->header.stamp.toSec());
00356 return;
00357 }
00358
00359 maxLaserScans = scanCloudMaxPoints_;
00360 if(containNormals)
00361 {
00362 pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
00363 pcl::fromROSMsg(*cloudMsg, *pclScan);
00364 if(!pclScan->is_dense)
00365 {
00366 pclScan = util3d::removeNaNNormalsFromPointCloud(pclScan);
00367 }
00368 scan = util3d::laserScanFromPointCloud(*pclScan);
00369 }
00370 else
00371 {
00372 pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
00373 pcl::fromROSMsg(*cloudMsg, *pclScan);
00374 if(!pclScan->is_dense)
00375 {
00376 pclScan = util3d::removeNaNFromPointCloud(pclScan);
00377 }
00378
00379 if(pclScan->size())
00380 {
00381 if(scanVoxelSize_ > 0.0f)
00382 {
00383 float pointsBeforeFiltering = (float)pclScan->size();
00384 pclScan = util3d::voxelize(pclScan, scanVoxelSize_);
00385 float ratio = float(pclScan->size()) / pointsBeforeFiltering;
00386 maxLaserScans = int(float(maxLaserScans) * ratio);
00387 }
00388 if(scanNormalK_ > 0 || scanNormalRadius_>0.0f)
00389 {
00390
00391 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
00392 pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
00393 pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
00394 scan = util3d::laserScanFromPointCloud(*pclScanNormal);
00395 }
00396 else
00397 {
00398 scan = util3d::laserScanFromPointCloud(*pclScan);
00399 }
00400 }
00401 }
00402 }
00403
00404 rtabmap::SensorData data(
00405 LaserScan::backwardCompatibility(scan,
00406 scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
00407 scanMsg.get() != 0?scanMsg->range_max:0,
00408 localScanTransform),
00409 ptrImage->image,
00410 ptrDepth->image,
00411 rtabmapModel,
00412 0,
00413 rtabmap_ros::timestampFromROS(stamp));
00414
00415 this->processData(data, stamp);
00416 }
00417 }
00418 }
00419
00420 protected:
00421 virtual void flushCallbacks()
00422 {
00423
00424 if(approxScanSync_)
00425 {
00426 delete approxScanSync_;
00427 approxScanSync_ = new message_filters::Synchronizer<MyApproxScanSyncPolicy>(MyApproxScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
00428 approxScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
00429 }
00430 if(exactScanSync_)
00431 {
00432 delete exactScanSync_;
00433 exactScanSync_ = new message_filters::Synchronizer<MyExactScanSyncPolicy>(MyExactScanSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, scan_sub_);
00434 exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
00435 }
00436 if(approxCloudSync_)
00437 {
00438 delete approxCloudSync_;
00439 approxCloudSync_ = new message_filters::Synchronizer<MyApproxCloudSyncPolicy>(MyApproxCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
00440 approxCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
00441 }
00442 if(exactCloudSync_)
00443 {
00444 delete exactCloudSync_;
00445 exactCloudSync_ = new message_filters::Synchronizer<MyExactCloudSyncPolicy>(MyExactCloudSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_, cloud_sub_);
00446 exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
00447 }
00448 }
00449
00450 private:
00451 image_transport::SubscriberFilter image_mono_sub_;
00452 image_transport::SubscriberFilter image_depth_sub_;
00453 message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00454 message_filters::Subscriber<sensor_msgs::LaserScan> scan_sub_;
00455 message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
00456 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyApproxScanSyncPolicy;
00457 message_filters::Synchronizer<MyApproxScanSyncPolicy> * approxScanSync_;
00458 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::LaserScan> MyExactScanSyncPolicy;
00459 message_filters::Synchronizer<MyExactScanSyncPolicy> * exactScanSync_;
00460 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyApproxCloudSyncPolicy;
00461 message_filters::Synchronizer<MyApproxCloudSyncPolicy> * approxCloudSync_;
00462 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> MyExactCloudSyncPolicy;
00463 message_filters::Synchronizer<MyExactCloudSyncPolicy> * exactCloudSync_;
00464 int queueSize_;
00465 int scanCloudMaxPoints_;
00466 double scanVoxelSize_;
00467 int scanNormalK_;
00468 double scanNormalRadius_;
00469 };
00470
00471 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDICPOdometry, nodelet::Nodelet);
00472
00473 }