$search
00001 00005 #ifndef _THERMALVIS_DEPTH_H_ 00006 #define _THERMALVIS_DEPTH_H_ 00007 00008 #include "time.h" 00009 00010 #include <stereo_msgs/DisparityImage.h> 00011 #include <sensor_msgs/CameraInfo.h> 00012 #include <sensor_msgs/PointCloud2.h> 00013 #include <sensor_msgs/image_encodings.h> 00014 #include <image_transport/image_transport.h> 00015 00016 #include <boost/thread/thread.hpp> 00017 00018 #include <ros/ros.h> 00019 00020 // #include <image_geometry/image_geometry.h> 00021 00022 // #include "/opt/ros/electric/stacks/vision_opencv/image_geometry/include/image_geometry/stereo_camera_model.h" 00023 #include <image_geometry/stereo_camera_model.h> 00024 00025 namespace enc = sensor_msgs::image_encodings; 00026 00027 //#include "../../shared-src/super.h" 00028 #include "ros_resources.hpp" 00029 #include "general_resources.hpp" 00030 #include "opencv_resources.hpp" 00031 #include "camera.hpp" 00032 #include "tools.hpp" 00033 #include "improc.hpp" 00034 #include "stereo.hpp" 00035 // #include "../../shared-src/video.h" 00036 // #include "../../shared-src/reconstruction.h" 00037 00038 #include <dynamic_reconfigure/server.h> 00039 #include "depthConfig.h" 00040 00041 #define MAXIMUM_FRAMES_TO_STORE 100 00042 00043 const char __PROGRAM__[] = "DEPTH"; 00044 00045 typedef dynamic_reconfigure::Server < thermalvis::depthConfig > Server; 00046 00047 using namespace std; 00048 using namespace cv; 00049 00051 struct stereoData { 00052 string video_stream; 00053 cameraParameters cameraData1, cameraData2; 00054 00055 double alpha; 00056 bool autoAlpha; 00057 00058 string read_addr; 00059 string extrinsics; 00060 00061 bool debugMode; 00062 double maxTimeDiff; 00063 00064 // May also need some extrinsic data, extracted from the streams... 00065 00066 bool obtainStartingData(ros::NodeHandle& nh); 00067 00068 }; 00069 00071 class stereoDepthNode { 00072 private: 00073 00074 ros::Timer timer; 00075 00076 bool alphaChanged; 00077 00078 //sensor_msgs::Image disp_image; 00079 stereo_msgs::DisparityImage disp_object; 00080 sensor_msgs::PointCloud2 colouredCloud; 00081 00082 image_geometry::StereoCameraModel stereo_model; 00083 00084 sensor_msgs::CameraInfo cam_info_1, cam_info_2; 00085 00086 Mat t; 00087 Mat R0, R1; 00088 Mat P0, P1; 00089 Mat F, Q1, Q2; 00090 00091 unsigned int checkIndex_1, checkIndex_2; 00092 // unsigned int duplicateFlags_1[MAXIMUM_FRAMES_TO_STORE], duplicateFlags_2[MAXIMUM_FRAMES_TO_STORE]; 00093 ros::Time time_buffer_1[MAXIMUM_FRAMES_TO_STORE], time_buffer_2[MAXIMUM_FRAMES_TO_STORE]; 00094 vector<unsigned int> validPairs[2]; 00095 unsigned int pairsProcessed; 00096 00097 ros::Publisher disparityPublisher; 00098 00099 stereoData configData; 00100 00101 sensor_msgs::CameraInfo debug_camera_info; 00102 00103 char cam_pub_name_1[256], cam_pub_name_2[256]; 00104 sensor_msgs::Image msg_1, msg_2; 00105 sensor_msgs::CameraInfo camera_info_1, camera_info_2; 00106 image_transport::CameraPublisher depth_pub, cam_pub_1, cam_pub_2; 00107 00108 image_transport::CameraSubscriber camera_sub_1, camera_sub_2; 00109 00110 Mat disp16; 00111 00112 cv_bridge::CvImagePtr cv_ptr_1; 00113 cv_bridge::CvImagePtr cv_ptr_2; 00114 00115 Mat rawImageBuffer_1[MAXIMUM_FRAMES_TO_STORE], rawImageBuffer_2[MAXIMUM_FRAMES_TO_STORE]; 00116 Mat newImage, grayImage, realImage, mappedImage, lastImage_1, lastImage_2, olderImage_1, olderImage_2, normalizedMat; 00117 00118 Mat map11, map12, map21, map22; 00119 00120 Rect roi1, roi2; 00121 00122 struct timeval cycle_timer; 00123 double elapsedTime; 00124 00125 unsigned int frameCount, frameCount_1, frameCount_2; 00126 00127 char depth_pub_name[256]; 00128 ros::NodeHandle private_node_handle; 00129 00130 string nodeName; 00131 00132 Mat displayImage, drawImage; 00133 00134 Mat grayImageBuffer_1[MAXIMUM_FRAMES_TO_STORE], grayImageBuffer_2[MAXIMUM_FRAMES_TO_STORE]; 00135 00136 Mat blurredImage; 00137 00138 bool infoProcessed_1; 00139 bool infoProcessed_2; 00140 00141 bool firstImProcessed_1, firstImProcessed_2, firstPairProcessed; 00142 00143 StereoBM bm; 00144 StereoSGBM sgbm; 00145 Mat disp, disp8, dispFloat, disparityImage; 00146 00147 int SADWindowSize; 00148 int numberOfDisparities; 00149 00150 bool firstPair; 00151 00152 boost::shared_mutex _access; 00153 00154 dynamic_reconfigure::Server<thermalvis::depthConfig> server; 00155 dynamic_reconfigure::Server<thermalvis::depthConfig>::CallbackType f; 00156 00157 public: 00158 stereoDepthNode(ros::NodeHandle& nh, stereoData startupData); 00159 00160 void timed_loop(const ros::TimerEvent& event); 00161 00162 void handle_image_1(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg); 00163 void handle_image_2(const sensor_msgs::ImageConstPtr& msg_ptr, const sensor_msgs::CameraInfoConstPtr& info_msg); 00164 00165 void publishMaps(); 00166 00167 void assignDepthCameraInfo(); 00168 00169 void updatePairs(); 00170 void updateMaps(); 00171 00172 void serverCallback(thermalvis::depthConfig &config, uint32_t level); 00173 00174 }; 00175 00176 #endif