mono_odometer.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/image_encodings.h>
00003 #include <cv_bridge/cv_bridge.h>
00004 #include <image_transport/camera_subscriber.h>
00005 #include <image_transport/image_transport.h>
00006 #include <image_geometry/pinhole_camera_model.h>
00007 
00008 #include <viso_mono.h>
00009 
00010 #include <viso2_ros/VisoInfo.h>
00011 
00012 #include "odometer_base.h"
00013 #include "odometry_params.h"
00014 
00015 namespace viso2_ros
00016 {
00017 
00018 class MonoOdometer : public OdometerBase
00019 {
00020 
00021 private:
00022 
00023   boost::shared_ptr<VisualOdometryMono> visual_odometer_;
00024   VisualOdometryMono::parameters visual_odometer_params_;
00025 
00026   image_transport::CameraSubscriber camera_sub_;
00027 
00028   ros::Publisher info_pub_;
00029 
00030   bool replace_;
00031 
00032 public:
00033 
00034   MonoOdometer(const std::string& transport) : OdometerBase(), replace_(false)
00035   {
00036     // Read local parameters
00037     ros::NodeHandle local_nh("~");
00038     odometry_params::loadParams(local_nh, visual_odometer_params_);
00039 
00040     ros::NodeHandle nh;
00041     image_transport::ImageTransport it(nh);
00042     camera_sub_ = it.subscribeCamera("image", 1, &MonoOdometer::imageCallback, this, transport);
00043 
00044     info_pub_ = local_nh.advertise<VisoInfo>("info", 1);
00045   }
00046 
00047 protected:
00048 
00049   void imageCallback(
00050       const sensor_msgs::ImageConstPtr& image_msg,
00051       const sensor_msgs::CameraInfoConstPtr& info_msg)
00052   {
00053     ros::WallTime start_time = ros::WallTime::now();
00054  
00055     bool first_run = false;
00056     // create odometer if not exists
00057     if (!visual_odometer_)
00058     {
00059       first_run = true;
00060       // read calibration info from camera info message
00061       // to fill remaining odometer parameters
00062       image_geometry::PinholeCameraModel model;
00063       model.fromCameraInfo(info_msg);
00064       visual_odometer_params_.calib.f  = model.fx();
00065       visual_odometer_params_.calib.cu = model.cx();
00066       visual_odometer_params_.calib.cv = model.cy();
00067       visual_odometer_.reset(new VisualOdometryMono(visual_odometer_params_));
00068       if (image_msg->header.frame_id != "") setSensorFrameId(image_msg->header.frame_id);
00069       ROS_INFO_STREAM("Initialized libviso2 mono odometry "
00070                       "with the following parameters:" << std::endl << 
00071                       visual_odometer_params_);
00072     }
00073 
00074     // convert image if necessary
00075     uint8_t *image_data;
00076     int step;
00077     cv_bridge::CvImageConstPtr cv_ptr;
00078     if (image_msg->encoding == sensor_msgs::image_encodings::MONO8)
00079     {
00080       image_data = const_cast<uint8_t*>(&(image_msg->data[0]));
00081       step = image_msg->step;
00082     }
00083     else
00084     {
00085       cv_ptr = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8);
00086       image_data = cv_ptr->image.data;
00087       step = cv_ptr->image.step[0];
00088     }
00089 
00090     // run the odometer
00091     int32_t dims[] = {image_msg->width, image_msg->height, step};
00092     // on first run, only feed the odometer with first image pair without
00093     // retrieving data
00094     if (first_run)
00095     {
00096       visual_odometer_->process(image_data, dims);
00097       tf::Transform delta_transform;
00098       delta_transform.setIdentity();
00099       integrateAndPublish(delta_transform, image_msg->header.stamp);
00100     }
00101     else
00102     {
00103       bool success = visual_odometer_->process(image_data, dims);
00104       if(success)
00105       {
00106         replace_ = false;
00107         Matrix camera_motion = Matrix::inv(visual_odometer_->getMotion());
00108         ROS_DEBUG("Found %i matches with %i inliers.", 
00109                   visual_odometer_->getNumberOfMatches(),
00110                   visual_odometer_->getNumberOfInliers());
00111         ROS_DEBUG_STREAM("libviso2 returned the following motion:\n" << camera_motion);
00112 
00113         tf::Matrix3x3 rot_mat(
00114           camera_motion.val[0][0], camera_motion.val[0][1], camera_motion.val[0][2],
00115           camera_motion.val[1][0], camera_motion.val[1][1], camera_motion.val[1][2],
00116           camera_motion.val[2][0], camera_motion.val[2][1], camera_motion.val[2][2]);
00117         tf::Vector3 t(camera_motion.val[0][3], camera_motion.val[1][3], camera_motion.val[2][3]);
00118         tf::Transform delta_transform(rot_mat, t);
00119 
00120         integrateAndPublish(delta_transform, image_msg->header.stamp);
00121       }
00122       else
00123       {
00124         ROS_DEBUG("Call to VisualOdometryMono::process() failed. Assuming motion too small.");
00125         replace_ = true;
00126         tf::Transform delta_transform;
00127         delta_transform.setIdentity();
00128         integrateAndPublish(delta_transform, image_msg->header.stamp);
00129       }
00130 
00131       // create and publish viso2 info msg
00132       VisoInfo info_msg;
00133       info_msg.header.stamp = image_msg->header.stamp;
00134       info_msg.got_lost = !success;
00135       info_msg.change_reference_frame = false;
00136       info_msg.num_matches = visual_odometer_->getNumberOfMatches();
00137       info_msg.num_inliers = visual_odometer_->getNumberOfInliers();
00138       ros::WallDuration time_elapsed = ros::WallTime::now() - start_time;
00139       info_msg.runtime = time_elapsed.toSec();
00140       info_pub_.publish(info_msg);
00141     }
00142   }
00143 };
00144 
00145 } // end of namespace
00146 
00147 
00148 int main(int argc, char **argv)
00149 {
00150   ros::init(argc, argv, "mono_odometer");
00151   if (ros::names::remap("image").find("rect") == std::string::npos) {
00152     ROS_WARN("mono_odometer needs rectified input images. The used image "
00153              "topic is '%s'. Are you sure the images are rectified?",
00154              ros::names::remap("image").c_str());
00155   }
00156 
00157   std::string transport = argc > 1 ? argv[1] : "raw";
00158   viso2_ros::MonoOdometer odometer(transport);
00159   
00160   ros::spin();
00161   return 0;
00162 }
00163 


viso2_ros
Author(s): Stephan Wirth
autogenerated on Tue Jan 7 2014 11:42:16