mono_depth_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 <depth_image.hpp>
00009 
00010 #include "mono_depth_processor.hpp"
00011 #include "odometer_base.hpp"
00012 #include "visualization.hpp"
00013 
00014 namespace fovis_ros
00015 {
00016 
00017 class MonoDepthOdometer : public MonoDepthProcessor, OdometerBase
00018 {
00019 
00020 private:
00021 
00022   fovis::DepthImage* depth_image_;
00023 
00024 public:
00025 
00026   MonoDepthOdometer(const std::string& transport) : 
00027     MonoDepthProcessor(transport),
00028     depth_image_(NULL)
00029   {
00030   }
00031 
00032   ~MonoDepthOdometer()
00033   {
00034     if (depth_image_) delete depth_image_;
00035   }
00036 
00037 protected:
00038 
00039   fovis::DepthImage* createDepthSource(
00040       const sensor_msgs::CameraInfoConstPtr& image_info_msg,
00041       const sensor_msgs::CameraInfoConstPtr& depth_info_msg) const
00042   {
00043     // read calibration info from camera info message
00044     image_geometry::PinholeCameraModel model;
00045     model.fromCameraInfo(*image_info_msg);
00046     
00047     // initialize left camera parameters
00048     fovis::CameraIntrinsicsParameters parameters;
00049     rosToFovis(model, parameters);
00050 
00051     return new fovis::DepthImage(parameters, 
00052         depth_info_msg->width, depth_info_msg->height);
00053   }
00054 
00055   void imageCallback(
00056       const sensor_msgs::ImageConstPtr& image_msg,
00057       const sensor_msgs::ImageConstPtr& depth_msg,
00058       const sensor_msgs::CameraInfoConstPtr& image_info_msg,
00059       const sensor_msgs::CameraInfoConstPtr& depth_info_msg)
00060   {
00061     if (!depth_image_)
00062     {
00063       depth_image_ = createDepthSource(image_info_msg, depth_info_msg);
00064       setDepthSource(depth_image_);
00065     }
00066 
00067     if (depth_msg->encoding != sensor_msgs::image_encodings::TYPE_32FC1)
00068     {
00069       ROS_ERROR("Depth image must be in 32bit floating point format!");
00070       return;
00071     }
00072     ROS_ASSERT(depth_msg->step == depth_msg->width * sizeof(float));
00073     const float* depth_data = reinterpret_cast<const float*>(depth_msg->data.data());
00074 
00075     // pass data to depth source
00076     depth_image_->setDepthImage(depth_data);
00077 
00078     // call base implementation
00079     process(image_msg, image_info_msg);
00080   }
00081 };
00082 
00083 } // end of namespace
00084 
00085 
00086 int main(int argc, char **argv)
00087 {
00088   ros::init(argc, argv, "mono_depth_odometer");
00089   std::string transport = argc > 1 ? argv[1] : "raw";
00090   fovis_ros::MonoDepthOdometer odometer(transport);
00091   ros::spin();
00092   return 0;
00093 }
00094 


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