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 #include "pluginlib/class_list_macros.h"
00030 #include "nodelet/nodelet.h"
00031
00032 #include <message_filters/subscriber.h>
00033 #include <message_filters/time_synchronizer.h>
00034 #include <message_filters/sync_policies/approximate_time.h>
00035
00036 #include <image_transport/image_transport.h>
00037 #include <image_transport/subscriber_filter.h>
00038
00039 #include <sensor_msgs/Image.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <sensor_msgs/Imu.h>
00042
00043 #include <image_geometry/stereo_camera_model.h>
00044
00045 #include <cv_bridge/cv_bridge.h>
00046
00047 #include "rtabmap_ros/MsgConversion.h"
00048
00049 #include <rtabmap/utilite/ULogger.h>
00050 #include <rtabmap/utilite/UTimer.h>
00051 #include <rtabmap/utilite/UStl.h>
00052 #include <rtabmap/utilite/UConversion.h>
00053 #include <rtabmap/core/Odometry.h>
00054
00055 using namespace rtabmap;
00056
00057 namespace rtabmap_ros
00058 {
00059
00060 class StereoOdometry : public rtabmap_ros::OdometryROS
00061 {
00062 public:
00063 StereoOdometry() :
00064 rtabmap_ros::OdometryROS(true, true, false),
00065 approxSync_(0),
00066 exactSync_(0),
00067 queueSize_(5)
00068 {
00069 }
00070
00071 virtual ~StereoOdometry()
00072 {
00073 if(approxSync_)
00074 {
00075 delete approxSync_;
00076 }
00077 if(exactSync_)
00078 {
00079 delete exactSync_;
00080 }
00081 }
00082
00083 private:
00084 virtual void onOdomInit()
00085 {
00086 ros::NodeHandle & nh = getNodeHandle();
00087 ros::NodeHandle & pnh = getPrivateNodeHandle();
00088
00089 bool approxSync = false;
00090 bool subscribeRGBD = false;
00091 pnh.param("approx_sync", approxSync, approxSync);
00092 pnh.param("queue_size", queueSize_, queueSize_);
00093 pnh.param("subscribe_rgbd", subscribeRGBD, subscribeRGBD);
00094
00095 NODELET_INFO("StereoOdometry: approx_sync = %s", approxSync?"true":"false");
00096 NODELET_INFO("StereoOdometry: queue_size = %d", queueSize_);
00097 NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
00098
00099 std::string subscribedTopicsMsg;
00100 if(subscribeRGBD)
00101 {
00102 rgbdSub_ = nh.subscribe("rgbd_image", 1, &StereoOdometry::callbackRGBD, this);
00103
00104 subscribedTopicsMsg =
00105 uFormat("\n%s subscribed to:\n %s",
00106 getName().c_str(),
00107 rgbdSub_.getTopic().c_str());
00108 }
00109 else
00110 {
00111 ros::NodeHandle left_nh(nh, "left");
00112 ros::NodeHandle right_nh(nh, "right");
00113 ros::NodeHandle left_pnh(pnh, "left");
00114 ros::NodeHandle right_pnh(pnh, "right");
00115 image_transport::ImageTransport left_it(left_nh);
00116 image_transport::ImageTransport right_it(right_nh);
00117 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00118 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00119
00120 imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
00121 imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
00122 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00123 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00124
00125 if(approxSync)
00126 {
00127 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00128 approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
00129 }
00130 else
00131 {
00132 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00133 exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
00134 }
00135
00136
00137 subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
00138 getName().c_str(),
00139 approxSync?"approx":"exact",
00140 imageRectLeft_.getTopic().c_str(),
00141 imageRectRight_.getTopic().c_str(),
00142 cameraInfoLeft_.getTopic().c_str(),
00143 cameraInfoRight_.getTopic().c_str());
00144 }
00145
00146 int odomStrategy = 0;
00147 Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
00148 if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
00149 {
00150 imuSub_ = nh.subscribe("imu", queueSize_*5, &StereoOdometry::callbackIMU, this);
00151 NODELET_INFO("VIO approach selected, subscribing to IMU topic %s", imuSub_.getTopic().c_str());
00152 }
00153
00154 this->startWarningThread(subscribedTopicsMsg, approxSync);
00155 }
00156
00157 virtual void updateParameters(ParametersMap & parameters)
00158 {
00159
00160 ParametersMap::iterator iter = parameters.find(Parameters::kRegStrategy());
00161 if(iter != parameters.end() && iter->second.compare("0") != 0)
00162 {
00163 ROS_WARN("Stereo odometry works only with \"Reg/Strategy\"=0. Ignoring value %s.", iter->second.c_str());
00164 }
00165 uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "0"));
00166 }
00167
00168 void callback(
00169 const sensor_msgs::ImageConstPtr& imageRectLeft,
00170 const sensor_msgs::ImageConstPtr& imageRectRight,
00171 const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
00172 const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
00173 {
00174 callbackCalled();
00175 if(!this->isPaused())
00176 {
00177 if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00178 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00179 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00180 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00181 !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00182 imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00183 imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00184 imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00185 {
00186 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended), received types are %s (left) and %s (right)",
00187 imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
00188 return;
00189 }
00190
00191 ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
00192
00193 Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
00194 if(localTransform.isNull())
00195 {
00196 return;
00197 }
00198
00199 int quality = -1;
00200 if(imageRectLeft->data.size() && imageRectRight->data.size())
00201 {
00202 rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
00203 if(stereoModel.baseline() <= 0)
00204 {
00205 NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
00206 "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
00207 return;
00208 }
00209
00210 if(stereoModel.baseline() > 10.0)
00211 {
00212 static bool shown = false;
00213 if(!shown)
00214 {
00215 NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
00216 "right camera_info P(0,3) correctly set? Note that "
00217 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
00218 stereoModel.baseline());
00219 shown = true;
00220 }
00221 }
00222
00223 cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
00224 cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
00225
00226 UTimer stepTimer;
00227
00228 UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
00229 rtabmap::SensorData data(
00230 ptrImageLeft->image,
00231 ptrImageRight->image,
00232 stereoModel,
00233 0,
00234 rtabmap_ros::timestampFromROS(stamp));
00235
00236 this->processData(data, stamp);
00237 }
00238 else
00239 {
00240 NODELET_WARN("Odom: input images empty?!?");
00241 }
00242 }
00243 }
00244
00245 void callbackRGBD(
00246 const rtabmap_ros::RGBDImageConstPtr& image)
00247 {
00248 callbackCalled();
00249 if(!this->isPaused())
00250 {
00251 cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
00252 rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
00253
00254 if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00255 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00256 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00257 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00258 !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00259 imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00260 imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00261 imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00262 {
00263 NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended), received types are %s (left) and %s (right)",
00264 imageRectLeft->encoding.c_str(), imageRectRight->encoding.c_str());
00265 return;
00266 }
00267
00268 ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
00269
00270 Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
00271 if(localTransform.isNull())
00272 {
00273 return;
00274 }
00275
00276 ros::WallTime time = ros::WallTime::now();
00277
00278 int quality = -1;
00279 if(!imageRectLeft->image.empty() && !imageRectRight->image.empty())
00280 {
00281 rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(image->rgbCameraInfo, image->depthCameraInfo, localTransform);
00282 if(stereoModel.baseline() <= 0)
00283 {
00284 NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
00285 "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
00286 return;
00287 }
00288
00289 if(stereoModel.baseline() > 10.0)
00290 {
00291 static bool shown = false;
00292 if(!shown)
00293 {
00294 NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
00295 "right camera_info P(0,3) correctly set? Note that "
00296 "baseline=-P(0,3)/P(0,0). This warning is printed only once.",
00297 stereoModel.baseline());
00298 shown = true;
00299 }
00300 }
00301
00302 cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
00303 cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
00304
00305 UTimer stepTimer;
00306
00307 UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
00308 rtabmap::SensorData data(
00309 ptrImageLeft->image,
00310 ptrImageRight->image,
00311 stereoModel,
00312 0,
00313 rtabmap_ros::timestampFromROS(stamp));
00314
00315 this->processData(data, stamp);
00316 }
00317 else
00318 {
00319 NODELET_WARN("Odom: input images empty?!?");
00320 }
00321 }
00322 }
00323
00324 void callbackIMU(
00325 const sensor_msgs::ImuConstPtr& msg)
00326 {
00327 if(!this->isPaused())
00328 {
00329 double stamp = msg->header.stamp.toSec();
00330 rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
00331 if(this->frameId().compare(msg->header.frame_id) != 0)
00332 {
00333 localTransform = getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp);
00334 }
00335 if(localTransform.isNull())
00336 {
00337 ROS_ERROR("Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
00338 msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
00339 return;
00340 }
00341
00342 IMU imu(
00343 cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
00344 cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
00345 cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
00346 cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
00347 localTransform);
00348
00349 SensorData data(imu, 0, stamp);
00350 this->processData(data, msg->header.stamp);
00351 }
00352 }
00353
00354 protected:
00355 virtual void flushCallbacks()
00356 {
00357
00358 if(approxSync_)
00359 {
00360 delete approxSync_;
00361 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00362 approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
00363 }
00364 if(exactSync_)
00365 {
00366 delete exactSync_;
00367 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00368 exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
00369 }
00370 }
00371
00372 private:
00373 image_transport::SubscriberFilter imageRectLeft_;
00374 image_transport::SubscriberFilter imageRectRight_;
00375 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00376 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00377 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00378 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00379 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00380 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00381 ros::Subscriber rgbdSub_;
00382 ros::Subscriber imuSub_;
00383 int queueSize_;
00384 };
00385
00386 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet);
00387
00388 }
00389