ZED Camera Support With OPT

README-ZED WITH OPT INSTALL ONLY FOLLOW THIS IF USING ZED.txt

@@ -0,0 +1,119 @@
+Built for Ubuntu 14.04 with Ros Indigo
+
+---------------------------------INSTALLING OPT WITH ZED--------------------------------------------------------
+
+First Install Cuda 7.5 Toolkit:
+
+Using below link:
+https://developer.nvidia.com/cuda-75-downloads-archive
+
+For Options Select:
+Linux
+x86_64
+Ubuntu
+14.04
+deb (local)
+
+download cuda-repo-ubuntu1404-7-5-local package
+
+After downloaded:
+In terminal go to folder containing cuda package
+$ sudo dpkg -i cuda-repo-ubuntu1404-7-5-local_7.5-18_amd64.deb
+$ sudo apt-get update
+$ sudo apt-get install cuda-7-5
+
+After install, reboot computer
+Verify Nvidia installed:
+$ nvidia-settings
+
+and make sure there is an nvidia driver number
+
+Next make sure enviroment path is set:
+$ export CUDA_HOME=/usr/local/cuda-7.5 
+$ export LD_LIBRARY_PATH=${CUDA_HOME}/lib64 
+
+$ PATH=${CUDA_HOME}/bin:${PATH} 
+$ export PATH 
+
+Finally verify install:
+$ cuda-install-samples-7.5.sh  ~ 
+$ cd ~/NVIDIA_CUDA-7.5_Samples 
+$ cd 1_Utilities/deviceQuery 
+$ make 
+$ ./deviceQuery
+
+The terminal then should output the cuda version info
+
+Next it is necessary to install ROS/OpenCV 2.0 (included in ROS):
+from directory where OPT is copied currently
+$ cd open_ptrack/scripts
+$ chmod +x *.sh
+$ ./ros_install.sh
+$ source /opt/ros/indigo/setup.bash
+$ ./ros_configure.sh
+
+OpenCV 3.1 is required for ZED to work (however it cannot conflict with OpenCV 2!! Therefore we will create a separate install path):
+$ cd
+$ sudo apt-get update
+
+$ sudo apt-get install libopencv-dev build-essential checkinstall cmake pkg-config yasm libtiff4-dev libjpeg-dev libjasper-dev libavcodec-dev libavformat-dev libswscale-dev libdc1394-22-dev libxine-dev libgstreamer0.10-dev libgstreamer-plugins-base0.10-dev libv4l-dev python-dev python-numpy libtbb-dev libqt4-dev libgtk2.0-dev libfaac-dev libmp3lame-dev libopencore-amrnb-dev libopencore-amrwb-dev libtheora-dev libvorbis-dev libxvidcore-dev x264 v4l-utils
+
+$ sudo add-apt-repository ppa:joyard-nicolas/ffmpeg 
+$ sudo apt-get update  
+$ sudo apt-get install ffmpeg  
+$ sudo apt-get install frei0r-plugins  
+
+$ sudo mkdir /usr/local/opencv3
+
+$ mkdir OpenCV  
+$ cd OpenCV  
+$ git clone https://github.com/opencv/opencv.git
+$ cd opencv 
+$ git checkout tags/3.1.0
+$ mkdir release  
+$ cd release  
+$ cmake -D CMAKE_BUILD_TYPE=RELEASE -D CMAKE_INSTALL_PREFIX=/usr/local/opencv3 -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_QT=ON -D WITH_OPENGL=ON -D ENABLE_FAST_MATH=1 -D CUDA_FAST_MATH=1 -D WITH_CUBLAS=1 ..
+
+Note adjust the -j7 to the number of available cores on computer
+$ make -j7
+$ sudo make install
+This process WILL TAKE FOREVER...
+
+$ printf '# OpenCV\nPKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/opencv3/lib/pkgconfig\nexport PKG_CONFIG_PATH\n' >> ~/.bashrc  
+$ source ~/.bashrc  
+
+Now Time to Install ZED Camera Drivers:
+Download ZED v1.1.0 SDK from: https://www.stereolabs.com/developers/release/archives/
+It is very important v1.1.0 is downloaded and not any other version
+
+First make sure ZED Camera is plugged into a POWERED USB 3.0 Slot
+$ ./ZED_SDK_Linux_x86_64_v1.1.0.run
+Press q to exit license agreement and accept all defaults
+
+Next you need to get the proper ZED ROS Wrapper:
+$ cd
+$ cd workspace/ros/catkin/src
+$ git clone https://github.com/chanbrown007/zed_ros_wrapper_OPT_update.git
+
+Finally Time To Install OPT:
+$ cd
+$ cd open_ptrack/scripts
+$ ./openptrack_install.sh
+Note sometimes you have to run ./openptrack_install.sh twice for it to fully install
+If there is a persistent error that appears to be linked with OpenCV it sometimes is then necessary to install OpenCV 2 in /usr/local (following same procedure as before) and make sure to add package configure (/usr/local/lib/pkgconfig) to path
+
+It is suggested to delete openp_track folder in home and then:
+$ ln -s ~/workspace/ros/catkin/src/open_ptrack ~/open_ptrack
+
+------------------RUNNING OPT WITH ZED-----------------------------------------------------
+To Run make Sure ZED IS PLUGGED IN:
+$ roslaunch tracking detection_and_tracking_zed.launch
+
+In a 2nd terminal, tracking can be fine tuned using:
+$ rosrun rqt_reconfigure rqt_reconfigure
+
+ALSO ZED Camera Settings Can Be Adjusted in workspace/ros/catkin/src/zed_ros_wrapper_OPT_update/launch/zed.launch:
+Resolution Parameter: 0 = 2K, 1 = 1080HD, 2 = 720HD, 3 = VGA
+Quality Parameter (Disparity Map Quality): 0 = none, 1 = performance, 2 = medium, 3 = high
+Frame Rate: 2K has max frame rate of 15fps, 1080 - 30fps and 720 - 60fps
+---------------------------------------------------------------------------------------------

detection/CMakeLists.txt

@@ -11,11 +11,12 @@ if(VTK_FOUND)
 	include (${VTK_USE_FILE})
 endif() 
 
-find_package(OpenCV REQUIRED)
+find_package(OpenCV 2.4 REQUIRED)
 include_directories(${OpenCV_INCLUDE_DIRS})
 link_directories(${OpenCV_LIB_DIR})
 add_definitions(${OpenCV_DEFINITIONS})
 
+
 find_package(Eigen REQUIRED)
 include_directories(${Eigen_INCLUDE_DIRS} include ${catkin_INCLUDE_DIRS})
 

detection/apps/ground_based_people_detector_node.cpp

@@ -74,6 +74,9 @@
 #include <dynamic_reconfigure/server.h>
 #include <detection/GroundBasedPeopleDetectorConfig.h>
 
+#include <cmath>
+#include <math.h> 
+
 using namespace opt_msgs;
 using namespace sensor_msgs;
 
@@ -115,6 +118,11 @@ bool background_subtraction;
 // Threshold on the ratio of valid points needed for ground estimation
 double valid_points_threshold;
 
+//ros::Publisher extractedRGB_cloud;
+//ros::Publisher beforeVoxelFilter_cloud;
+//ros::Publisher preProcessed_cloud;
+//ros::Publisher groundRemoval_cloud;
+
 void
 cloud_cb (const PointCloudT::ConstPtr& callback_cloud)
 {
@@ -143,6 +151,51 @@ updateBackgroundCallback (const std_msgs::String::ConstPtr & msg)
   }
 }
 
+/*
+//Publish Intermediate Point Cloud Calls
+void
+publishExtractRGBCloud(PointCloudT::Ptr& cloud)
+{
+  sensor_msgs::PointCloud2 output;
+  pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message
+  extractedRGB_cloud.publish(output);
+}
+
+void
+publishpreProcessedCloud(PointCloudT::Ptr& cloud)
+{
+  sensor_msgs::PointCloud2 output;
+  pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message
+  preProcessed_cloud.publish(output);
+}
+
+
+void
+publishBeforeVoxelFilterCloud(PointCloudT::Ptr& cloud)
+{
+  sensor_msgs::PointCloud2 output;
+  pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message
+  beforeVoxelFilter_cloud.publish(output);
+}
+
+void
+publishBeforeVoxelFilterDummyCloud(PointCloudT::Ptr& cloud)
+{
+  //sensor_msgs::PointCloud2 output;
+  //pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message
+  //beforeVoxelFilter_cloud.publish(output);
+}
+
+
+void
+publishgroundRemovalCloud(PointCloudT::Ptr& cloud)
+{
+  sensor_msgs::PointCloud2 output;
+  pcl::toROSMsg(*cloud, output); // Convert the point cloud to a ROS message
+  groundRemoval_cloud.publish(output);
+}
+*/
+
 void
 computeBackgroundCloud (int frames, float voxel_size, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud)
 {
@@ -155,7 +208,7 @@ computeBackgroundCloud (int frames, float voxel_size, std::string frame_id, ros:
   {
     // Point cloud pre-processing (downsampling and filtering):
     PointCloudT::Ptr cloud_filtered(new PointCloudT);
-    cloud_filtered = people_detector.preprocessCloud (cloud);
+    cloud_filtered = people_detector.preprocessCloud (cloud);//, &publishBeforeVoxelFilterDummyCloud);
 
     *background_cloud += *cloud_filtered;
     ros::spinOnce();
@@ -316,6 +369,11 @@ main (int argc, char** argv)
   ros::Publisher detection_pub;
   detection_pub= nh.advertise<DetectionArray>(output_topic, 3);
 
+	//extractedRGB_cloud = nh.advertise<sensor_msgs::PointCloud2> (pointcloud_topic + "/extractedRGB", 3);
+	//preProcessed_cloud = nh.advertise<sensor_msgs::PointCloud2> (pointcloud_topic + "/preProcessed", 3);
+	//groundRemoval_cloud = nh.advertise<sensor_msgs::PointCloud2> (pointcloud_topic + "/groundRemoval", 3);
+	//beforeVoxelFilter_cloud = nh.advertise<sensor_msgs::PointCloud2> (pointcloud_topic + "/beforeVoxelFilter", 3);
+
   Rois output_rois_;
   open_ptrack::opt_utils::Conversions converter;
 
@@ -407,10 +465,8 @@ main (int argc, char** argv)
     if (new_cloud_available_flag)
     {
 	  new_cloud_available_flag = false;
-
       // Convert PCL cloud header to ROS header:
       std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud->header);
-
       // If requested, update background:
       if (update_background)
       {
@@ -427,9 +483,11 @@ main (int argc, char** argv)
 
       // Perform people detection on the new cloud:
       std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
+
+
       people_detector.setInputCloud(cloud);
       people_detector.setGround(ground_coeffs);                    // set floor coefficients
-      people_detector.compute(clusters);                           // perform people detection
+      people_detector.compute(clusters);//, &publishExtractRGBCloud, &publishpreProcessedCloud , &publishgroundRemovalCloud, &publishBeforeVoxelFilterCloud);                // perform people detection
 
       // If not lock_ground, update ground coefficients:
       if (not lock_ground)

detection/apps/haardispada_node.cpp

@@ -60,8 +60,8 @@ POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/subscriber_filter.h>
 
 // Used to display OPENCV images
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
 
 // Dynamic reconfigure:
 #include <dynamic_reconfigure/server.h>

detection/apps/haardispada_node_sr.cpp

@@ -60,8 +60,8 @@ POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/subscriber_filter.h>
 
 // Used to display OPENCV images
-#include <opencv2/core/core.hpp>
-#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
 
 using namespace stereo_msgs;
 using namespace message_filters::sync_policies;

detection/conf/ground_based_people_detector_zed.yaml

@@ -0,0 +1,52 @@
+#######################
+## Ground estimation ##
+#######################
+# Ground estimation mode - 0:manual, 1:semi-auto, 2:auto with user validation, 3:fully automatic (default 0) 
+ground_estimation_mode: 3 
+# Flag stating if the ground should be read from file, if present:
+read_ground_from_file: false
+# Flag that locks the ground plane update:
+lock_ground: false
+# Threshold on the ratio of valid points needed for ground estimation:
+valid_points_threshold: 0.1
+
+############################
+## Background subtraction ##
+############################
+# Flag enabling/disabling background subtraction:
+background_subtraction: false
+# Resolution of the octree representing the background:
+background_resolution: 0.3
+# Seconds to use to learn the background:
+background_seconds: 3.0
+
+##############################################
+## Ground based people detection parameters ##
+##############################################
+# Minimum detection confidence:
+ground_based_people_detection_min_confidence: -5
+# Minimum person height:
+minimum_person_height: 0.5
+# Maximum person height:
+maximum_person_height: 2.3
+# Kinect's best range is 5.0 m, greater is possible w/ larger error
+max_distance: 10.0 
+# Point cloud downsampling factor: 4
+sampling_factor: 4
+# Flag stating if classifiers based on RGB image should be used or not:
+use_rgb: true
+# Threshold on image luminance. If luminance is over this threhsold, classifiers on RGB image are also used:
+minimum_luminance: 120
+# If true, sensor tilt angle wrt ground plane is compensated to improve people detection:
+sensor_tilt_compensation: flase  
+# Minimum distance between two persons:
+heads_minimum_distance: 0.3
+# Voxel size used to downsample the point cloud (lower: detection slower but more precise; higher: detection faster but less precise):
+voxel_size: .06
+# Denoising flag. If true, a statistical filter is applied to the point cloud to remove noise:
+apply_denoising: false
+# MeanK for denoising (the higher it is, the stronger is the filtering):
+mean_k_denoising: 5
+# Standard deviation for denoising (the lower it is, the stronger is the filtering):
+std_dev_denoising: 0.3
+

detection/conf/ground_zed_tracked_frame.txt

@@ -0,0 +1,4 @@
+2.94239e-07
+4.57398e-41
+1.49029e-38
+0

detection/include/open_ptrack/detection/ground_based_people_detection_app.h

@@ -317,7 +317,7 @@ namespace open_ptrack
          * \return The cloud after pre-processing.
          */
         PointCloudPtr
-        preprocessCloud (PointCloudPtr& input_cloud);
+        preprocessCloud (PointCloudPtr& input_cloud);//, void (*publishBeforeVoxelFilterCloud)(PointCloudPtr&));
 
         /**
          * \brief Perform people detection on the input data and return people clusters information.
@@ -327,7 +327,7 @@ namespace open_ptrack
          * \return true if the compute operation is successful, false otherwise.
          */
         bool
-        compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters);
+        compute (std::vector<pcl::people::PersonCluster<PointT> >& clusters);//, void (*publishExtractRGB)(PointCloudPtr&), void (*publishPreProcessed)(PointCloudPtr&), void (*publishGroundRemoval)(PointCloudPtr&), void (*publishBeforeVoxelFilterCloud)(PointCloudPtr&));
 
       protected:
         /** \brief sampling factor used to downsample the point cloud */