rgbd_odometry.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 
00042 #include <sensor_msgs/Image.h>
00043 #include <sensor_msgs/image_encodings.h>
00044 #include <cv_bridge/cv_bridge.h>
00045 
00046 #include "rtabmap_ros/MsgConversion.h"
00047 
00048 #include <rtabmap/core/util3d.h>
00049 #include <rtabmap/core/util2d.h>
00050 #include <rtabmap/utilite/ULogger.h>
00051 #include <rtabmap/utilite/UConversion.h>
00052 #include <rtabmap/utilite/UStl.h>
00053 
00054 using namespace rtabmap;
00055 
00056 namespace rtabmap_ros
00057 {
00058 
00059 class RGBDOdometry : public rtabmap_ros::OdometryROS
00060 {
00061 public:
00062         RGBDOdometry() :
00063                 OdometryROS(false, true, false),
00064                 approxSync_(0),
00065                 exactSync_(0),
00066                 approxSync2_(0),
00067                 exactSync2_(0),
00068                 approxSync3_(0),
00069                 exactSync3_(0),
00070                 approxSync4_(0),
00071                 exactSync4_(0),
00072                 queueSize_(5)
00073         {
00074         }
00075 
00076         virtual ~RGBDOdometry()
00077         {
00078                 rgbdSub_.shutdown();
00079                 if(approxSync_)
00080                 {
00081                         delete approxSync_;
00082                 }
00083                 if(exactSync_)
00084                 {
00085                         delete exactSync_;
00086                 }
00087                 if(approxSync2_)
00088                 {
00089                         delete approxSync2_;
00090                 }
00091                 if(exactSync2_)
00092                 {
00093                         delete exactSync2_;
00094                 }
00095                 if(approxSync3_)
00096                 {
00097                         delete approxSync3_;
00098                 }
00099                 if(exactSync3_)
00100                 {
00101                         delete exactSync3_;
00102                 }
00103                 if(approxSync4_)
00104                 {
00105                         delete approxSync4_;
00106                 }
00107                 if(exactSync4_)
00108                 {
00109                         delete exactSync4_;
00110                 }
00111         }
00112 
00113 private:
00114 
00115         virtual void onOdomInit()
00116         {
00117                 ros::NodeHandle & nh = getNodeHandle();
00118                 ros::NodeHandle & pnh = getPrivateNodeHandle();
00119 
00120                 int rgbdCameras = 1;
00121                 bool approxSync = true;
00122                 bool subscribeRGBD = false;
00123                 pnh.param("approx_sync", approxSync, approxSync);
00124                 pnh.param("queue_size", queueSize_, queueSize_);
00125                 pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
00126                 if(pnh.hasParam("depth_cameras"))
00127                 {
00128                         ROS_ERROR("\"depth_cameras\" parameter doesn't exist anymore. It is replaced by \"rgbd_cameras\" with the \"rgbd_image\" input topics. \"subscribe_rgbd\" should be also set to true.");
00129                 }
00130                 pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
00131                 if(rgbdCameras <= 0)
00132                 {
00133                         rgbdCameras = 1;
00134                 }
00135                 if(rgbdCameras > 4)
00136                 {
00137                         NODELET_FATAL("Only 4 cameras maximum supported yet.");
00138                 }
00139 
00140                 NODELET_INFO("RGBDOdometry: approx_sync    = %s", approxSync?"true":"false");
00141                 NODELET_INFO("RGBDOdometry: queue_size     = %d", queueSize_);
00142                 NODELET_INFO("RGBDOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
00143                 NODELET_INFO("RGBDOdometry: rgbd_cameras   = %d", rgbdCameras);
00144 
00145                 std::string subscribedTopicsMsg;
00146                 if(subscribeRGBD)
00147                 {
00148                         if(rgbdCameras >= 2)
00149                         {
00150                                 rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
00151                                 rgbd_image2_sub_.subscribe(nh, "rgbd_image1", 1);
00152                                 if(rgbdCameras >= 3)
00153                                 {
00154                                         rgbd_image3_sub_.subscribe(nh, "rgbd_image2", 1);
00155                                 }
00156                                 if(rgbdCameras >= 4)
00157                                 {
00158                                         rgbd_image4_sub_.subscribe(nh, "rgbd_image3", 1);
00159                                 }
00160 
00161                                 if(rgbdCameras == 2)
00162                                 {
00163                                         if(approxSync)
00164                                         {
00165                                                 approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
00166                                                                 MyApproxSync2Policy(queueSize_),
00167                                                                 rgbd_image1_sub_,
00168                                                                 rgbd_image2_sub_);
00169                                                 approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00170                                         }
00171                                         else
00172                                         {
00173                                                 exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
00174                                                                 MyExactSync2Policy(queueSize_),
00175                                                                 rgbd_image1_sub_,
00176                                                                 rgbd_image2_sub_);
00177                                                 exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00178                                         }
00179                                         subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s",
00180                                                         getName().c_str(),
00181                                                         approxSync?"approx":"exact",
00182                                                         rgbd_image1_sub_.getTopic().c_str(),
00183                                                         rgbd_image2_sub_.getTopic().c_str());
00184                                 }
00185                                 else if(rgbdCameras == 3)
00186                                 {
00187                                         if(approxSync)
00188                                         {
00189                                                 approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
00190                                                                 MyApproxSync3Policy(queueSize_),
00191                                                                 rgbd_image1_sub_,
00192                                                                 rgbd_image2_sub_,
00193                                                                 rgbd_image3_sub_);
00194                                                 approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00195                                         }
00196                                         else
00197                                         {
00198                                                 exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
00199                                                                 MyExactSync3Policy(queueSize_),
00200                                                                 rgbd_image1_sub_,
00201                                                                 rgbd_image2_sub_,
00202                                                                 rgbd_image3_sub_);
00203                                                 exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00204                                         }
00205                                         subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
00206                                                         getName().c_str(),
00207                                                         approxSync?"approx":"exact",
00208                                                         rgbd_image1_sub_.getTopic().c_str(),
00209                                                         rgbd_image2_sub_.getTopic().c_str(),
00210                                                         rgbd_image3_sub_.getTopic().c_str());
00211                                 }
00212                                 else if(rgbdCameras == 4)
00213                                 {
00214                                         if(approxSync)
00215                                         {
00216                                                 approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
00217                                                                 MyApproxSync4Policy(queueSize_),
00218                                                                 rgbd_image1_sub_,
00219                                                                 rgbd_image2_sub_,
00220                                                                 rgbd_image3_sub_,
00221                                                                 rgbd_image4_sub_);
00222                                                 approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00223                                         }
00224                                         else
00225                                         {
00226                                                 exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
00227                                                                 MyExactSync4Policy(queueSize_),
00228                                                                 rgbd_image1_sub_,
00229                                                                 rgbd_image2_sub_,
00230                                                                 rgbd_image3_sub_,
00231                                                                 rgbd_image4_sub_);
00232                                                 exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00233                                         }
00234                                         subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s,\n   %s",
00235                                                         getName().c_str(),
00236                                                         approxSync?"approx":"exact",
00237                                                         rgbd_image1_sub_.getTopic().c_str(),
00238                                                         rgbd_image2_sub_.getTopic().c_str(),
00239                                                         rgbd_image3_sub_.getTopic().c_str(),
00240                                                         rgbd_image4_sub_.getTopic().c_str());
00241                                 }
00242                         }
00243                         else
00244                         {
00245                                 rgbdSub_ = nh.subscribe("rgbd_image", 1, &RGBDOdometry::callbackRGBD, this);
00246 
00247                                 subscribedTopicsMsg =
00248                                                 uFormat("\n%s subscribed to:\n   %s",
00249                                                 getName().c_str(),
00250                                                 rgbdSub_.getTopic().c_str());
00251                         }
00252                 }
00253                 else
00254                 {
00255                         ros::NodeHandle rgb_nh(nh, "rgb");
00256                         ros::NodeHandle depth_nh(nh, "depth");
00257                         ros::NodeHandle rgb_pnh(pnh, "rgb");
00258                         ros::NodeHandle depth_pnh(pnh, "depth");
00259                         image_transport::ImageTransport rgb_it(rgb_nh);
00260                         image_transport::ImageTransport depth_it(depth_nh);
00261                         image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
00262                         image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
00263 
00264                         image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
00265                         image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
00266                         info_sub_.subscribe(rgb_nh, "camera_info", 1);
00267 
00268                         if(approxSync)
00269                         {
00270                                 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00271                                 approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00272                         }
00273                         else
00274                         {
00275                                 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00276                                 exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00277                         }
00278 
00279                         subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n   %s,\n   %s,\n   %s",
00280                                         getName().c_str(),
00281                                         approxSync?"approx":"exact",
00282                                         image_mono_sub_.getTopic().c_str(),
00283                                         image_depth_sub_.getTopic().c_str(),
00284                                         info_sub_.getTopic().c_str());
00285                 }
00286                 this->startWarningThread(subscribedTopicsMsg, approxSync);
00287         }
00288 
00289         virtual void updateParameters(ParametersMap & parameters)
00290         {
00291                 //make sure we are using Reg/Strategy=0
00292                 ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
00293                 if(iter != parameters.end() && iter->second.compare("0") != 0)
00294                 {
00295                         ROS_WARN("RGBD odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
00296                 }
00297                 uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
00298 
00299                 int estimationType = Parameters::defaultVisEstimationType();
00300                 Parameters::parse(parameters, Parameters::kVisEstimationType(), estimationType);
00301                 ros::NodeHandle & pnh = getPrivateNodeHandle();
00302                 int rgbdCameras = 1;
00303                 bool subscribeRGBD = false;
00304                 pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
00305                 pnh.param("rgbd_cameras", rgbdCameras, rgbdCameras);
00306                 if(subscribeRGBD && rgbdCameras> 1 && estimationType>0)
00307                 {
00308                         NODELET_WARN("Setting \"%s\" parameter to 0 (%d is not supported "
00309                                         "for multi-cameras) as \"subscribe_rgbd\" is "
00310                                         "true and \"rgbd_cameras\">1. Set \"%s\" to 0 to suppress this warning.",
00311                                         Parameters::kVisEstimationType().c_str(),
00312                                         estimationType,
00313                                         Parameters::kVisEstimationType().c_str());
00314                         uInsert(parameters, ParametersPair(Parameters::kVisEstimationType(), "0"));
00315                 }
00316         }
00317 
00318         void commonCallback(
00319                                 const std::vector<cv_bridge::CvImageConstPtr> & rgbImages,
00320                                 const std::vector<cv_bridge::CvImageConstPtr> & depthImages,
00321                                 const std::vector<sensor_msgs::CameraInfo>& cameraInfos)
00322         {
00323                 ROS_ASSERT(rgbImages.size() > 0 && rgbImages.size() == depthImages.size() && rgbImages.size() == cameraInfos.size());
00324                 ros::Time higherStamp;
00325                 int imageWidth = rgbImages[0]->image.cols;
00326                 int imageHeight = rgbImages[0]->image.rows;
00327                 int depthWidth = depthImages[0]->image.cols;
00328                 int depthHeight = depthImages[0]->image.rows;
00329 
00330                 UASSERT_MSG(
00331                         imageWidth % depthWidth == 0 && imageHeight % depthHeight == 0 &&
00332                         imageWidth/depthWidth == imageHeight/depthHeight,
00333                         uFormat("rgb=%dx%d depth=%dx%d", imageWidth, imageHeight, depthWidth, depthHeight).c_str());
00334 
00335                 int cameraCount = rgbImages.size();
00336                 cv::Mat rgb;
00337                 cv::Mat depth;
00338                 pcl::PointCloud<pcl::PointXYZ> scanCloud;
00339                 std::vector<CameraModel> cameraModels;
00340                 for(unsigned int i=0; i<rgbImages.size(); ++i)
00341                 {
00342                         if(!(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
00343                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00344                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00345                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00346                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
00347                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
00348                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0 ||
00349                                  rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::BAYER_GRBG8) == 0) ||
00350                                 !(depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
00351                                  depthImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
00352                                  depthImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
00353                                  {
00354                                         NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8,bgra8,rgba8 and "
00355                                         "image_depth=32FC1,16UC1,mono16. Current rgb=%s and depth=%s",
00356                                                 rgbImages[i]->encoding.c_str(),
00357                                                 depthImages[i]->encoding.c_str());
00358                                         return;
00359                                 }
00360                         UASSERT_MSG(rgbImages[i]->image.cols == imageWidth && rgbImages[i]->image.rows == imageHeight,
00361                                         uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
00362                                                         imageWidth,
00363                                                         rgbImages[i]->image.cols,
00364                                                         imageHeight,
00365                                                         rgbImages[i]->image.rows).c_str());
00366                         UASSERT_MSG(depthImages[i]->image.cols == depthWidth && depthImages[i]->image.rows == depthHeight,
00367                                         uFormat("depthWidth=%d vs %d depthHeight=%d vs %d",
00368                                                         depthWidth,
00369                                                         depthImages[i]->image.cols,
00370                                                         depthHeight,
00371                                                         depthImages[i]->image.rows).c_str());
00372 
00373                         ros::Time stamp = rgbImages[i]->header.stamp>depthImages[i]->header.stamp?rgbImages[i]->header.stamp:depthImages[i]->header.stamp;
00374 
00375                         if(i == 0)
00376                         {
00377                                 higherStamp = stamp;
00378                         }
00379                         else if(stamp > higherStamp)
00380                         {
00381                                 higherStamp = stamp;
00382                         }
00383 
00384                         Transform localTransform = getTransform(this->frameId(), rgbImages[i]->header.frame_id, stamp);
00385                         if(localTransform.isNull())
00386                         {
00387                                 return;
00388                         }
00389 
00390                         cv_bridge::CvImageConstPtr ptrImage = rgbImages[i];
00391                         if(rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
00392                            rgbImages[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
00393                         {
00394                                 ptrImage = cv_bridge::cvtColor(rgbImages[i], "mono8");
00395                         }
00396 
00397                         cv_bridge::CvImageConstPtr ptrDepth = depthImages[i];
00398                         cv::Mat subDepth = ptrDepth->image;
00399 
00400                         // initialize
00401                         if(rgb.empty())
00402                         {
00403                                 rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
00404                         }
00405                         if(depth.empty())
00406                         {
00407                                 depth = cv::Mat(depthHeight, depthWidth*cameraCount, subDepth.type());
00408                         }
00409 
00410                         if(ptrImage->image.type() == rgb.type())
00411                         {
00412                                 ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
00413                         }
00414                         else
00415                         {
00416                                 NODELET_ERROR("Some RGB images are not the same type!");
00417                                 return;
00418                         }
00419 
00420                         if(subDepth.type() == depth.type())
00421                         {
00422                                 subDepth.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight)));
00423                         }
00424                         else
00425                         {
00426                                 NODELET_ERROR("Some Depth images are not the same type!");
00427                                 return;
00428                         }
00429 
00430                         cameraModels.push_back(rtabmap_ros::cameraModelFromROS(cameraInfos[i], localTransform));
00431                 }
00432 
00433                 rtabmap::SensorData data(
00434                                 rgb,
00435                                 depth,
00436                                 cameraModels,
00437                                 0,
00438                                 rtabmap_ros::timestampFromROS(higherStamp));
00439 
00440                 this->processData(data, higherStamp);
00441         }
00442 
00443         void callback(
00444                         const sensor_msgs::ImageConstPtr& image,
00445                         const sensor_msgs::ImageConstPtr& depth,
00446                         const sensor_msgs::CameraInfoConstPtr& cameraInfo)
00447         {
00448                 callbackCalled();
00449                 if(!this->isPaused())
00450                 {
00451                         std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
00452                         std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
00453                         std::vector<sensor_msgs::CameraInfo> infoMsgs;
00454                         imageMsgs[0] = cv_bridge::toCvShare(image);
00455                         depthMsgs[0] = cv_bridge::toCvShare(depth);
00456                         infoMsgs.push_back(*cameraInfo);
00457 
00458                         this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00459                 }
00460         }
00461 
00462         void callbackRGBD(
00463                         const rtabmap_ros::RGBDImageConstPtr& image)
00464         {
00465                 callbackCalled();
00466                 if(!this->isPaused())
00467                 {
00468                         std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
00469                         std::vector<cv_bridge::CvImageConstPtr> depthMsgs(1);
00470                         std::vector<sensor_msgs::CameraInfo> infoMsgs;
00471                         rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00472                         infoMsgs.push_back(image->rgbCameraInfo);
00473 
00474                         this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00475                 }
00476         }
00477 
00478         void callbackRGBD2(
00479                         const rtabmap_ros::RGBDImageConstPtr& image,
00480                         const rtabmap_ros::RGBDImageConstPtr& image2)
00481         {
00482                 callbackCalled();
00483                 if(!this->isPaused())
00484                 {
00485                         std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
00486                         std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2);
00487                         std::vector<sensor_msgs::CameraInfo> infoMsgs;
00488                         rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00489                         rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
00490                         infoMsgs.push_back(image->rgbCameraInfo);
00491                         infoMsgs.push_back(image2->rgbCameraInfo);
00492 
00493                         this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00494                 }
00495         }
00496 
00497         void callbackRGBD3(
00498                         const rtabmap_ros::RGBDImageConstPtr& image,
00499                         const rtabmap_ros::RGBDImageConstPtr& image2,
00500                         const rtabmap_ros::RGBDImageConstPtr& image3)
00501         {
00502                 callbackCalled();
00503                 if(!this->isPaused())
00504                 {
00505                         std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
00506                         std::vector<cv_bridge::CvImageConstPtr> depthMsgs(3);
00507                         std::vector<sensor_msgs::CameraInfo> infoMsgs;
00508                         rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00509                         rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
00510                         rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
00511                         infoMsgs.push_back(image->rgbCameraInfo);
00512                         infoMsgs.push_back(image2->rgbCameraInfo);
00513                         infoMsgs.push_back(image3->rgbCameraInfo);
00514 
00515                         this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00516                 }
00517         }
00518 
00519         void callbackRGBD4(
00520                         const rtabmap_ros::RGBDImageConstPtr& image,
00521                         const rtabmap_ros::RGBDImageConstPtr& image2,
00522                         const rtabmap_ros::RGBDImageConstPtr& image3,
00523                         const rtabmap_ros::RGBDImageConstPtr& image4)
00524         {
00525                 callbackCalled();
00526                 if(!this->isPaused())
00527                 {
00528                         std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
00529                         std::vector<cv_bridge::CvImageConstPtr> depthMsgs(4);
00530                         std::vector<sensor_msgs::CameraInfo> infoMsgs;
00531                         rtabmap_ros::toCvShare(image, imageMsgs[0], depthMsgs[0]);
00532                         rtabmap_ros::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
00533                         rtabmap_ros::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
00534                         rtabmap_ros::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
00535                         infoMsgs.push_back(image->rgbCameraInfo);
00536                         infoMsgs.push_back(image2->rgbCameraInfo);
00537                         infoMsgs.push_back(image3->rgbCameraInfo);
00538                         infoMsgs.push_back(image4->rgbCameraInfo);
00539 
00540                         this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
00541                 }
00542         }
00543 
00544 protected:
00545         virtual void flushCallbacks()
00546         {
00547                 // flush callbacks
00548                 if(approxSync_)
00549                 {
00550                         delete approxSync_;
00551                         approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00552                         approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00553                 }
00554                 if(exactSync_)
00555                 {
00556                         delete exactSync_;
00557                         exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
00558                         exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
00559                 }
00560                 if(approxSync2_)
00561                 {
00562                         delete approxSync2_;
00563                         approxSync2_ = new message_filters::Synchronizer<MyApproxSync2Policy>(
00564                                         MyApproxSync2Policy(queueSize_),
00565                                         rgbd_image1_sub_,
00566                                         rgbd_image2_sub_);
00567                         approxSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00568                 }
00569                 if(exactSync2_)
00570                 {
00571                         delete exactSync2_;
00572                         exactSync2_ = new message_filters::Synchronizer<MyExactSync2Policy>(
00573                                         MyExactSync2Policy(queueSize_),
00574                                         rgbd_image1_sub_,
00575                                         rgbd_image2_sub_);
00576                         exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD2, this, _1, _2));
00577                 }
00578                 if(approxSync3_)
00579                 {
00580                         delete approxSync3_;
00581                         approxSync3_ = new message_filters::Synchronizer<MyApproxSync3Policy>(
00582                                         MyApproxSync3Policy(queueSize_),
00583                                         rgbd_image1_sub_,
00584                                         rgbd_image2_sub_,
00585                                         rgbd_image3_sub_);
00586                         approxSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00587                 }
00588                 if(exactSync3_)
00589                 {
00590                         delete exactSync3_;
00591                         exactSync3_ = new message_filters::Synchronizer<MyExactSync3Policy>(
00592                                         MyExactSync3Policy(queueSize_),
00593                                         rgbd_image1_sub_,
00594                                         rgbd_image2_sub_,
00595                                         rgbd_image3_sub_);
00596                         exactSync3_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD3, this, _1, _2, _3));
00597                 }
00598                 if(approxSync4_)
00599                 {
00600                         delete approxSync4_;
00601                         approxSync4_ = new message_filters::Synchronizer<MyApproxSync4Policy>(
00602                                         MyApproxSync4Policy(queueSize_),
00603                                         rgbd_image1_sub_,
00604                                         rgbd_image2_sub_,
00605                                         rgbd_image3_sub_,
00606                                         rgbd_image4_sub_);
00607                         approxSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00608                 }
00609                 if(exactSync4_)
00610                 {
00611                         delete exactSync4_;
00612                         exactSync4_ = new message_filters::Synchronizer<MyExactSync4Policy>(
00613                                         MyExactSync4Policy(queueSize_),
00614                                         rgbd_image1_sub_,
00615                                         rgbd_image2_sub_,
00616                                         rgbd_image3_sub_,
00617                                         rgbd_image4_sub_);
00618                         exactSync4_->registerCallback(boost::bind(&RGBDOdometry::callbackRGBD4, this, _1, _2, _3, _4));
00619                 }
00620         }
00621 
00622 private:
00623         image_transport::SubscriberFilter image_mono_sub_;
00624         image_transport::SubscriberFilter image_depth_sub_;
00625         message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
00626 
00627         ros::Subscriber rgbdSub_;
00628         message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image1_sub_;
00629         message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image2_sub_;
00630         message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image3_sub_;
00631         message_filters::Subscriber<rtabmap_ros::RGBDImage> rgbd_image4_sub_;
00632 
00633         typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00634         message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00635         typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00636         message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00637         typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync2Policy;
00638         message_filters::Synchronizer<MyApproxSync2Policy> * approxSync2_;
00639         typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync2Policy;
00640         message_filters::Synchronizer<MyExactSync2Policy> * exactSync2_;
00641         typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync3Policy;
00642         message_filters::Synchronizer<MyApproxSync3Policy> * approxSync3_;
00643         typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync3Policy;
00644         message_filters::Synchronizer<MyExactSync3Policy> * exactSync3_;
00645         typedef message_filters::sync_policies::ApproximateTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyApproxSync4Policy;
00646         message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
00647         typedef message_filters::sync_policies::ExactTime<rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage, rtabmap_ros::RGBDImage> MyExactSync4Policy;
00648         message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
00649         int queueSize_;
00650 };
00651 
00652 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);
00653 
00654 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:30:49