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 "OdometryROS.h"
00029
00030 #include <message_filters/subscriber.h>
00031 #include <message_filters/time_synchronizer.h>
00032 #include <message_filters/sync_policies/approximate_time.h>
00033
00034 #include <image_transport/image_transport.h>
00035 #include <image_transport/subscriber_filter.h>
00036
00037 #include <sensor_msgs/Image.h>
00038 #include <sensor_msgs/image_encodings.h>
00039
00040 #include <image_geometry/stereo_camera_model.h>
00041
00042 #include <cv_bridge/cv_bridge.h>
00043
00044 #include "rtabmap_ros/MsgConversion.h"
00045
00046 #include <rtabmap/utilite/ULogger.h>
00047 #include <rtabmap/utilite/UTimer.h>
00048
00049 using namespace rtabmap;
00050
00051 class StereoOdometry : public rtabmap_ros::OdometryROS
00052 {
00053 public:
00054 StereoOdometry(int argc, char * argv[]) :
00055 rtabmap_ros::OdometryROS(argc, argv),
00056 approxSync_(0),
00057 exactSync_(0)
00058 {
00059 ros::NodeHandle nh;
00060
00061 ros::NodeHandle pnh("~");
00062
00063 bool approxSync = false;
00064 int queueSize = 5;
00065 pnh.param("approx_sync", approxSync, approxSync);
00066 pnh.param("queue_size", queueSize, queueSize);
00067 ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
00068
00069 ros::NodeHandle left_nh(nh, "left");
00070 ros::NodeHandle right_nh(nh, "right");
00071 ros::NodeHandle left_pnh(pnh, "left");
00072 ros::NodeHandle right_pnh(pnh, "right");
00073 image_transport::ImageTransport left_it(left_nh);
00074 image_transport::ImageTransport right_it(right_nh);
00075 image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
00076 image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
00077
00078 imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
00079 imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
00080 cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
00081 cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
00082
00083 ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
00084 ros::this_node::getName().c_str(),
00085 imageRectLeft_.getTopic().c_str(),
00086 imageRectRight_.getTopic().c_str(),
00087 cameraInfoLeft_.getTopic().c_str(),
00088 cameraInfoRight_.getTopic().c_str());
00089
00090 if(approxSync)
00091 {
00092 approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00093 approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
00094 }
00095 else
00096 {
00097 exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
00098 exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
00099 }
00100 }
00101
00102 ~StereoOdometry()
00103 {
00104 if(approxSync_)
00105 {
00106 delete approxSync_;
00107 }
00108 if(exactSync_)
00109 {
00110 delete exactSync_;
00111 }
00112 }
00113
00114 void callback(
00115 const sensor_msgs::ImageConstPtr& imageRectLeft,
00116 const sensor_msgs::ImageConstPtr& imageRectRight,
00117 const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
00118 const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
00119 {
00120 if(!this->isPaused())
00121 {
00122 if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00123 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00124 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00125 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
00126 !(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
00127 imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
00128 imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
00129 imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
00130 {
00131 ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
00132 return;
00133 }
00134
00135 tf::StampedTransform localTransform;
00136 try
00137 {
00138 if(this->waitForTransform())
00139 {
00140 if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, ros::Duration(1)))
00141 {
00142 ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str());
00143 return;
00144 }
00145 }
00146
00147 this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
00148 }
00149 catch(tf::TransformException & ex)
00150 {
00151 ROS_WARN("%s",ex.what());
00152 return;
00153 }
00154
00155 ros::WallTime time = ros::WallTime::now();
00156
00157 int quality = -1;
00158 if(imageRectLeft->data.size() && imageRectRight->data.size())
00159 {
00160 image_geometry::StereoCameraModel model;
00161 model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
00162
00163 float fx = model.left().fx();
00164 float cx = model.left().cx();
00165 float cy = model.left().cy();
00166 float baseline = model.baseline();
00167 cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
00168 cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
00169
00170 if(baseline <= 0)
00171 {
00172 ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
00173 "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", baseline);
00174 return;
00175 }
00176
00177 UTimer stepTimer;
00178
00179 UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str());
00180 rtabmap::SensorData data(ptrImageLeft->image,
00181 ptrImageRight->image,
00182 fx,
00183 baseline,
00184 cx,
00185 cy,
00186 rtabmap_ros::transformFromTF(localTransform),
00187 rtabmap::Transform(),
00188 1.0f,
00189 1.0f,
00190 0,
00191 rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp));
00192
00193 this->processData(data, imageRectLeft->header);
00194 }
00195 else
00196 {
00197 ROS_WARN("Odom: input images empty?!?");
00198 }
00199 }
00200 }
00201
00202 private:
00203 image_transport::SubscriberFilter imageRectLeft_;
00204 image_transport::SubscriberFilter imageRectRight_;
00205 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
00206 message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
00207 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
00208 message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
00209 typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
00210 message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
00211 };
00212
00213 int main(int argc, char *argv[])
00214 {
00215 ULogger::setType(ULogger::kTypeConsole);
00216 ULogger::setLevel(ULogger::kWarning);
00217
00218 ros::init(argc, argv, "stereo_odometry");
00219
00220 StereoOdometry odom(argc, argv);
00221 ros::spin();
00222 return 0;
00223 }