implementation of CloudSource

CMakeLists.txt

@@ -1,13 +1,17 @@
 cmake_minimum_required(VERSION 2.8)
 project(object_recognition_ros)
+find_package(OpenCV REQUIRED)
+find_package(PCL REQUIRED)
 
+# missing ecto_pcl?
 find_package(catkin REQUIRED cmake_modules ecto ecto_ros geometric_shapes geometry_msgs object_recognition_core
-                             object_recognition_msgs pluginlib rosbag roscpp sensor_msgs
+                             object_recognition_msgs pluginlib rosbag roscpp sensor_msgs ecto_image_pipeline  ecto_pcl
+                       opencv_candidate
                              visualization_msgs
 )
 catkin_package(INCLUDE_DIRS include
   LIBRARIES object_information_cache
-  CATKIN_DEPENDS object_recognition_core object_recognition_msgs
+  CATKIN_DEPENDS object_recognition_core object_recognition_msgs ecto_image_pipeline 
 )
 
 #install targets for all things python

package.xml

@@ -14,6 +14,7 @@
   <build_depend>cmake_modules</build_depend>
   <build_depend>ecto</build_depend>
   <build_depend>ecto_ros</build_depend>
+  <build_depend>ecto_pcl</build_depend>
   <build_depend>ecto_image_pipeline</build_depend>
   <build_depend>geometric_shapes</build_depend>
   <build_depend>object_recognition_core</build_depend>
@@ -24,6 +25,7 @@
   <run_depend>boost</run_depend>
   <run_depend>ecto</run_depend>
   <run_depend>ecto_ros</run_depend>
+  <run_depend>ecto_pcl</run_depend>
   <run_depend>ecto_image_pipeline</run_depend>
   <run_depend>geometric_shapes</run_depend>
   <run_depend>object_recognition_core</run_depend>

python/object_recognition_ros/io/source/__init__.py

@@ -1,2 +1,3 @@
 from .ros_kinect import RosKinect
 from .bag_reader import BagReader
+from .cloud_source import CloudSource

python/object_recognition_ros/io/source/cloud_source.py

@@ -0,0 +1,102 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Willow Garage, Inc.
+# 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 Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derivedo
+# 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 OWNER 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.
+"""
+Module defining several outputs for the object recognition pipeline
+"""
+
+from ecto_image_pipeline.io.source import create_source
+from object_recognition_core.io.source import SourceBase
+from object_recognition_ros import init_ros
+import ecto
+import ecto_pcl_ros
+import ecto_pcl
+import object_recognition_ros
+from object_recognition_ros.ecto_cells.io_ros  import EctoPointCloudOrganizedToMatXYZ
+from ecto_pcl_ros import Message2PointCloud
+
+from ecto_image_pipeline.io.source.camera_base import CameraType
+from ecto import BlackBoxCellInfo as CellInfo, BlackBoxForward as Forward
+import ecto_ros
+import ecto_ros.ecto_sensor_msgs as ecto_sensor_msgs
+
+PointSub = ecto_sensor_msgs.Subscriber_PointCloud2
+CameraInfoSub = ecto_sensor_msgs.Subscriber_CameraInfo
+
+# similar to SourceBase and Similar to OpenNISubscriber
+#
+# OpenNISubscriber in ecto_image_pipeline from BaseSource: connects to topics and forwards them
+# BaseSource (base of OpenNISubscriber) makes the 3d using DepthTo3D but we do not this: DepthTo3D is sent to crop_box emitted as points3d (CropBox in ecto_opencv)
+#
+# We need the converter from PointCloud2 to points and then simply subscribe
+#
+# Conversions in reconstruction: MatToPointCloud XYZ/XYZRGB Organized
+# Conversions in ecto_pcl: Message2PointCloud and PointCloud2Message
+class CloudSource(ecto.BlackBox):
+    CAMERA_TYPE = CameraType.RGBD
+    def __init__(self, *args, **kwargs):
+        init_ros()
+        ecto.BlackBox.__init__(self, *args, **kwargs)
+
+    @staticmethod
+    def declare_cells(_p):
+        qsize = 1
+        cells = ecto.BlackBox.declare_cells(_p)
+        subs = dict(Ccamera_points=PointSub(topic_name='/bogus_topic_image', queue_size=qsize),
+                    Ccamera_info=CameraInfoSub(topic_name='/bogus_topic_image', queue_size=qsize),
+                 )
+        cells.update(subs)
+        cells['Csource'] = ecto_ros.Synchronizer('Synchronizator', subs=subs)
+        cells['Ccamera_info_image'] = CellInfo(ecto_ros.CameraInfo2Cv) 
+        cells['Cmsg2points'] = Message2PointCloud("msg2cloud",format=ecto_pcl.XYZ)
+        cells['Cpoints2mat'] = EctoPointCloudOrganizedToMatXYZ("points2mat")
+        return cells
+
+    @staticmethod
+    def declare_forwards(_p):
+        p = {}
+        p['Ccamera_info'] = [Forward('topic_name', 'camera_info', 'The ROS topic for the RGB camera info.','/kinect2/sd/camera_info')]
+        p['Ccamera_points']  = [Forward('topic_name', 'camera_points', 'The ROS topic for the RGB camera info.','/kinect2/sd/points')]
+        i = {}
+        o = {'Ccamera_info_image': [Forward('K', 'K_image', 'The camera intrinsics matrix of the image camera.')],
+            "Cpoints2mat" : [Forward('points',"points3d", "The output points for OpenCV")] }
+        return (p,i,o)
+    def configure(self, _p, _i, _o):
+        pass
+    def connections(self, p):
+        #ros message converters
+        graph = [
+          self.Csource["Ccamera_info"] >> self.Ccamera_info_image['camera_info'],
+          self.Csource["Ccamera_points"] >> self.Cmsg2points["input"],
+          self.Cmsg2points["output"] >> self.Cpoints2mat["point_cloud"]
+        ]
+
+        return graph

src/io/CMakeLists.txt

@@ -4,16 +4,25 @@ find_package(Eigen REQUIRED)
 include_directories(SYSTEM
                     ${EIGEN_INCLUDE_DIRS}
                     ${ecto_ros_INCLUDE_DIRS}
+                    ${ecto_pcl_INCLUDE_DIRS}
                     ${catkin_INCLUDE_DIRS}
                     ${geometry_msgs_INCLUDE_DIRS}
                     ${object_recognition_core_INCLUDE_DIRS}
+                   ${PCL_INCLUDE_DIRS}
 )
 
 ectomodule(io_ros DESTINATION ${PROJECT_NAME}/ecto_cells
                   INSTALL
            module.cpp
            MsgAssembler.cpp
            visualization_msgs_MarkerArray.cpp
+		   MatToPointCloud.cpp
+ 		   PointCloudToMat.cpp
 )
 
-link_ecto(io_ros ${catkin_LIBRARIES})
+link_ecto(io_ros ${catkin_LIBRARIES}
+                                            ${OpenCV_LIBRARIES}
+                                            ${PCL_LIBRARIES}
+)
+
+

src/io/MatToPointCloud.cpp

@@ -0,0 +1,185 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 Willow Garage, Inc. 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 OWNER 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.
+ *
+ */
+
+#include <fstream>
+#include <iostream>
+
+#include <boost/foreach.hpp>
+#include <boost/shared_ptr.hpp>
+
+#include <ecto/ecto.hpp>
+
+#include <opencv2/core/core.hpp>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include "conversions.hpp"
+
+using ecto::tendrils;
+
+namespace object_recognition_core
+{
+  namespace conversion
+  {
+    /** Ecto implementation of a module that takes a point cloud as
+        an input and stacks it in a matrix of floats:
+        - if the point cloud is organized, the return
+          a matrix is width by height with 3 channels (for x, y and z)
+        - if the point cloud is unorganized, the
+          return a matrix is n_point by 1 with 3 channels (for x, y and z)
+     */
+    struct MatToPointCloudXYZ
+    {
+      // Get the original keypoints and point cloud
+      typedef pcl::PointXYZ PointType;
+      typedef pcl::PointCloud<PointType> CloudType;
+      typedef CloudType::ConstPtr CloudOutT;
+
+
+      static void
+      declare_io(const tendrils& params, tendrils& inputs, tendrils& outputs)
+      {
+        inputs.declare(&MatToPointCloudXYZ::points3d, "points",
+                       "The width by height by 3 channels (x, y and z)");
+        outputs.declare(&MatToPointCloudXYZ::cloud_out, "point_cloud",
+                        "The XYZ point cloud");
+      }
+
+      int
+      process(const tendrils& inputs, const tendrils& outputs)
+      {
+        CloudType::Ptr point_cloud(new CloudType);
+        image_pipeline::cvToCloud(*points3d, *point_cloud);
+        *cloud_out = point_cloud;
+        return ecto::OK;
+      }
+      ecto::spore<cv::Mat> points3d;
+      ecto::spore<CloudOutT> cloud_out;
+    };
+    struct MatToPointCloudXYZOrganized
+    {
+      // Get the original keypoints and point cloud
+      typedef pcl::PointXYZ PointType;
+      typedef pcl::PointCloud<PointType> CloudType;
+      typedef CloudType::ConstPtr CloudOutT;
+
+
+      static void
+      declare_io(const tendrils& params, tendrils& inputs, tendrils& outputs)
+      {
+        inputs.declare(&MatToPointCloudXYZOrganized::points3d, "points", "The width by height by 3 channels (x, y and z)");
+        outputs.declare(&MatToPointCloudXYZOrganized::cloud_out, "point_cloud", "The XYZ point cloud");
+      }
+
+      int
+      process(const tendrils& inputs, const tendrils& outputs)
+      {
+        CloudType::Ptr point_cloud(new CloudType);
+       image_pipeline:: cvToCloudOrganized(*points3d, *point_cloud);
+        *cloud_out = point_cloud;
+        return ecto::OK;
+      }
+      ecto::spore<cv::Mat> points3d;
+      ecto::spore<CloudOutT> cloud_out;
+    };
+    struct MatToPointCloudXYZRGB
+    {
+      typedef pcl::PointXYZRGB PointType;
+      // Get the original keypoints and point cloud
+      typedef pcl::PointCloud<PointType> CloudType;
+      typedef CloudType::ConstPtr CloudOutT;
+
+
+      static void
+      declare_io(const tendrils& params, tendrils& inputs, tendrils& outputs)
+      {
+        inputs.declare(&MatToPointCloudXYZRGB::image, "image", "The rgb image.").required(true);
+        inputs.declare(&MatToPointCloudXYZRGB::points3d, "points", "The 3d points.").required(true);
+        inputs.declare(&MatToPointCloudXYZRGB::mask, "mask", "The binary mask for valid points.");
+        outputs.declare(&MatToPointCloudXYZRGB::cloud_out, "point_cloud", "The XYZRGB point cloud");
+      }
+
+      int
+      process(const tendrils& i, const tendrils& o)
+      {
+        //extract the cloud
+        CloudType::Ptr cloud(new CloudType);
+       image_pipeline:: cvToCloudXYZRGB(*points3d, *cloud, *image, *mask, false);
+        *cloud_out = cloud;
+        return ecto::OK;
+      }
+      ecto::spore<cv::Mat> mask, image, points3d;
+      ecto::spore<CloudOutT> cloud_out;
+
+    };
+    struct MatToPointCloudXYZRGBOrganized
+        {
+          // Get the original keypoints and point cloud
+          typedef pcl::PointXYZRGB PointType;
+          typedef pcl::PointCloud<PointType> CloudType;
+          typedef CloudType::ConstPtr CloudOutT;
+
+
+          static void
+          declare_io(const tendrils& params, tendrils& inputs, tendrils& outputs)
+          {
+            inputs.declare(&MatToPointCloudXYZRGBOrganized::points3d, "points", "The width by height by 3 channels (x, y and z)").required(true);
+            inputs.declare(&MatToPointCloudXYZRGBOrganized::image, "image", "The rgb image.").required(true);
+            outputs.declare(&MatToPointCloudXYZRGBOrganized::cloud_out, "point_cloud", "The XYZRGB organized point cloud");
+          }
+
+          int
+          process(const tendrils& inputs, const tendrils& outputs)
+          {
+            CloudType::Ptr point_cloud(new CloudType);
+
+           image_pipeline::cvToCloudRGBOrganized(*points3d, *image, *point_cloud);
+            *cloud_out = point_cloud;
+            return ecto::OK;
+          }
+          ecto::spore<cv::Mat> points3d, image;
+          ecto::spore<CloudOutT> cloud_out;
+        };
+  }
+}
+
+ECTO_CELL(io_ros, object_recognition_core::conversion::MatToPointCloudXYZ, "MatToPointCloudXYZ",
+          "Given a cv::Mat, convert it to pcl::PointCloud<pcl::PointXYZ>.");
+ECTO_CELL(io_ros, object_recognition_core::conversion::MatToPointCloudXYZOrganized, "MatToPointCloudXYZOrganized",
+          "Given a cv::Mat, convert it to an organized pcl::PointCloud<pcl::PointXYZ>.");
+ECTO_CELL(io_ros, object_recognition_core::conversion::MatToPointCloudXYZRGB, "MatToPointCloudXYZRGB",
+          "Given a cv::Mat, convert it to pcl::PointCloud<pcl::PointXYZRGB>.");
+ECTO_CELL(io_ros, object_recognition_core::conversion::MatToPointCloudXYZRGBOrganized, "MatToPointCloudXYZRGBOrganized",
+          "Given a cv::Mat, convert it to an organized pcl::PointCloud<pcl::PointXYZRGB>.");