$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 /* author: Radu Bogdan Rusu <rusu@cs.tum.edu> */ 00031 00032 #ifndef STOC_DRIVER_H 00033 #define STOC_DRIVER_H 00034 00035 // ROS includes 00036 #include <sensor_msgs/PointCloud.h> 00037 #include <deprecated_msgs/ImageArray.h> 00038 #include <deprecated_msgs/Image.h> 00039 00040 #include <stdexcept> 00041 #include <string> 00042 #include <SVS/svsclass.h> 00043 00044 #include <opencv/cv.h> 00045 00046 namespace stoc 00047 { 00049 // code borrowed from drivers/laser/hokuyo_driver/hokuyo.h 00050 #define DEF_EXCEPTION(name, parent) \ 00051 class name : public parent { \ 00052 public: \ 00053 name (const char* msg) : parent (msg) {} \ 00054 } 00055 00057 DEF_EXCEPTION(Exception, std::runtime_error); 00058 00059 class STOC 00060 { 00061 public: 00062 STOC (); 00063 ~STOC (); 00064 00065 int open (); 00066 int close (); 00067 void readData (sensor_msgs::PointCloud &cloud, deprecated_msgs::ImageArray &images); 00068 void readDataLeft (sensor_msgs::PointCloud &cloud, deprecated_msgs::Image &left_image); 00069 00070 void sendInternalParameters (); 00071 void sendStereoParameters (); 00072 void readParametersFromFile (const char* parameter_file = NULL); 00073 00074 std::string device_id_; 00075 00076 // Acquisition device calls (cannot be handled online through the ROS parameter server) 00077 int capture_type_, format_, channel_; 00078 bool swap_mode_; 00079 int color_mode_, color_alg_; 00080 00081 // These must be set before streaming 00082 int proc_mode_, rate_, frame_div_, size_w_, size_h_; 00083 00084 bool rectification_; 00085 float z_max_; 00086 00087 bool multiproc_en_; 00088 int cut_di_; 00089 00090 void setNDisp (int n) { ndisp_ = n; video_->SetNDisp (ndisp_); if (debug_) ROS_INFO ("[STOC] >> Number of disparities set to %d", ndisp_); } 00091 void setTexThresh (int n) { tex_thresh_ = n; video_->SetThresh (tex_thresh_); if (debug_) ROS_INFO ("[STOC] >> Texture filter threshold set to %d", tex_thresh_); } 00092 void setUnique (int n) { unique_ = n; video_->SetUnique (unique_); if (debug_) ROS_INFO ("[STOC] >> Uniqueness filter threshold set to %d", unique_); } 00093 void setCorrSize (int n) { corrsize_ = n; video_->SetCorrsize (corrsize_); if (debug_) ROS_INFO ("[STOC] >> Correlation window size set to %d", corrsize_); } 00094 void setHoropter (int n) { horopter_ = n; video_->SetHoropter (horopter_); if (debug_) ROS_INFO ("[STOC] >> Horopter (X-Offset) value set to %d", horopter_); } 00095 void setSpeckleSize (int n) { speckle_size_ = n; video_->SetSpeckleSize (speckle_size_); if (debug_) ROS_INFO ("[STOC] >> Minimum disparity region size set to %d", speckle_size_); } 00096 void setSpeckleDiff (int n) { speckle_diff_ = n; video_->SetSpeckleDiff (speckle_diff_); if (debug_) ROS_INFO ("[STOC] >> Disparity region neighbor diff to %d", speckle_diff_); } 00097 private: 00098 00099 int speckle_diff_, speckle_size_, horopter_, corrsize_, unique_, tex_thresh_, ndisp_; 00100 00101 // SVS objects 00102 svsVideoImages *video_; 00103 svsStereoProcess *process_; 00104 svsMultiProcess *multiproc_; 00105 00106 bool debug_; 00107 00108 // Stores the intrinsic camera matrix and the distortion coefficients after calibration 00109 CvMat *intrinsic_, *distortion_; 00110 void undistort (uint8_t *img, uint8_t *un_img, int width, int height); 00111 }; 00112 00113 }; 00114 00115 #endif 00116