stereo_odometer.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/image_encodings.h>
00003 #include <image_geometry/stereo_camera_model.h>
00004 #include <cv_bridge/cv_bridge.h>
00005 
00006 #include <fovis_ros/FovisInfo.h>
00007 
00008 #include <stereo_depth.hpp>
00009 
00010 #include "stereo_processor.hpp"
00011 #include "odometer_base.hpp"
00012 #include "visualization.hpp"
00013 
00014 namespace fovis_ros
00015 {
00016 
00017 class StereoOdometer : public StereoProcessor, OdometerBase
00018 {
00019 
00020 private:
00021 
00022   fovis::StereoDepth* stereo_depth_;
00023 
00024 public:
00025 
00026   StereoOdometer(const std::string& transport) : 
00027     StereoProcessor(transport),
00028     stereo_depth_(NULL)
00029   {
00030   }
00031 
00032   ~StereoOdometer()
00033   {
00034     if (stereo_depth_) delete stereo_depth_;
00035   }
00036 
00037 protected:
00038 
00039   fovis::StereoDepth* createStereoDepth(
00040       const sensor_msgs::CameraInfoConstPtr& l_info_msg,
00041       const sensor_msgs::CameraInfoConstPtr& r_info_msg) const
00042   {
00043     // read calibration info from camera info message
00044     // to fill remaining parameters
00045     image_geometry::StereoCameraModel model;
00046     model.fromCameraInfo(*l_info_msg, *r_info_msg);
00047     
00048     // initialize left camera parameters
00049     fovis::CameraIntrinsicsParameters left_parameters;
00050     rosToFovis(model.left(), left_parameters);
00051     left_parameters.height = l_info_msg->height;
00052     left_parameters.width = l_info_msg->width;
00053     // initialize right camera parameters
00054     fovis::CameraIntrinsicsParameters right_parameters;
00055     rosToFovis(model.right(), right_parameters);
00056     right_parameters.height = r_info_msg->height;
00057     right_parameters.width = r_info_msg->width;
00058 
00059     // as we use rectified images, rotation is identity
00060     // and translation is baseline only
00061     fovis::StereoCalibrationParameters stereo_parameters;
00062     stereo_parameters.left_parameters = left_parameters;
00063     stereo_parameters.right_parameters = right_parameters;
00064     stereo_parameters.right_to_left_rotation[0] = 1.0;
00065     stereo_parameters.right_to_left_rotation[1] = 0.0;
00066     stereo_parameters.right_to_left_rotation[2] = 0.0;
00067     stereo_parameters.right_to_left_rotation[3] = 0.0;
00068     stereo_parameters.right_to_left_translation[0] = -model.baseline();
00069     stereo_parameters.right_to_left_translation[1] = 0.0;
00070     stereo_parameters.right_to_left_translation[2] = 0.0;
00071 
00072     fovis::StereoCalibration* stereo_calibration = 
00073       new fovis::StereoCalibration(stereo_parameters);
00074 
00075     return new fovis::StereoDepth(stereo_calibration, getOptions());
00076   }
00077 
00078   void imageCallback(
00079       const sensor_msgs::ImageConstPtr& l_image_msg,
00080       const sensor_msgs::ImageConstPtr& r_image_msg,
00081       const sensor_msgs::CameraInfoConstPtr& l_info_msg,
00082       const sensor_msgs::CameraInfoConstPtr& r_info_msg)
00083   {
00084     if (!stereo_depth_)
00085     {
00086       stereo_depth_ = createStereoDepth(l_info_msg, r_info_msg);
00087       setDepthSource(stereo_depth_);
00088     }
00089     // convert image if necessary
00090     uint8_t *r_image_data;
00091     int r_step;
00092     cv_bridge::CvImageConstPtr r_cv_ptr;
00093     r_cv_ptr = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8);
00094     r_image_data = r_cv_ptr->image.data;
00095     r_step = r_cv_ptr->image.step[0];
00096 
00097     ROS_ASSERT(r_step == static_cast<int>(r_image_msg->width));
00098     ROS_ASSERT(l_image_msg->width == r_image_msg->width);
00099     ROS_ASSERT(l_image_msg->height == r_image_msg->height);
00100 
00101     // pass image to depth source
00102     stereo_depth_->setRightImage(r_image_data);
00103 
00104     // call base implementation
00105     process(l_image_msg, l_info_msg);
00106   }
00107 };
00108 
00109 } // end of namespace
00110 
00111 
00112 int main(int argc, char **argv)
00113 {
00114   ros::init(argc, argv, "stereo_odometer");
00115   if (ros::names::remap("stereo") == "stereo") {
00116     ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n"
00117              "\t$ rosrun fovis_ros stereo_odometer stereo:=narrow_stereo image:=image_rect");
00118   }
00119   if (ros::names::remap("image").find("rect") == std::string::npos) {
00120     ROS_WARN("stereo_odometer needs rectified input images. The used image "
00121              "topic is '%s'. Are you sure the images are rectified?",
00122              ros::names::remap("image").c_str());
00123   }
00124 
00125   std::string transport = argc > 1 ? argv[1] : "raw";
00126   fovis_ros::StereoOdometer odometer(transport);
00127   
00128   ros::spin();
00129   return 0;
00130 }
00131 


fovis_ros
Author(s): Stephan Wirth
autogenerated on Fri Sep 12 2014 12:12:54