diff --git a/hlpr_audio_features/CMakeLists.txt b/hlpr_audio_features/CMakeLists.txt new file mode 100644 index 0000000..26b0ebd --- /dev/null +++ b/hlpr_audio_features/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hlpr_audio_features) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES hlpr_audio_features +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/hlpr_audio_features.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/hlpr_audio_features_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_hlpr_audio_features.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/hlpr_audio_features/package.xml b/hlpr_audio_features/package.xml new file mode 100644 index 0000000..4209e7f --- /dev/null +++ b/hlpr_audio_features/package.xml @@ -0,0 +1,68 @@ + + + hlpr_audio_features + 0.0.0 + The hlpr_audio_features package + + + + + eshort + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/hlpr_audio_features/setup.py b/hlpr_audio_features/setup.py new file mode 100644 index 0000000..be337e5 --- /dev/null +++ b/hlpr_audio_features/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['hlpr_audio_features'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/hlpr_audio_features/src/hlpr_audio_features/__init__.py b/hlpr_audio_features/src/hlpr_audio_features/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hlpr_audio_features/src/hlpr_audio_features/audio_features.py b/hlpr_audio_features/src/hlpr_audio_features/audio_features.py new file mode 100755 index 0000000..da92b66 --- /dev/null +++ b/hlpr_audio_features/src/hlpr_audio_features/audio_features.py @@ -0,0 +1,154 @@ +#!/usr/bin/env python + +# Copyright (c) 2017, Elaine Short, SIM Lab +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the SIM Lab nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import rospy +import pyaudio as pya +from std_msgs.msg import Float64MultiArray +import numpy as np +from scipy import stats +import struct +import threading +import argparse + +VERBOSE=False + +class AudioFeature(): + def __init__(self, print_only): + self._pa = pya.PyAudio() + d_info=None + d_index=None + + self._channels = 4 + #self._rate = 48000 + self._rate = 16000 + fmt = pya.paInt16 + self._timestep = 0.01 + self._frames_per_block = int(self._timestep*self._rate) + self._data = {} + self._data["data"]=[] + + found = False + + for i in range(self._pa.get_device_count()): + d_info = self._pa.get_device_info_by_index(i) + name = d_info["name"] + channels = d_info["maxInputChannels"] + if channels >0: + print i, ":", d_info["name"], channels + + if not found or print_only: + exit(-1) + + self._stream = self._pa.open(format=fmt, channels=self._channels, rate=self._rate, input=True, frames_per_buffer=self._frames_per_block, input_device_index=d_index) + self._intensity_pub = rospy.Publisher("/contingency/audio/intensity",ArrayFeature, queue_size=1) + self._flatness_pub = rospy.Publisher("/contingency/audio/flatness", ArrayFeature, queue_size=1) + self._band_pub = rospy.Publisher("/contingency/audio/bands", ArrayFeature, queue_size=1) + + + self._listen_thread=threading.Thread(target=self.listen) + self._listen_thread.start() + + def get_features(self,data): + data = np.reshape(data, [4,-1],"F") + #print data + band_features = [[] for i in range(5)] + flatness_features = [] + amp_features = [] + for k in range(4): + fft = np.fft.fft(data[k]) + spectrum = abs(fft)**2 + freqs = abs(np.fft.fftfreq(len(data[k]),d=1.0/self._rate)) + + bands = [0,300,1200,2100,3000,24000] + + band_energies = [0 for b in range(len(bands)-1)] + total_energy = sum(spectrum) + + for j in range(len(freqs)): + for i in range(len(bands)-1): + if freqs[j]>=bands[i] and freqs[j] for more details on the algorithm. + +# Test the face detection package +```` +Without GPU: roslaunch hlpr_face_detection face_detector.launch +With GPU (more accurate version): roslaunch hlpr_face_detection face_detector_gpu.launch +```` + + diff --git a/hlpr_face_detection/launch/face_detector.launch b/hlpr_face_detection/launch/face_detector.launch new file mode 100644 index 0000000..f08e2a0 --- /dev/null +++ b/hlpr_face_detection/launch/face_detector.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/hlpr_face_detection/launch/face_detector_gpu.launch b/hlpr_face_detection/launch/face_detector_gpu.launch new file mode 100644 index 0000000..4bd706f --- /dev/null +++ b/hlpr_face_detection/launch/face_detector_gpu.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/hlpr_face_detection/msg/FaceDetectionTopic.msg b/hlpr_face_detection/msg/FaceDetectionTopic.msg new file mode 100644 index 0000000..5794032 --- /dev/null +++ b/hlpr_face_detection/msg/FaceDetectionTopic.msg @@ -0,0 +1 @@ +std_msgs/Float32MultiArray[] faces diff --git a/hlpr_face_detection/package.xml b/hlpr_face_detection/package.xml new file mode 100644 index 0000000..6b67a23 --- /dev/null +++ b/hlpr_face_detection/package.xml @@ -0,0 +1,74 @@ + + + hlpr_face_detection + 1.0.0 + The face_detection package + + + + + Akanksha Saran + Akanksha Saran + + + + + + BSD + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + message_generation + rospy + std_msgs + cv_bridge + image_transport + sensor_msgs + + rospy + + rospy + std_msgs + cv_bridge + image_transport + message_runtime + sensor_msgs + + + + + + + diff --git a/hlpr_face_detection/setup.py b/hlpr_face_detection/setup.py new file mode 100644 index 0000000..0607335 --- /dev/null +++ b/hlpr_face_detection/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['hlpr_face_detection'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/hlpr_face_detection/src/__init__.py b/hlpr_face_detection/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hlpr_face_detection/src/face_detector.py b/hlpr_face_detection/src/face_detector.py new file mode 100755 index 0000000..39f8125 --- /dev/null +++ b/hlpr_face_detection/src/face_detector.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python +import rospy +import sys +import cv2 +from std_msgs.msg import String, Float32MultiArray, Bool +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image, PointCloud2, CompressedImage +from hlpr_face_detection.msg import FaceDetectionTopic +import face_recognition + +class FaceDetector: + def __init__(self, tgtdir='.'): + print "Starting Init for Face Detector" + self.face_detector_pub = rospy.Publisher("face_detections",FaceDetectionTopic,queue_size=10) + # rospy.sleep(2) + self.bridge = CvBridge() + self.rgb_image = None + self.tgtdir = tgtdir + detection_sub = rospy.Subscriber("image_topic", Image, self.callback, queue_size=1, buff_size=52428800) + + rospy.sleep(5) + print "Finished Init for Face Detector" + + + def callback(self,rgb): + self.rgb_image = rgb + print "callback" + cv_image = self.bridge.imgmsg_to_cv2(rgb, "bgr8") + + faces = self.detect_faces(cv_image) + msg = FaceDetectionTopic() + msg.faces = faces + # print "faces {0}".format(faces) + self.face_detector_pub.publish(msg) + + + def detect_faces(self, img): + use_gpu = rospy.get_param("/use_gpu") + if(use_gpu): + faces = face_recognition.face_locations(img, number_of_times_to_upsample=1, model="cnn") + else: + faces = face_recognition.face_locations(img, number_of_times_to_upsample=1) + multi_array = [] + for face in faces: + face_array = Float32MultiArray() + face_array.data = face + multi_array.append(face_array) + + return multi_array + +if __name__ == '__main__': + rospy.init_node('face_detector', anonymous=True) + obj = FaceDetector() + while not rospy.is_shutdown(): + rospy.spin() + diff --git a/hlpr_feature_extraction/CMakeLists.txt b/hlpr_feature_extraction/CMakeLists.txt index 687a556..bd7e1e5 100644 --- a/hlpr_feature_extraction/CMakeLists.txt +++ b/hlpr_feature_extraction/CMakeLists.txt @@ -6,15 +6,6 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so - /opt/ros/${ROS_DISTRO}/lib/libtf2.so - /opt/ros/${ROS_DISTRO}/lib/libtf2_ros.so) - #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) find_package(OpenCL QUIET) @@ -24,23 +15,18 @@ IF(OpenCL_FOUND) ${LIBRARIES} ) ENDIF(OpenCL_FOUND) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs tf2_ros hlpr_segmentation) -find_package(PCL 1.8 REQUIRED COMPONENTS) -find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) -add_message_files( - FILES - PcFeatures.msg - PcFeatureArray.msg -) - -generate_messages( - DEPENDENCIES +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp std_msgs geometry_msgs + tf2_ros hlpr_segmentation + pcl_ros ) +find_package(freenect2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #generate_dynamic_reconfigure_options( # cfg/segmentation.cfg @@ -48,7 +34,7 @@ generate_messages( catkin_package( # CATKIN_DEPENDS dynamic_reconfigure message_runtime roscpp std_msgs geometry_msgs - CATKIN_DEPENDS message_runtime roscpp std_msgs geometry_msgs hlpr_segmentation + CATKIN_DEPENDS message_runtime roscpp std_msgs geometry_msgs hlpr_perception_msgs ) include_directories(${PCL_INCLUDE_DIRS}) @@ -60,13 +46,13 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(ftex SHARED src/cluster_processing.cpp src/rotcalipers.cpp src/utils.cpp src/utils_pcl_ros.cpp) -target_link_libraries(ftex ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(ftex ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_dependencies(ftex hlpr_feature_extraction_generate_messages_cpp) add_executable(ft_ex src/feature_extraction.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(ft_ex ${PCL_LIBRARIES} ftex ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(ft_ex ${PCL_LIBRARIES} ftex ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) add_dependencies(ft_ex hlpr_feature_extraction_generate_messages_cpp) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) diff --git a/hlpr_feature_extraction/include/objectFeatures.hpp b/hlpr_feature_extraction/include/objectFeatures.hpp index 6e63c08..d432250 100644 --- a/hlpr_feature_extraction/include/objectFeatures.hpp +++ b/hlpr_feature_extraction/include/objectFeatures.hpp @@ -8,6 +8,8 @@ #define OBJECTFEATURES_HPP_ #include +#include +#include #include #include @@ -22,10 +24,10 @@ #include #include #include +#include //use eigen data structures? - class pc_cluster_features { private: @@ -36,46 +38,65 @@ class pc_cluster_features } - void boundingBoxWithZ(pcl::PointCloud &cluster) + // Added by Priyanka - for non-planar segmentation + void orientedBoundingBox(pcl::PointCloud &cluster) { - pcl::PointCloud::Ptr projected_cluster = cluster.makeShared(); - for(size_t j = 0; j < projected_cluster->points.size(); j++) - projected_cluster->points[j].z = 1; - - volume2 = cluster.size(); - - pcl::getMinMax3D(cluster, min, max); - aligned_bounding_box = minAreaRect(projected_cluster); - aligned_bounding_box.center.z = min[2]+(max[2]-min[2])/2; //this should be maxheight/2+planeHeight - aligned_bounding_box.size.zSize = fabs((float)(max[2]-min[2])); //this should be max height! - - aligned_bounding_box.fillQuatGivenAxisAngle(); - } + pcl::PointCloud::Ptr projected_cluster = cluster.makeShared(); + volume2 = cluster.size(); + pcl::getMinMax3D(cluster, min, max); + + std::vector moment_of_inertia; + std::vector eccentricity; + pcl::PointXYZ min_point_OBB; + pcl::PointXYZ max_point_OBB; + pcl::PointXYZ position_OBB; + Eigen::Matrix3f rotational_matrix_OBB; + //Eigen::Vector3f major_vector, middle_vector, minor_vector; + + pcl::PointCloud::Ptr cluster_xyzrgb (projected_cluster); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::copyPointCloud(*cluster_xyzrgb, *cloud); + pcl::MomentOfInertiaEstimation feature_extractor; + feature_extractor.setInputCloud(cloud); + feature_extractor.compute(); + + //feature_extractor.getMomentOfInertia (moment_of_inertia); + //feature_extractor.getEccentricity (eccentricity); + feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); + //feature_extractor.getEigenValues(major_value, middle_value, minor_value); + //feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector); + //feature_extractor.getMassCenter(mass_center); + + // Fill up the Box3D structure + Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z); + oriented_bounding_box.center.x = position(0); + oriented_bounding_box.center.y = position(1); + oriented_bounding_box.center.z = position(2); + + Eigen::Quaternion quat (rotational_matrix_OBB); + oriented_bounding_box.rot_quat[0] = quat.x(); + oriented_bounding_box.rot_quat[1] = quat.y(); + oriented_bounding_box.rot_quat[2] = quat.z(); + oriented_bounding_box.rot_quat[3] = quat.w(); + + oriented_bounding_box.size.xSize = max_point_OBB.x - min_point_OBB.x; + std::cout<<"xSize : "< &cluster, pcl::ModelCoefficients::Ptr coefficients) { pcl::PointCloud::Ptr projected_cluster = cluster.makeShared(); - pcl::PointCloud::Ptr cloud_transformed (new pcl::PointCloud); - /*pcl::PointCloud::Ptr cloud_projected (new pcl::PointCloud); - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (cluster.makeShared()); - proj.setModelCoefficients (coefficients); - proj.filter (*cloud_projected);*/ - Eigen::Vector3f z_axis(0,0,1); Eigen::Vector3f plane_normal(coefficients->values[0],coefficients->values[1],coefficients->values[2]); -// Eigen::Vector3f plane_normal(0.0270649, 0.849503, 0.526889); - plane_normal.normalize(); - Eigen::Vector3f c = plane_normal.cross(z_axis);//z_axis.cross(plane_normal); - c.normalize(); - - //std::cout << c << std::endl; - //std::cout << plane_normal << std::endl << std::endl; + c.normalize(); double cost = plane_normal.dot(z_axis); double half_theta = acos(cost)/2; @@ -86,55 +107,64 @@ class pc_cluster_features Eigen::Affine3f T = Eigen::Affine3f::Identity(); T.rotate(R); - //std::cout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " << std::endl; - pcl::transformPointCloud (*projected_cluster, *cloud_transformed, T); volume2 = cluster.size(); pcl::getMinMax3D(*cloud_transformed, min, max); for(size_t j = 0; j < cloud_transformed->points.size(); j++) - cloud_transformed->points[j].z = 1; + cloud_transformed->points[j].z = 1; - aligned_bounding_box = minAreaRect(cloud_transformed); - - - Eigen::Quaternion q2(cos(-aligned_bounding_box.angle/2), 0, 0, sin(-aligned_bounding_box.angle/2)); + oriented_bounding_box = minAreaRect(cloud_transformed); + Eigen::Quaternion q2(cos(-oriented_bounding_box.angle/2), 0, 0, sin(-oriented_bounding_box.angle/2)); Eigen::Quaternion q3 = q2*q; q3 = q3.inverse(); - //pcl::getMinMax3D(cluster, min, max); - //aligned_bounding_box = minAreaRect(projected_cluster); - - /*for(int i = 0; i< 3 ; i++) - aligned_bounding_box.rot_axis[i] = c[i]; - aligned_bounding_box.fillQuatGivenAxisAngle();*/ + oriented_bounding_box.center.z = min[2]+(max[2]-min[2])/2; //this should be maxheight/2+planeHeight + oriented_bounding_box.size.zSize = fabs((float)(max[2]-min[2])); //this should be max height! - aligned_bounding_box.center.z = min[2]+(max[2]-min[2])/2; //this should be maxheight/2+planeHeight - aligned_bounding_box.size.zSize = fabs((float)(max[2]-min[2])); //this should be max height! - - Eigen::Vector3f X(aligned_bounding_box.center.x,aligned_bounding_box.center.y,aligned_bounding_box.center.z); + Eigen::Vector3f X(oriented_bounding_box.center.x,oriented_bounding_box.center.y,oriented_bounding_box.center.z); Eigen::Vector3f Y(0,0,0); Y = T.inverse()*X; - aligned_bounding_box.center.x = Y.x(); - aligned_bounding_box.center.y = Y.y(); - aligned_bounding_box.center.z = Y.z(); + oriented_bounding_box.center.x = Y.x(); + oriented_bounding_box.center.y = Y.y(); + oriented_bounding_box.center.z = Y.z(); + oriented_bounding_box.rot_quat[0] = q3.x(); + oriented_bounding_box.rot_quat[1] = q3.y(); + oriented_bounding_box.rot_quat[2] = q3.z(); + oriented_bounding_box.rot_quat[3] = q3.w(); + } + + // Added by Priyanka - for planes + void planeConst(pcl::PointCloud &contour) + { + plane_cloud = contour.makeShared(); - /*for(int i = 0; i < 3; i++) - aligned_bounding_box.rot_axis[i] = c[i];//coefficients->values[i]; + float r = 0.0f; + float g = 0.0f; + float b = 0.0f; + float n = static_cast (contour.size()); + for (size_t j = 0; j < n; j++) + { + r += (float) (contour[j].r); + g += (float) (contour[j].g); + b += (float) (contour[j].b); + } + r /= n; g /= n; b /= n; - aligned_bounding_box.angle = //half_theta*2; + hue = rgb2hue(r,g,b); - aligned_bounding_box.fillQuatGivenAxisAngle();*/ + //std::cout << rgb2hue(r,g,b) << endl; - aligned_bounding_box.rot_quat[0] = q3.x(); - aligned_bounding_box.rot_quat[1] = q3.y(); - aligned_bounding_box.rot_quat[2] = q3.z(); - aligned_bounding_box.rot_quat[3] = q3.w(); + int ri = r; int gi = (int)g; int bi = (int)b; + color[0] = (unsigned char) ri; + color[1] = (unsigned char) gi; + color[2] = (unsigned char) bi; + fill(); } void clusterConst(pcl::PointCloud &cluster) @@ -186,7 +216,9 @@ class pc_cluster_features pcl::compute3DCentroid(cluster, centroid); - boundingBoxWithZ(cluster); + orientedBoundingBox(cluster); // Changed by Priyanka for non-planar segmentation + + //boundingBoxWithZ(cluster); //BARIS: PROJECT HERE // pcl::transformPointCloud (cluster, *projected_cluster, plane_transformation); @@ -202,8 +234,8 @@ class pc_cluster_features // projected_cluster->points[j].z = 1; clusterConst(cluster); - } + pc_cluster_features(pcl::PointCloud &cluster, Eigen::Vector4f &used_plane_model) { defaultConst(); @@ -225,18 +257,31 @@ class pc_cluster_features if(zero_count > 3) use_projection = false; - if(use_projection) + //if(use_projection) boundingBoxWithCoeff(cluster, coefficients); - else - boundingBoxWithZ(cluster); + //else + // boundingBoxWithZ(cluster); clusterConst(cluster); } + pc_cluster_features(pcl::PointCloud::Ptr &contour){ + defaultConst(); + + pcl::compute3DCentroid(*contour, centroid); + + orientedBoundingBox(*contour); + + planeConst(*contour); + + orientedBoundingBox(*contour); + } + ~pc_cluster_features(){} - Box3D aligned_bounding_box; + //Box3D aligned_bounding_box; + Box3D oriented_bounding_box; //Box3D aligned_bounding_box2; //FEATURES @@ -248,7 +293,7 @@ class pc_cluster_features //Eigen::Vector3f color; ColorVec color; float hue; - + Eigen::Vector4f centroid; Eigen::Quaternionf rotation_quat; // kind of hard to get full orientation, need to fit a box which is not trivial! n^3, needs 3d convez hull and hard to implement, approximations exist @@ -257,7 +302,7 @@ class pc_cluster_features //assuming on table, coming from the bounding box float bb_aspect_ratio; - float bb_orientation; //wrt to the table, use a complex number? + //float bb_orientation; //wrt to the table, use a complex number? float compactness; // volume/bb_volume need a better name? float av_ratio; // area/volume @@ -268,24 +313,46 @@ class pc_cluster_features float objectBuffer[5000]; pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr plane_cloud; pcl::PointCloud::Ptr parent_cloud; pcl::PointIndices indices; //indices in the parent point cloud pcl::PointCloud normals; + + bool setViewpointHist = true; // boolean to check if viewpoint Histogram is being computed and set (Default - true) pcl::PointCloud vfhs;// (new pcl::PointCloud ()); - + + bool setShapeHist = true; // boolean to check if shape histogram is being computed and set (Default - true) + pcl::PointCloud cvfhs; // (new pcl::PointCloud ()); + pcl::PointCloud fpfhs; //(new pcl::PointCloud ()); + + // Normalized cvfh feature vector? + //std::vector histogram_cvfh_vector (308); + + // Normalized fpfh feature vector? + //std::vector histogram_fpfh_vector (308); + + // Normalized hue-saturation histogram + bool setColorHist = true; // boolean to check if color Histogram is being computed and set (Default - true) + std::vector histogram_hs; + + // Other features (default is set to false) + bool setOtherFeatures = false; + std::vector otherFeatures; // Vector to store the other features + //Image based features??? blob features, color histogram? using opencv void fill() { - aligned_bounding_box.calculateProperties(); + oriented_bounding_box.calculateProperties(); //aligned_bounding_box.fillQuatGivenAxisAngle(); - bb_orientation = aligned_bounding_box.angle; - bb_volume = aligned_bounding_box.volume; - bb_area = aligned_bounding_box.area; - bb_aspect_ratio = aligned_bounding_box.aspect_ratio; - rotation_quat = Eigen::Quaternionf(aligned_bounding_box.rot_quat); - + //bb_orientation = aligned_bounding_box.angle; + bb_volume = oriented_bounding_box.volume; + bb_area = oriented_bounding_box.area; + bb_aspect_ratio = oriented_bounding_box.aspect_ratio; + rotation_quat = Eigen::Quaternionf(oriented_bounding_box.rot_quat); + + //NOTE: compactness is not valid for planes compactness = (float)bb_volume/volume2; //where is the real area/real volume? av_ratio = bb_area/bb_volume; @@ -299,10 +366,10 @@ class pc_cluster_features out_features[feature_counter++] = numFeatures; - out_features[feature_counter++] = aligned_bounding_box.center.x; - out_features[feature_counter++] = aligned_bounding_box.center.y; - out_features[feature_counter++] = aligned_bounding_box.center.z; - out_features[feature_counter++] = aligned_bounding_box.angle; + out_features[feature_counter++] = oriented_bounding_box.center.x; + out_features[feature_counter++] = oriented_bounding_box.center.y; + out_features[feature_counter++] = oriented_bounding_box.center.z; + //out_features[feature_counter++] = oriented_bounding_box.angle; for(int i = 0;i<3;feature_counter++,i++) out_features[feature_counter] = centroid[i]; @@ -320,9 +387,9 @@ class pc_cluster_features out_features[feature_counter++] = volume2; - out_features[feature_counter++] = aligned_bounding_box.size.xSize; - out_features[feature_counter++] = aligned_bounding_box.size.ySize; - out_features[feature_counter++] = aligned_bounding_box.size.zSize; + out_features[feature_counter++] = oriented_bounding_box.size.xSize; + out_features[feature_counter++] = oriented_bounding_box.size.ySize; + out_features[feature_counter++] = oriented_bounding_box.size.zSize; out_features[feature_counter++] = bb_volume; out_features[feature_counter++] = bb_area; @@ -400,5 +467,96 @@ for(size_t j = 0; j < projected_cluster->points.size(); j++) projected_cluster->points[j].z = projected_cluster->points[j].z - coefficients->values[2]*t; }*/ +typedef pcl::PointCloud PointCloudT; +typedef unsigned int uint; + +// Histogram of hue and saturation +class HSHistogram{ + private: + int dim; + public: + HSHistogram(int dim):dim(dim){ } // empty constructor -> just sets the dimension of the histogram + + void computeHistogram(PointCloudT &cloud, std::vector > &hist2, std::vector &hist2_double_vector){ + int cloud_size = cloud.points.size(); + + for(int i = 0; i < cloud_size; i++){ + int r = (int)(double)(cloud.points[i].r); + int g = (int)(double)(cloud.points[i].g); + int b = (int)(double)(cloud.points[i].b); + + // convert the rgb value to HS value + std::pair HS = rgb2hue(r,g,b); + //std::cout<<"Value of hue: "<(HS)<< " sat: "<(HS)<(HS) / round); // Get a bin for hue from 0-15 + int s = (int) (std::get<1>(HS) * (dim-1)); // Get a bin for saturation from 0-15 + + //std::cout<<"Value of h: "< rgb2hue(int r, int g, int b){ + const unsigned char max = std::max (r, std::max (g, b)); + const unsigned char min = std::min (r, std::min (g, b)); + + float hue; + float sat; + + const float diff = static_cast (max - min); + + if (max == 0) // division by zero + { + sat = 0; + //hue undefined! set to your favorite value (any value with zero saturation will produce black) + hue = 0; + } + else + { + sat = diff/max; + + if (min == max) // diff == 0 -> division by zero + { + sat = 0; + //hue undefined! set to your favorite value (black) + hue = 0; + } + else + { + if (max == r) hue = 60.f * ( static_cast (g - b) / diff); + else if (max == g) hue = 60.f * (2.f + static_cast (b - r) / diff); + else hue = 60.f * (4.f + static_cast (r - g) / diff); // max == b + + if (hue < 0.f) hue += 360.f; + } + } + + //if (sat < ridiculous_global_variables::saturation_threshold && ridiculous_global_variables::ignore_low_sat) + //hue = ridiculous_global_variables::saturation_mapped_value; //hackzz oh the hackz + + // Return both hue and saturation as a pair + std::pair returnHS = std::make_pair(hue,sat); + + return returnHS; + } +}; + #endif /* OBJECTFEATURES_HPP_ */ diff --git a/hlpr_feature_extraction/include/utils.hpp b/hlpr_feature_extraction/include/utils.hpp index ca6bae4..8aac49d 100644 --- a/hlpr_feature_extraction/include/utils.hpp +++ b/hlpr_feature_extraction/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_feature_extraction/include/utils_pcl_ros.hpp b/hlpr_feature_extraction/include/utils_pcl_ros.hpp index 3038867..5cd7c17 100644 --- a/hlpr_feature_extraction/include/utils_pcl_ros.hpp +++ b/hlpr_feature_extraction/include/utils_pcl_ros.hpp @@ -17,7 +17,11 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include /* @@ -40,17 +44,31 @@ void Cube_2_Arrows(pcl::ModelCoefficients &cube, boost::shared_ptr &viewer, int index); Box3D -//boundingBoxWithCoeff(pcl::PointCloud &cluster, pcl::ModelCoefficients::Ptr coefficients); boundingBoxWithCoeff(pcl::PointCloud &cluster, pcl::ModelCoefficients::Ptr coefficients, pcl::PointCloud::Ptr &cloud_transformed); void -fillRosMessage (hlpr_feature_extraction::PcFeatures &outRosMsg, const pc_cluster_features &inObjFeatures); +fillBasicFeaturesMsg (hlpr_perception_msgs::BasicFeatures &basicInfo, const pc_cluster_features &inObjFeatures); + +void +fillOrientedBoundingBoxMsg (hlpr_perception_msgs::OrientedBoundingBox &obb, const pc_cluster_features &inObjFeatures); + +void +fillRosMessageForObjects (hlpr_perception_msgs::ObjectFeatures &objRosMsg, + const pc_cluster_features &inObjFeatures); + +// Top-level message for Objects +void +fillObjectFeaturesMsg (hlpr_perception_msgs::ObjectFeatures &objRosMsg, + hlpr_perception_msgs::BasicFeatures &basicInfo, + hlpr_perception_msgs::OrientedBoundingBox &obb, + hlpr_perception_msgs::OtherFeatures &other, + const pc_cluster_features &inObjFeatures); void objectPoseTF(geometry_msgs::Transform geom_transform); void -getObjectMarker(visualization_msgs::Marker &marker, hlpr_feature_extraction::PcFeatures &feats); +getObjectMarker(visualization_msgs::Marker &marker, hlpr_perception_msgs::ObjectFeatures &feats); template diff --git a/hlpr_feature_extraction/msg/PcFeatureArray.msg b/hlpr_feature_extraction/msg/PcFeatureArray.msg deleted file mode 100644 index 02b6a9e..0000000 --- a/hlpr_feature_extraction/msg/PcFeatureArray.msg +++ /dev/null @@ -1,5 +0,0 @@ -# PcFeatures for a single cluster - -Header header -PcFeatures[] objects -geometry_msgs/Transform[] transforms diff --git a/hlpr_feature_extraction/msg/PcFeatures.msg b/hlpr_feature_extraction/msg/PcFeatures.msg deleted file mode 100644 index abfb251..0000000 --- a/hlpr_feature_extraction/msg/PcFeatures.msg +++ /dev/null @@ -1,36 +0,0 @@ -# PcFeatures for a single cluster - -Header header - -# Object transform, however calculated -geometry_msgs/Transform transform - - -################################# -# Raw point Related - -# Cluster centroid, min, max and number of points -geometry_msgs/Vector3 points_centroid -geometry_msgs/Vector3 points_min # -geometry_msgs/Vector3 points_max # -float64 num_points - -# Average color (RGBA nad hue) -std_msgs/ColorRGBA rgba_color -float64 hue - -################################# -#Bounding box - -#position wrt sensor and angle wrt table normal -geometry_msgs/Vector3 bb_center -float64 bb_angle - -# Bounding box dimensions -geometry_msgs/Vector3 bb_dims - -################################# -#Other such as vfh, color hist etc. Unpacking will be on the other side - -uint32 other_features_size -float64[] data \ No newline at end of file diff --git a/hlpr_feature_extraction/package.xml b/hlpr_feature_extraction/package.xml index 3fc5145..ae18cb6 100644 --- a/hlpr_feature_extraction/package.xml +++ b/hlpr_feature_extraction/package.xml @@ -15,5 +15,5 @@ rospy std_msgs geometry_msgs - hlpr_segmentation + hlpr_perception_msgs diff --git a/hlpr_feature_extraction/src/cluster_processing.cpp b/hlpr_feature_extraction/src/cluster_processing.cpp index e865bc2..d458202 100644 --- a/hlpr_feature_extraction/src/cluster_processing.cpp +++ b/hlpr_feature_extraction/src/cluster_processing.cpp @@ -312,7 +312,7 @@ void OpenNIOrganizedMultiPlaneSegmentation::featureExtraction( //Feature Extraction pc_cluster_features feat(cluster, used_plane_model); - fittedBoxes.push_back(feat.aligned_bounding_box); + fittedBoxes.push_back(feat.oriented_bounding_box); cluster_colors.push_back(feat.color); vfh.setInputCloud (cluster.makeShared()); diff --git a/hlpr_feature_extraction/src/feature_extraction.cpp b/hlpr_feature_extraction/src/feature_extraction.cpp index 07f305c..7769720 100644 --- a/hlpr_feature_extraction/src/feature_extraction.cpp +++ b/hlpr_feature_extraction/src/feature_extraction.cpp @@ -20,9 +20,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -108,7 +108,7 @@ cloud_cb_ros_ (const sensor_msgs::PointCloud2ConstPtr& msg) void -cluster_cb (const hlpr_segmentation::SegmentedClusters& msg) +cluster_cb (const hlpr_perception_msgs::SegClusters& msg) { clusters.clear(); for(int i = 0; i < msg.clusters.size(); i++) { @@ -202,7 +202,7 @@ main (int argc, char **argv) std::cout << "Publishing ros topic: " << outRostopic << std::endl; //pub = nh->advertise(outRostopic,5); - pub = nh->advertise(outRostopic,5); + pub = nh->advertise(outRostopic,5); objectPub = nh->advertise(objectPCOutRostopic,5); //transformPub = nh->advertise(transformRostopic,5); @@ -238,16 +238,16 @@ main (int argc, char **argv) if(selected_cluster_index < 0) continue; - float angle = feats[selected_cluster_index].aligned_bounding_box.angle; + float angle = feats[selected_cluster_index].oriented_bounding_box.angle; //std::cout << "Selected cluster angle (rad, deg): " << angle << " " << angle*180.0/3.14159 << std::endl; //std::cout << "Selected cluster hue: " << feats[selected_cluster_index].hue << std::endl; - hlpr_feature_extraction::PcFeatureArray rosMsg; + hlpr_perception_msgs::ExtractedFeaturesArray rosMsg; rosMsg.header.stamp = ros::Time::now(); for(int i = 0; i < feats.size(); i++) { - hlpr_feature_extraction::PcFeatures ft; - fillRosMessage(ft, feats[i]); + hlpr_perception_msgs::ObjectFeatures ft; + fillRosMessageForObjects(ft, feats[i]); rosMsg.objects.push_back(ft); rosMsg.transforms.push_back(ft.transform); } diff --git a/hlpr_feature_extraction/src/utils_pcl_ros.cpp b/hlpr_feature_extraction/src/utils_pcl_ros.cpp index bf06e6c..f23b369 100644 --- a/hlpr_feature_extraction/src/utils_pcl_ros.cpp +++ b/hlpr_feature_extraction/src/utils_pcl_ros.cpp @@ -45,18 +45,117 @@ Cube_2_Arrows(pcl::ModelCoefficients &cube, boost::shared_ptrstd_msgs geometry_msgs sensor_msgs - hlpr_feature_extraction + hlpr_perception_msgs diff --git a/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py b/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py index 8a7a45c..0fda976 100755 --- a/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py +++ b/hlpr_knowledge_retrieval/src/object_knowledge_retrieval.py @@ -7,8 +7,7 @@ import roslib import rospy -from hlpr_object_labeling.msg import LabeledObjects -from hlpr_knowledge_retrieval.msg import ObjectKnowledge, ObjectKnowledgeArray +from hlpr_perception_msgs.msg import LabeledObjects, ObjectKnowledge, ObjectKnowledgeArray def get_param(name, value=None): private = "~%s" % name @@ -24,17 +23,16 @@ def __init__(self): self.subscriber = rospy.Subscriber("/beliefs/labels", LabeledObjects, self.cbLabels, queue_size = 1) self.knowPub = rospy.Publisher("/beliefs/knowledge", ObjectKnowledge) - fileref = get_param("data_file_location") - if fileref is not None: - self.filename = os.path.expanduser(fileref) - self.readObjectKnowledge(self.filename) - print "Reading knowledge data from " + self.filename - else: - self.filename = None topicref = get_param("data_file_rostopic") + fileref = get_param("data_file_location") + self.filename = None if topicref is not None: self.rostopic = os.path.expanduser(topicref) self.fileSub = rospy.Subscriber(self.rostopic, String, self.cbFile, queue_size = 1) + elif fileref is not None: + self.filename = os.path.expanduser(fileref) + self.readObjectKnowledge(self.filename) + print "Reading knowledge data from " + self.filename def cbFile(self, ros_data): if self.filename is not ros_data.data: diff --git a/hlpr_nonplanar_feature_extraction/CMakeLists.txt b/hlpr_nonplanar_feature_extraction/CMakeLists.txt index 8793ce0..7b3ccf2 100644 --- a/hlpr_nonplanar_feature_extraction/CMakeLists.txt +++ b/hlpr_nonplanar_feature_extraction/CMakeLists.txt @@ -6,20 +6,19 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so - /opt/ros/${ROS_DISTRO}/lib/libtf2.so - /opt/ros/${ROS_DISTRO}/lib/libtf2_ros.so) #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs tf2_ros hlpr_perception_msgs) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + roscpp + message_generation + roscpp + std_msgs geometry_msgs + tf2_ros + hlpr_perception_msgs + pcl_ros +) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #generate_dynamic_reconfigure_options( # cfg/segmentation.cfg @@ -39,13 +38,13 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(nonplanarftex SHARED src/cluster_processing.cpp src/rotcalipers.cpp src/utils.cpp src/utils_pcl_ros.cpp) -target_link_libraries(nonplanarftex ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(nonplanarftex ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_dependencies(nonplanarftex hlpr_perception_msgs_generate_messages_cpp) add_executable(nonplanar_ft_ex src/feature_extraction.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(nonplanar_ft_ex ${PCL_LIBRARIES} nonplanarftex ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(nonplanar_ft_ex ${PCL_LIBRARIES} nonplanarftex ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) add_dependencies(nonplanar_ft_ex hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_nonplanar_feature_extraction/include/utils.hpp b/hlpr_nonplanar_feature_extraction/include/utils.hpp index 7e8645d..18fd43d 100644 --- a/hlpr_nonplanar_feature_extraction/include/utils.hpp +++ b/hlpr_nonplanar_feature_extraction/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_nonplanar_feature_extraction/package.xml b/hlpr_nonplanar_feature_extraction/package.xml index 1b46666..c8e6a09 100644 --- a/hlpr_nonplanar_feature_extraction/package.xml +++ b/hlpr_nonplanar_feature_extraction/package.xml @@ -15,4 +15,5 @@ tf2_ros std_msgs geometry_msgs + hlpr_perception_msgs diff --git a/hlpr_nonplanar_segmentation/CMakeLists.txt b/hlpr_nonplanar_segmentation/CMakeLists.txt index 80d3ee8..791ee71 100644 --- a/hlpr_nonplanar_segmentation/CMakeLists.txt +++ b/hlpr_nonplanar_segmentation/CMakeLists.txt @@ -6,18 +6,17 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so) - #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + geometry_msgs + sensor_msgs + hlpr_perception_msgs + pcl_ros +) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #generate_dynamic_reconfigure_options( # cfg/segmentation.cfg @@ -36,12 +35,12 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(nonplanarseg SHARED src/pc_segmentation.cpp src/utils.cpp) -target_link_libraries(nonplanarseg ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(nonplanarseg ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_executable(nonplanar_seg src/segmentation.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(nonplanar_seg ${PCL_LIBRARIES} nonplanarseg ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBRARIES}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(nonplanar_seg ${PCL_LIBRARIES} nonplanarseg ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) add_dependencies(nonplanar_seg hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_nonplanar_segmentation/include/k2g.h b/hlpr_nonplanar_segmentation/include/k2g.h index 5fecec4..97665ea 100644 --- a/hlpr_nonplanar_segmentation/include/k2g.h +++ b/hlpr_nonplanar_segmentation/include/k2g.h @@ -248,8 +248,8 @@ class K2G { libfreenect2::SyncMultiFrameListener listener_; libfreenect2::FrameMap frames_; libfreenect2::Frame undistorted_, registered_, big_mat_; - Eigen::Matrix colmap; - Eigen::Matrix rowmap; + Eigen::Matrix colmap; + Eigen::Matrix rowmap; std::string serial_; int map_[512 * 424]; float qnan_; diff --git a/hlpr_nonplanar_segmentation/include/utils.hpp b/hlpr_nonplanar_segmentation/include/utils.hpp index a282e50..89348fd 100644 --- a/hlpr_nonplanar_segmentation/include/utils.hpp +++ b/hlpr_nonplanar_segmentation/include/utils.hpp @@ -394,7 +394,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -406,7 +406,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -430,7 +430,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -447,7 +447,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -491,7 +491,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -502,7 +502,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_nonplanar_segmentation/package.xml b/hlpr_nonplanar_segmentation/package.xml index fd4f40a..febae10 100644 --- a/hlpr_nonplanar_segmentation/package.xml +++ b/hlpr_nonplanar_segmentation/package.xml @@ -15,4 +15,5 @@ std_msgs geometry_msgs sensor_msgs + hlpr_perception_msgs diff --git a/hlpr_object_labeling/CMakeLists.txt b/hlpr_object_labeling/CMakeLists.txt index b7c7b2a..aa62217 100644 --- a/hlpr_object_labeling/CMakeLists.txt +++ b/hlpr_object_labeling/CMakeLists.txt @@ -1,26 +1,12 @@ cmake_minimum_required(VERSION 2.8.3) project(hlpr_object_labeling) -find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_feature_extraction) +find_package(catkin REQUIRED COMPONENTS message_generation rospy std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) #catkin_python_setup() -add_message_files( - FILES - LabeledObjects.msg -) - -## Generate added messages and services with any dependencies -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs - sensor_msgs - hlpr_feature_extraction -) - catkin_package( - CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_feature_extraction + CATKIN_DEPENDS message_runtime std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs ) diff --git a/hlpr_object_labeling/msg/LabeledObjects.msg b/hlpr_object_labeling/msg/LabeledObjects.msg deleted file mode 100644 index 14e6ab5..0000000 --- a/hlpr_object_labeling/msg/LabeledObjects.msg +++ /dev/null @@ -1,3 +0,0 @@ -Header header -hlpr_feature_extraction/PcFeatures[] objects -std_msgs/String[] labels diff --git a/hlpr_object_labeling/package.xml b/hlpr_object_labeling/package.xml index 7bae93a..30f56bc 100644 --- a/hlpr_object_labeling/package.xml +++ b/hlpr_object_labeling/package.xml @@ -13,6 +13,6 @@ std_msgs geometry_msgs sensor_msgs - hlpr_feature_extraction + hlpr_perception_msgs diff --git a/hlpr_object_labeling/src/object_labeling.py b/hlpr_object_labeling/src/object_labeling.py index abe7781..5add1b7 100755 --- a/hlpr_object_labeling/src/object_labeling.py +++ b/hlpr_object_labeling/src/object_labeling.py @@ -11,8 +11,7 @@ import tf import itertools from Tkinter import * -from hlpr_feature_extraction.msg import PcFeatureArray -from hlpr_object_labeling.msg import LabeledObjects +from hlpr_perception_msgs.msg import ObjectFeatures, ExtractedFeaturesArray, LabeledObjects from std_msgs.msg import String pf = None @@ -44,15 +43,14 @@ def __init__(self): self.valW = get_param("hsv_val_weight",1) self.sizeW = get_param("size_weight",50000) fileref = get_param("feature_file_location") - if fileref is not None: - self.filename = os.path.expanduser(fileref) - else: - self.filename = None topicref = get_param("feature_file_rostopic") + self.filename = None if topicref is not None: self.rostopic = os.path.expanduser(topicref) self.fileSub = rospy.Subscriber(self.rostopic, String, self.cbFile, queue_size = 1) - self.subscriber = rospy.Subscriber("/beliefs/features", PcFeatureArray, self.cbClusters, queue_size = 1) + elif fileref is not None: + self.filename = os.path.expanduser(fileref) + self.subscriber = rospy.Subscriber("/beliefs/features", ExtractedFeaturesArray, self.cbClusters, queue_size = 1) self.pauseSub = rospy.Subscriber("/pause_labeling", String, self.cbPause, queue_size = 1) self.orderPub = rospy.Publisher("/beliefs/labels", LabeledObjects, queue_size = 1) @@ -136,9 +134,9 @@ def loadObjects(self): def hsvDiff(self, c1,c2): hsv1 = c1[1:4] - r2 = c2.rgba_color.r - g2 = c2.rgba_color.g - b2 = c2.rgba_color.b + r2 = c2.basicInfo.rgba_color.r + g2 = c2.basicInfo.rgba_color.g + b2 = c2.basicInfo.rgba_color.b hsv2 = cv2.cvtColor(np.array([[(r2,g2,b2)]],dtype='float32'), cv2.COLOR_RGB2HSV) h1 = hsv2[0][0][0] h2 = float(hsv1[0]) @@ -147,7 +145,7 @@ def hsvDiff(self, c1,c2): return abs(huediff), abs(hsv2[0][0][1]-float(hsv1[1])), abs(hsv2[0][0][2]-float(hsv1[2])) def sizeDiff(self, c1,c2): - size = c2.bb_dims.x * c2.bb_dims.y + size = c2.obb.bb_dims.x * c2.obb.bb_dims.y return abs(size - float(c1[4])) def calculateError(self, init, cluster): @@ -223,20 +221,22 @@ def drawClusters(self,clusters,ids): return self.canvas.delete("all") for idx in range(0,len(clusters)): - c = clusters[idx] - if c is None: + cluster = clusters[idx] + if cluster is None: continue + c = cluster.basicInfo pts = [(c.points_min.x,c.points_min.y),(c.points_min.x,c.points_max.y),(c.points_max.x,c.points_max.y),(c.points_max.x,c.points_min.y)] offset = complex(c.points_centroid.x,c.points_centroid.y) cangle = 0 # cmath.exp(c.angle*1j) rot = [] for x,y in pts: r = cangle * (complex(x,y)-offset) + offset - rot.append((-r.real + 0.5) * 500) - rot.append((-r.imag + 0.5) * 500) + rot.append((r.real + 0.5) * 500) + rot.append((r.imag + 0.5) * 500) rgb = '#%02x%02x%02x' % (c.rgba_color.r,c.rgba_color.g,c.rgba_color.b) poly = self.canvas.create_polygon(rot,outline=rgb,fill='white',width=5) - label = self.canvas.create_text((-c.points_centroid.x+0.5)*500, (-c.points_centroid.y + 0.5)*500,text=str(ids[idx].data),font="Verdana 10 bold") + label = self.canvas.create_text((c.points_centroid.x+0.5)*500, (c.points_centroid.y + 0.5)*500,text=str(ids[idx].data),font="Verdana 10 bold") + #label = self.canvas.create_text((-c.points_centroid.x+0.5)*500, (-c.points_centroid.y + 0.5)*500,text=str(ids[idx].data),font="Verdana 10 bold") self.canvas.pack() def main(args): diff --git a/hlpr_object_labeling/src/object_recording.py b/hlpr_object_labeling/src/object_recording.py index c0a9242..47cc5d0 100755 --- a/hlpr_object_labeling/src/object_recording.py +++ b/hlpr_object_labeling/src/object_recording.py @@ -9,7 +9,7 @@ import rospy import pdb from Tkinter import * -from hlpr_feature_extraction.msg import PcFeatureArray +from hlpr_perception_msgs.msg import ExtractedFeaturesArray pf = None display = None @@ -27,12 +27,13 @@ def get_param(name, value=None): class filter: def __init__(self): rospy.init_node('tracker', anonymous=True) - self.subscriber = rospy.Subscriber("/beliefs/features", PcFeatureArray, self.cbClusters, queue_size = 1) + self.subscriber = rospy.Subscriber("/beliefs/features", ExtractedFeaturesArray, self.cbClusters, queue_size = 1) self.initialized = False self.labeled = False self.labeledIdx = 0 self.initX = [] self.finished = False + self.exit = False self.filename = os.path.expanduser(get_param("feature_file_location", "tracked_object_data.txt")) self.minSize = get_param("min_object_size", 0.001) self.outf = open(self.filename, "w") @@ -44,11 +45,11 @@ def cbClusters(self, ros_data): return if self.initialized is False: self.initX = [] - for c in clusters: - size = c.bb_dims.x * c.bb_dims.y + for cluster in clusters: + size = cluster.obb.bb_dims.x * cluster.obb.bb_dims.y print 'Object size: ' + str(size) if size > self.minSize: - self.initX.append(c) + self.initX.append(cluster) if len(self.initX) is 0: return print str(len(self.initX)) + ' objects detected' @@ -56,21 +57,24 @@ def cbClusters(self, ros_data): elif self.labeled is False: var = raw_input("Enter label for object " + str(self.labeledIdx) + ": ") print "You entered: ", var - c = self.initX[self.labeledIdx] + cluster = self.initX[self.labeledIdx] + c = cluster.basicInfo + bb = cluster.obb r = c.rgba_color.r g = c.rgba_color.g b = c.rgba_color.b hsv = cv2.cvtColor(np.array([[(r,g,b)]],dtype='float32'), cv2.COLOR_RGB2HSV) - size = c.bb_dims.x * c.bb_dims.y + size = bb.bb_dims.x * bb.bb_dims.y self.outf.write(var + "," + str(hsv[0][0][0]) + "," + str(hsv[0][0][1]) + "," + str(hsv[0][0][2]) + "," + str(size) + "\n") self.labeledIdx = self.labeledIdx + 1 self.labeled = self.labeledIdx == len(self.initX) elif self.finished is False: self.outf.close() self.finished = True - elif self.finished is True: + elif self.finished is True and self.exit is False: print "Objects written to " + str(self.filename) + self.exit = True sys.exit() class ui: @@ -90,20 +94,21 @@ def drawClusters(self,clusters): return self.canvas.delete("all") for idx in range(0,len(clusters)): - c = clusters[idx] - if c is None: + cluster = clusters[idx] + if cluster is None: continue + c = cluster.basicInfo pts = [(c.points_min.x,c.points_min.y),(c.points_min.x,c.points_max.y),(c.points_max.x,c.points_max.y),(c.points_max.x,c.points_min.y)] offset = complex(c.points_centroid.x,c.points_centroid.y) cangle = 0 # cmath.exp(c.angle*1j) rot = [] for x,y in pts: r = cangle * (complex(x,y)-offset) + offset - rot.append((-r.real + 0.5) * 500) - rot.append((-r.imag + 0.5) * 500) + rot.append((r.real + 0.5) * 500) + rot.append((r.imag + 0.5) * 500) rgb = '#%02x%02x%02x' % (c.rgba_color.r,c.rgba_color.g,c.rgba_color.b) poly = self.canvas.create_polygon(rot,outline=rgb,fill='white',width=5) - label = self.canvas.create_text((-c.points_centroid.x+0.5)*500, (-c.points_centroid.y + 0.5)*500,text=str(idx),font="Verdana 10 bold") + label = self.canvas.create_text((c.points_centroid.x+0.5)*500, (c.points_centroid.y + 0.5)*500,text=str(idx),font="Verdana 10 bold") self.canvas.pack() def main(args): diff --git a/hlpr_perception_msgs/CMakeLists.txt b/hlpr_perception_msgs/CMakeLists.txt index c571951..edb4e2b 100644 --- a/hlpr_perception_msgs/CMakeLists.txt +++ b/hlpr_perception_msgs/CMakeLists.txt @@ -1,15 +1,15 @@ cmake_minimum_required(VERSION 2.8.3) project(hlpr_perception_msgs) find_package(catkin REQUIRED COMPONENTS + roscpp geometry_msgs sensor_msgs std_msgs tf2_ros message_generation + pcl_ros ) -find_package(PCL 1.8 REQUIRED COMPONENTS) - add_message_files( FILES # Segmentation msgs @@ -17,7 +17,7 @@ add_message_files( SegClusters.msg SegPlanes.msg ClusterIndex.msg - # Feature extraction msgs + # Feature extraction msgsnrg BasicFeatures.msg ShapeHist.msg ColorHist.msg @@ -27,6 +27,10 @@ add_message_files( ObjectFeatures.msg PlaneFeatures.msg ExtractedFeaturesArray.msg + # Knowledge feature msgs + LabeledObjects.msg + ObjectKnowledge.msg + ObjectKnowledgeArray.msg ) generate_messages( diff --git a/hlpr_perception_msgs/msg/LabeledObjects.msg b/hlpr_perception_msgs/msg/LabeledObjects.msg new file mode 100644 index 0000000..73e4c4d --- /dev/null +++ b/hlpr_perception_msgs/msg/LabeledObjects.msg @@ -0,0 +1,3 @@ +Header header +hlpr_perception_msgs/ObjectFeatures[] objects +std_msgs/String[] labels diff --git a/hlpr_knowledge_retrieval/msg/ObjectKnowledge.msg b/hlpr_perception_msgs/msg/ObjectKnowledge.msg similarity index 100% rename from hlpr_knowledge_retrieval/msg/ObjectKnowledge.msg rename to hlpr_perception_msgs/msg/ObjectKnowledge.msg diff --git a/hlpr_knowledge_retrieval/msg/ObjectKnowledgeArray.msg b/hlpr_perception_msgs/msg/ObjectKnowledgeArray.msg similarity index 100% rename from hlpr_knowledge_retrieval/msg/ObjectKnowledgeArray.msg rename to hlpr_perception_msgs/msg/ObjectKnowledgeArray.msg diff --git a/hlpr_segmentation/CMakeLists.txt b/hlpr_segmentation/CMakeLists.txt index 91f08c9..a380811 100644 --- a/hlpr_segmentation/CMakeLists.txt +++ b/hlpr_segmentation/CMakeLists.txt @@ -6,15 +6,17 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) -set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so - /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so - /opt/ros/${ROS_DISTRO}/lib/librosconsole.so - /opt/ros/${ROS_DISTRO}/lib/libroslib.so - /opt/ros/${ROS_DISTRO}/lib/librostime.so) #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs sensor_msgs) +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs + geometry_msgs + sensor_msgs + roscpp + hlpr_perception_msgs +) find_package(OpenCL QUIET) IF(OpenCL_FOUND) SET(LIBRARIES @@ -23,9 +25,10 @@ IF(OpenCL_FOUND) ) ENDIF(OpenCL_FOUND) -find_package(PCL 1.7 REQUIRED COMPONENTS) +#find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(PCL REQUIRED) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) add_message_files( FILES @@ -63,8 +66,8 @@ target_link_libraries(pcseg ${PCL_LIBRARIES} ${ROS_LIBRARIES}) add_executable(pc_seg src/segmentation.cpp) #add_executable(listener src/listener.cpp src/nodes/listener_node.cpp) -#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) -target_link_libraries(pc_seg ${PCL_LIBRARIES} pcseg ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +#target_link_libraries(pc_seg ${catkin_LIBRARIES} ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(pc_seg ${PCL_LIBRARIES} pcseg ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) #add_dependencies(pc_seg segmentation_gencfg segmentation_generate_messages_cpp) add_dependencies(pc_seg segmentation_generate_messages_cpp hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_segmentation/include/k2g.h b/hlpr_segmentation/include/k2g.h index 5fecec4..97665ea 100644 --- a/hlpr_segmentation/include/k2g.h +++ b/hlpr_segmentation/include/k2g.h @@ -248,8 +248,8 @@ class K2G { libfreenect2::SyncMultiFrameListener listener_; libfreenect2::FrameMap frames_; libfreenect2::Frame undistorted_, registered_, big_mat_; - Eigen::Matrix colmap; - Eigen::Matrix rowmap; + Eigen::Matrix colmap; + Eigen::Matrix rowmap; std::string serial_; int map_[512 * 424]; float qnan_; diff --git a/hlpr_segmentation/include/utils.hpp b/hlpr_segmentation/include/utils.hpp index ca6bae4..8aac49d 100644 --- a/hlpr_segmentation/include/utils.hpp +++ b/hlpr_segmentation/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_segmentation/msg/ClusterIndex.msg b/hlpr_segmentation/msg/ClusterIndex.msg deleted file mode 100644 index c07888d..0000000 --- a/hlpr_segmentation/msg/ClusterIndex.msg +++ /dev/null @@ -1 +0,0 @@ -std_msgs/Int32[] indices diff --git a/hlpr_segmentation/msg/SegmentedClusters.msg b/hlpr_segmentation/msg/SegmentedClusters.msg deleted file mode 100644 index c91e5f9..0000000 --- a/hlpr_segmentation/msg/SegmentedClusters.msg +++ /dev/null @@ -1,5 +0,0 @@ -Header header -sensor_msgs/PointCloud2[] clusters -sensor_msgs/PointCloud2[] normals -std_msgs/Float32MultiArray plane -ClusterIndex[] cluster_ids diff --git a/hlpr_segmentation/src/segmentation.cpp b/hlpr_segmentation/src/segmentation.cpp index b48e4c5..86795f0 100644 --- a/hlpr_segmentation/src/segmentation.cpp +++ b/hlpr_segmentation/src/segmentation.cpp @@ -170,8 +170,8 @@ main (int argc, char **argv) multi_plane_app.setViewer(viewer); } - // float workSpace[] = {-0.3,0.4,-0.25,0.35,0.3,2.0}; - float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; + //float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; + float workSpace[] = {-0.55,0.5,0.0,0.35,0.3,2.0}; multi_plane_app.setWorkingVolumeThresholds(workSpace); //if(pA.output_type == comType::cROS) diff --git a/hlpr_segmentation/src/segmentation.cpp~ b/hlpr_segmentation/src/segmentation.cpp~ deleted file mode 100644 index a5bf711..0000000 --- a/hlpr_segmentation/src/segmentation.cpp~ +++ /dev/null @@ -1,330 +0,0 @@ -/* - * ircp_loop.cpp - * - * Created on: Sep 4, 2014 - * Author: baris - */ - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -//#include - -#include - -#include - -pcl::PointCloud::ConstPtr prev_cloud; -pcl::PointCloud cloud_store; -boost::mutex cloud_mutex; -boost::mutex imageName_mutex; -bool writePCD2File = false; -char imageName[100]; -bool gotFirst = false; -bool interrupt = false; -const char *clusterOutRostopic = "/beliefs/clusters"; -const char *normalOutRostopic = "/beliefs/normals"; -const char *planeOutRostopic = "/beliefs/plane"; -const char *segOutRostopic = "/beliefs/clusters"; -bool viz_ = false; - -void -cloud_cb_ros_ (const sensor_msgs::PointCloud2ConstPtr& msg) -{ - pcl::PCLPointCloud2 pcl_pc; - - if (msg->height == 1){ - sensor_msgs::PointCloud2 new_cloud_msg; - new_cloud_msg.header = msg->header; - new_cloud_msg.height = 480; - new_cloud_msg.width = 640; - new_cloud_msg.fields = msg->fields; - new_cloud_msg.is_bigendian = msg->is_bigendian; - new_cloud_msg.point_step = msg->point_step; - new_cloud_msg.row_step = 20480; - new_cloud_msg.data = msg->data; - new_cloud_msg.is_dense = msg->is_dense; - - pcl_conversions::toPCL(new_cloud_msg, pcl_pc); - } - else - pcl_conversions::toPCL(*msg, pcl_pc); - - pcl::PointCloud cloud; - pcl::fromPCLPointCloud2(pcl_pc, cloud); - pcl::copyPointCloud(cloud, cloud_store); - - cloud_mutex.lock (); - prev_cloud = cloud.makeShared(); - if(writePCD2File) - { - pcl::PointCloud::Ptr saved_cloud(new pcl::PointCloud(*prev_cloud)); - std::cout << imageName << std::endl; - cloud_mutex.unlock (); - imageName_mutex.lock(); - pcl::io::savePCDFile(imageName, *saved_cloud); - imageName_mutex.unlock(); - writePCD2File = false; - } - else - cloud_mutex.unlock (); - - gotFirst = true; -} - -void interruptFn(int sig) { - interrupt = true; -} - -void -cloud_cb_direct_ (const pcl::PointCloud::ConstPtr& cloud) -{ - cloud_mutex.lock (); - prev_cloud = cloud; - if(writePCD2File) - { - pcl::PointCloud::Ptr saved_cloud(new pcl::PointCloud(*prev_cloud)); - std::cout << imageName << std::endl; - cloud_mutex.unlock (); - imageName_mutex.lock(); - pcl::io::savePCDFile(imageName, *saved_cloud); - imageName_mutex.unlock(); - writePCD2File = false; - } - else - cloud_mutex.unlock (); - - gotFirst = true; -} - -inline void -fake_cloud_cb_kinectv2_ (const pcl::PointCloud::ConstPtr& cloud) -{ - prev_cloud = cloud; - if(writePCD2File) - { - pcl::PointCloud::Ptr saved_cloud(new pcl::PointCloud(*prev_cloud)); - std::cout << imageName << std::endl; - cloud_mutex.unlock (); - imageName_mutex.lock(); - pcl::io::savePCDFile(imageName, *saved_cloud); - imageName_mutex.unlock(); - writePCD2File = false; - } -} - -int -main (int argc, char **argv) -{ - std::cout << "main function" << std::endl; - pcl::Grabber* interface; - ros::NodeHandle *nh; - ros::Subscriber sub; - ros::Publisher clusterPub, normalPub, planePub,msgPub; - - bool spawnObject = true; - - K2G *k2g; - processor freenectprocessor = OPENGL; - - boost::shared_ptr> cloud; - - std::cout << "ros node initialized" << std::endl; - ros::init(argc, argv, "hlpr_segmentation",ros::init_options::NoSigintHandler); - nh = new ros::NodeHandle("~"); - // nh->getParam("segmentation/viz", viz_); - // std::cout<<"viz is set to " << viz_ << endl; - - parsedArguments pA; - if(parseArguments(argc, argv, pA, *nh) < 0) - return 0; - - viz_ = pA.viz; - ridiculous_global_variables::ignore_low_sat = pA.saturation_hack; - ridiculous_global_variables::saturation_threshold = pA.saturation_hack_value; - ridiculous_global_variables::saturation_mapped_value = pA.saturation_mapped_value; - - OpenNIOrganizedMultiPlaneSegmentation multi_plane_app; - - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - pcl::PointCloud::Ptr ncloud_ptr (new pcl::PointCloud); - pcl::PointCloud::Ptr label_ptr (new pcl::PointCloud); - - boost::shared_ptr viewer; - multi_plane_app.initSegmentation(pA.seg_color_ind, pA.ecc_dist_thresh, pA.ecc_color_thresh); - if(viz_) { - viewer = cloudViewer(cloud_ptr); - multi_plane_app.setViewer(viewer); - } - - // float workSpace[] = {-0.3,0.4,-0.25,0.35,0.3,2.0}; - float workSpace[] = {-0.55,0.5,-0.2,0.45,0.3,2.0}; - multi_plane_app.setWorkingVolumeThresholds(workSpace); - - //if(pA.output_type == comType::cROS) - //{ - //pub = nh->advertise(outRostopic,5); - // clusterPub = nh->advertise>(clusterOutRostopic,5); - // normalPub = nh->advertise>(normalOutRostopic,5); - // planePub = nh->advertise(planeOutRostopic,5); - msgPub = nh->advertise(segOutRostopic,5); - // } - - switch (pA.pc_source) - { - case 0: - { - std::cout << "Using ros topic as input" << std::endl; - sub = nh->subscribe(pA.ros_topic, 1, cloud_cb_ros_); - break; - } - case 1: - { - std::cout << "Using the openni device" << std::endl; - - interface = new pcl::OpenNIGrabber (); - - boost::function::ConstPtr&)> f = boost::bind (&cloud_cb_direct_,_1); - boost::signals2::connection c = interface->registerCallback (f); - - interface->start (); - break; - } - case 2: - default: - { - std::cout << "Using kinect v2" << std::endl; - - freenectprocessor = static_cast(pA.freenectProcessor); - - k2g = new K2G(freenectprocessor, true); - cloud = k2g->getCloud(); - prev_cloud = cloud; - gotFirst = true; - break; - } - } - - signal(SIGINT, interruptFn); - while (!interrupt && (!viz_ || !viewer->wasStopped ())) - { - if(pA.ros_node) - { - ros::spinOnce(); - } - if(viz_) - viewer->spinOnce(20); - if(!gotFirst) - continue; - int selected_cluster_index = -1; - if(cloud_mutex.try_lock ()) - { - if(pA.pc_source == 2) - { - cloud = k2g->getCloud(); - fake_cloud_cb_kinectv2_(cloud); - } - - if(viz_ && pA.justViewPointCloud) - { - pcl::PointCloud::Ptr filtered_prev_cloud(new pcl::PointCloud(*prev_cloud)); - multi_plane_app.preProcPointCloud(filtered_prev_cloud); - if (!viewer->updatePointCloud (filtered_prev_cloud, "cloud")) - { - viewer->addPointCloud (filtered_prev_cloud, "cloud"); - } - selected_cluster_index = -1; - } - else - { - pcl::PointCloud::CloudVectorType clusters; - std::vector > clusterNormals; - Eigen::Vector4f plane; - std::vector clusterIndices; - std::vector> clusterIndicesStore; - - selected_cluster_index = multi_plane_app.processOnce(prev_cloud,clusters,clusterNormals,plane,clusterIndicesStore, - pA.pre_proc, - pA.merge_clusters, viz_, pA.filterNoise); //true is for the viewer - hlpr_segmentation::SegmentedClusters msg; - msg.header.stamp = ros::Time::now(); - - // Pull out the cluster indices and put in msg - for (int ti = 0; ti < clusterIndicesStore.size(); ti++){ - hlpr_segmentation::ClusterIndex cluster_idx_msg; - for (int j = 0; j < clusterIndicesStore[ti].size(); j++){ - std_msgs::Int32 temp_msg; - temp_msg.data = clusterIndicesStore[ti][j]; - cluster_idx_msg.indices.push_back(temp_msg); - } - msg.cluster_ids.push_back(cluster_idx_msg); - //clusterIndices.insert(clusterIndices.end(),clusterIndicesStore[ti].begin(), clusterIndicesStore[ti].end()); // For removing ALL cluster points - } - - for(int i = 0; i < clusters.size(); i++) { - sensor_msgs::PointCloud2 out; - pcl::PCLPointCloud2 tmp; - pcl::toPCLPointCloud2(clusters[i], tmp); - pcl_conversions::fromPCL(tmp, out); - msg.clusters.push_back(out); - } - - for(int i = 0; i < clusterNormals.size(); i++) { - sensor_msgs::PointCloud2 out; - pcl::PCLPointCloud2 tmp; - pcl::toPCLPointCloud2(clusterNormals[i], tmp); - pcl_conversions::fromPCL(tmp, out); - msg.normals.push_back(out); - } - - std_msgs::Float32MultiArray planeMsg; - planeMsg.data.clear(); - for(int i = 0; i < 4; i++) - planeMsg.data.push_back(plane[i]); - //planePub.publish(planeMsg); - msg.plane = planeMsg; - - msgPub.publish(msg); - - /*clusterPub.publish(clusters[0]); - normalPub.publish(clusterNormals[0]); - std_msgs::Float32MultiArray planeMsg; - planeMsg.data.clear(); - for(int i = 0; i < 4; i++) - planeMsg.data.push_back(plane[i]); - planePub.publish(planeMsg);*/ - } - - cloud_mutex.unlock(); - - /*the z_thresh may result in wrong cluster to be selected. it might be a good idea to do - * some sort of mean shift tracking, or selecting the biggest amongst the candidates (vs choosing the most similar color) - * or sending all the plausible ones to c6 and decide there - */ - } - - if(selected_cluster_index < 0) - continue; - } - if(pA.pc_source == 1) - { - interface->stop (); - delete interface; - } - delete nh; - - ros::shutdown(); - return 0; -} diff --git a/hlpr_single_plane_segmentation/CMakeLists.txt b/hlpr_single_plane_segmentation/CMakeLists.txt index 59ea9f3..43fdfcc 100644 --- a/hlpr_single_plane_segmentation/CMakeLists.txt +++ b/hlpr_single_plane_segmentation/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native") set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--as-needed -pthread") set(OPENNI_INCLUDE_DIRS /usr/include/ni) -set(ROS_DISTRO indigo) +set(ROS_DISTRO kinetic) set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so /opt/ros/${ROS_DISTRO}/lib/libroscpp_serialization.so /opt/ros/${ROS_DISTRO}/lib/librosconsole.so @@ -14,10 +14,17 @@ set(ROS_LIBRARIES /opt/ros/${ROS_DISTRO}/lib/libroscpp.so /opt/ros/${ROS_DISTRO}/lib/librostime.so) #find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure message_generation roscpp std_msgs geometry_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation roscpp std_msgs geometry_msgs sensor_msgs hlpr_perception_msgs) -find_package(PCL 1.8 REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS + message_generation + roscpp + std_msgs + geometry_msgs + sensor_msgs + hlpr_perception_msgs + pcl_ros +) find_package(freenect2 REQUIRED) -find_package(OpenCV 2 REQUIRED) +find_package(OpenCV 3 REQUIRED) #add_message_files( # FILES @@ -50,10 +57,10 @@ include_directories(include ${catkin_INCLUDE_DIRS}) include_directories(${PROJECT_SOURCE_DIR}/include) add_library(spseg SHARED src/pc_segmentation.cpp src/utils.cpp) -target_link_libraries(spseg ${PCL_LIBRARIES} ${ROS_LIBRARIES}) +target_link_libraries(spseg ${PCL_LIBRARIES} ${catkin_LIBRARIES}) add_executable(sp_seg src/segmentation.cpp) -target_link_libraries(sp_seg ${PCL_LIBRARIES} spseg ${freenect2_LIBRARY} ${ROS_LIBRARIES} ${OpenCV_LIBS}) +target_link_libraries(sp_seg ${PCL_LIBRARIES} spseg ${freenect2_LIBRARY} ${catkin_LIBRARIES} ${OpenCV_LIBS}) add_dependencies(sp_seg hlpr_perception_msgs_generate_messages_cpp) diff --git a/hlpr_single_plane_segmentation/include/k2g.h b/hlpr_single_plane_segmentation/include/k2g.h index 5fecec4..97665ea 100644 --- a/hlpr_single_plane_segmentation/include/k2g.h +++ b/hlpr_single_plane_segmentation/include/k2g.h @@ -248,8 +248,8 @@ class K2G { libfreenect2::SyncMultiFrameListener listener_; libfreenect2::FrameMap frames_; libfreenect2::Frame undistorted_, registered_, big_mat_; - Eigen::Matrix colmap; - Eigen::Matrix rowmap; + Eigen::Matrix colmap; + Eigen::Matrix rowmap; std::string serial_; int map_[512 * 424]; float qnan_; diff --git a/hlpr_single_plane_segmentation/include/utils.hpp b/hlpr_single_plane_segmentation/include/utils.hpp index ca6bae4..8aac49d 100644 --- a/hlpr_single_plane_segmentation/include/utils.hpp +++ b/hlpr_single_plane_segmentation/include/utils.hpp @@ -349,7 +349,7 @@ array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n"); template void -vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = "\n") +vector2file(const std::vector &vec, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -361,7 +361,7 @@ vector2file(const std::vector &vec, char *fileName = "tmp.txt", char *delim = template void -file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') +file2vector(std::vector &vec, char *fileName, char delim) { std::ifstream myfile; myfile.open (fileName); @@ -385,7 +385,7 @@ file2vector(std::vector &vec, char *fileName = "tmp.txt", char delim = ',') template void -doubleVector2file(const std::vector > &vec, char *fileName = "tmp.txt", char *delimiter = ",", char *delimiter2 = "\n") +doubleVector2file(const std::vector > &vec, char *fileName, char *delimiter, char *delimiter2) { std::ofstream myfile; myfile.open (fileName); @@ -402,7 +402,7 @@ doubleVector2file(const std::vector > &vec, char *fileName = "tmp template void -file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", char delimiter = ',', char delimiter2 = '\n') +file2doubleVector(std::vector > &vec, char *fileName, char delimiter, char delimiter2) { std::ifstream myfile; myfile.open (fileName); @@ -446,7 +446,7 @@ file2doubleVector(std::vector > &vec, char *fileName = "tmp.txt", template void -array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n") +array2file(const T *arr, int size, char *fileName, char *delim) { std::ofstream myfile; myfile.open (fileName); @@ -457,7 +457,7 @@ array2file(const T *arr, int size, char *fileName = "tmp.txt", char *delim = "\n template void -array2file(const T *arr, int size, std::ofstream &myfile, char *delim = "\n") +array2file(const T *arr, int size, std::ofstream &myfile, char *delim) { for(int i = 0; i < size; i++) myfile << arr[i] << delim; diff --git a/hlpr_visual_features/CMakeLists.txt b/hlpr_visual_features/CMakeLists.txt new file mode 100644 index 0000000..f152309 --- /dev/null +++ b/hlpr_visual_features/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hlpr_visual_features) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES hlpr_visual_features +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/hlpr_visual_features.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/hlpr_visual_features_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_hlpr_visual_features.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/hlpr_visual_features/package.xml b/hlpr_visual_features/package.xml new file mode 100644 index 0000000..0a152fc --- /dev/null +++ b/hlpr_visual_features/package.xml @@ -0,0 +1,68 @@ + + + hlpr_visual_features + 0.0.0 + The hlpr_visual_features package + + + + + eshort + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/hlpr_visual_features/setup.py b/hlpr_visual_features/setup.py new file mode 100644 index 0000000..f3f746d --- /dev/null +++ b/hlpr_visual_features/setup.py @@ -0,0 +1,12 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['hlpr_visual_features'], + package_dir={'': 'src'}, +) + +setup(**setup_args) diff --git a/hlpr_visual_features/src/hlpr_visual_features/__init__.py b/hlpr_visual_features/src/hlpr_visual_features/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py b/hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py new file mode 100755 index 0000000..aa83e22 --- /dev/null +++ b/hlpr_visual_features/src/hlpr_visual_features/open_cv_features.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python + +# Copyright (c) 2017, Elaine Short, SIM Lab +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# * Neither the name of the SIM Lab nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + +import rospy +import cv2 +import numpy +import math +import argparse +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image +from contingency.msg import ArrayFeature + + +class CVImageSubscriber: + def __init__(self, display_on): + self._sub=rospy.Subscriber("/camera/rgb/image_rect_color", Image, self._cb) + self._bridge = CvBridge() + self._fgbg = cv2.createBackgroundSubtractorMOG2(history=20) + self._motion_pub = rospy.Publisher("/contingency/opencv/motion",ArrayFeature, queue_size=1) + + self._flow_ang_pub = rospy.Publisher("/contingency/opencv/flow/angle",ArrayFeature, queue_size=1) + self._mag_pub = rospy.Publisher("/contingency/opencv/flow/magnitude",ArrayFeature, queue_size=1) + self._acc_pub = rospy.Publisher("/contingency/opencv/flow/acceleration",ArrayFeature, queue_size=1) + self._mag_total_pub = rospy.Publisher("/contingency/opencv/flow/total/magnitude",ArrayFeature, queue_size=1) + self._ang_total_pub = rospy.Publisher("/contingency/opencv/flow/total/angle",ArrayFeature, queue_size=1) + + + self._splits = 4 + self._disp = display_on + self._prev = [] + self._prev_mag = [0 for i in range(self._splits)] + + def _cb(self,data): + try: + cv_image=self._bridge.imgmsg_to_cv2(data, "passthrough") + cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) + except CvBridgeError as e: + rospy.logerr(e) + return + + fgmask=self._fgbg.apply(cv_image) + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE,(3,3)) + fgmask = cv2.morphologyEx(fgmask,cv2.MORPH_OPEN, kernel) + + flow = [] + if len(self._prev)>0: + hsv = numpy.zeros_like(cv2.cvtColor(cv2.cvtColor(cv_image,cv2.COLOR_GRAY2BGR),cv2.COLOR_BGR2HSV)) + hsv[...,1] = 255 + flow = cv2.calcOpticalFlowFarneback(self._prev,cv_image, None, + 0.5, 3, 15, 3, 5, 1.2, 0) + mag, ang = cv2.cartToPolar(flow[...,0], flow[...,1]) + total = [numpy.sum(flow[...,0]), + numpy.sum(flow[...,1])] + + total_mag = math.sqrt(total[0]*total[0]+total[1]*total[1]) + total_ang = math.atan2(total[1],total[0]) + + mag[numpy.isinf(mag)]=0 + + if self._disp: + cv2.imshow("frame",fgmask)#cv2.bitwise_and(cv_image,cv_image,mask=fgmask)) + cv2.imshow("frame0",cv_image) + if len(flow)>0: + hsv[...,0] = ang*180/numpy.pi/2 + hsv[...,2] = cv2.normalize(mag,None,0,255,cv2.NORM_MINMAX) + bgr = cv2.cvtColor(hsv,cv2.COLOR_HSV2BGR) + cv2.imshow('frame2',bgr) + cv2.waitKey(3) + + w = int(fgmask.shape[1]/self._splits) + + data_movement = [] + data_flow_mag = [] + data_flow_ang = [] + + for i in range(self._splits): + start = i*w + end = min(i*w, fgmask.shape[1]) + data_movement.append(numpy.average(fgmask[:,w*i:w*(i+1)])) + if len(flow)>0: + data_flow_mag.append(numpy.average(mag[:,w*i:w*(i+1)])) + try: + data_flow_ang.append(numpy.average(ang[:,w*i:w*(i+1)],weights = mag[:,w*i:w*(i+1)]*mag[:,w*i:w*(i+1)]>10)) + except ZeroDivisionError: + data_flow_ang.append(0) + self._prev = cv_image + + + self._motion_pub.publish(ArrayFeature(data=data_movement)) + + if len(flow)>0: + self._flow_ang_pub.publish(ArrayFeature(data=data_flow_ang)) + self._mag_pub.publish(ArrayFeature(data=data_flow_mag)) + self._acc_pub.publish(ArrayFeature(data=[data_flow_mag[i]-self._prev_mag[i] for i in range(len(data_flow_mag))])) + self._mag_total_pub.publish(ArrayFeature(data=[total_mag])) + self._ang_total_pub.publish(ArrayFeature(data=[total_ang])) + self._prev_mag = data_flow_mag + +if __name__=="__main__": + rospy.init_node("background_subtraction_features") + parser=argparse.ArgumentParser(description="Use opencv to get motion features from video") + parser.add_argument('-d', '--display-video', help="Show the masked video on screen", action='store_true') + args = parser.parse_known_args()[0] + + c = CVImageSubscriber(args.display_video) + rospy.spin()