Permalink
Browse files

adding segmenter

  • Loading branch information...
1 parent 2e65724 commit 5770e27e6b4dc1eb685252fe825b744b60a7877f Mike Krainin committed Aug 31, 2011
Showing with 127 additions and 0 deletions.
  1. +3 −0 CMakeLists.txt
  2. +47 −0 scripts/segmenter_test.py
  3. +1 −0 src/CMakeLists.txt
  4. +76 −0 src/Segmenter.cpp
View
@@ -23,4 +23,7 @@ include_directories(${geometry_msgs_INCLUDES})
include_directories(${pose_corrector_INCLUDES})
include_directories(${tabletop_object_detector_INCLUDES})
+#TODO: need some way to lookup where ecto_pcl is located
+include_directories(/wg/stor2a/mkrainin/workspace/ecto_kitchen/pcl/include)
+
add_subdirectory(src)
View
@@ -0,0 +1,47 @@
+#!/usr/bin/env python
+
+import ecto, ecto_ros, ecto_pcl, ecto_pcl_ros, ecto_sensor_msgs, ecto_opencv.highgui
+import ecto_corrector
+
+import sys
+
+debug = False
+
+if __name__ == "__main__":
+ ecto_ros.init(sys.argv, "segmenter_test")
+
+ #subscribers
+ sub_cloud = ecto_sensor_msgs.Subscriber_PointCloud2("Cloud2 Subscriber",topic_name="/camera/rgb/points")
+
+ #converters (PointCloud2 -> type erased PointCloud -> PointCloud<PointXYZ>)
+ msg2cloud = ecto_pcl_ros.Message2PointCloud("Cloud2 To Type-Erased",format=ecto_pcl.XYZ)
+ #cloud2typed = ecto_pcl.PointCloud2PointCloudT("Type-Erased To XYZ",format=ecto_pcl.XYZ)
+
+ #normals
+ normals = ecto_pcl.NormalEstimation("Normals", k_search=0, radius_search=0.006, spatial_locator=ecto_pcl.KDTREE_ORGANIZED_INDEX)
+ #normals = ecto_pcl.NormalEstimation("Normals", k_search=20, radius_search=0, spatial_locator=ecto_pcl.KDTREE_FLANN)
+
+ #segmentation
+ segmenter = ecto_corrector.Segmenter("Segmenter",depth_threshold=0.002,
+ normal_threshold=0.992,
+ curvature_threshold=0.024)
+
+ #drawing
+ im_drawer = ecto_opencv.highgui.imshow("Drawer",name="segments", waitKey=10)
+
+ graph = [
+ sub_cloud[:] >> msg2cloud[:],
+ msg2cloud[:] >> normals[:],
+ msg2cloud[:] >> segmenter["input"],
+ normals[:] >> segmenter["normals"],
+ segmenter[:] >> im_drawer[:]
+ ]
+
+ plasm = ecto.Plasm()
+ plasm.connect(graph)
+
+ if(debug):
+ ecto.view_plasm(plasm)
+
+ sched = ecto.schedulers.Threadpool(plasm)
+ sched.execute()
View
@@ -9,6 +9,7 @@ ectomodule(ecto_corrector
ModelLoader.cpp
ModelROI.cpp
ModelVertices.cpp
+ Segmenter.cpp
TODServiceCaller.cpp
TranslationOffset.cpp
)
View
@@ -0,0 +1,76 @@
+/*
+ * Segmenter.cpp
+ *
+ * Created on: Aug 26, 2011
+ * Author: mkrainin
+ */
+
+#include <ecto/ecto.hpp>
+
+#include <pose_corrector/utils.h>
+#include <pcl/point_cloud.h>
+#include <opencv2/core/core.hpp>
+
+#include <ecto_pcl.hpp>
+#include <pcl_cell_with_normals.hpp>
+
+namespace ecto_corrector{
+
+struct Segmenter
+{
+ Segmenter(){}
+
+ static void declare_params(ecto::tendrils& params)
+ {
+ params.declare<double>("depth_threshold", "depth difference that constitutes a discontinuity",0.005);
+ params.declare<double>("normal_threshold", "max dot product between adjacent normals",0.95);
+ params.declare<double>("curvature_threshold", "max curvature",0.04);
+ }
+
+ static void declare_io(const ecto::tendrils& params, ecto::tendrils& inputs, ecto::tendrils& outputs)
+ {
+ //output
+ outputs.declare<cv::Mat> ("segment_image", "Image showing the resulting segments in different colors");
+ }
+
+ void configure(const ecto::tendrils& params, const ecto::tendrils& inputs, const ecto::tendrils& outputs)
+ {
+ //params
+ depth_threshold_ = params["depth_threshold"];
+ normal_threshold_ = params["normal_threshold"];
+ curvature_threshold_ = params["curvature_threshold"];
+
+ //output
+ segment_image_ = outputs["segment_image"];
+ }
+
+ template <typename PointT>
+ int process(const ecto::tendrils& inputs, const ecto::tendrils& outputs,
+ boost::shared_ptr<const ::pcl::PointCloud<PointT> >& input,
+ boost::shared_ptr<const ::pcl::PointCloud< ::pcl::Normal> >& normals)
+ {
+ cv::Rect roi(0,0,input->width,input->height);
+
+ std::vector<std::vector<cv::Point2i> > valid_segments;
+ std::vector<cv::Point2i> invalid;
+ pose_corrector::segmentCloud(*input,*normals,roi,valid_segments,invalid,*depth_threshold_,*normal_threshold_,*curvature_threshold_);
+
+ *segment_image_ = pose_corrector::visualizeSegments(roi,valid_segments);
+
+ return ecto::OK;
+ }
+
+ //params
+ ecto::spore<double> depth_threshold_;
+ ecto::spore<double> normal_threshold_;
+ ecto::spore<double> curvature_threshold_;
+
+ //outputs
+ ecto::spore<cv::Mat> segment_image_;
+
+};
+
+}
+
+ECTO_CELL(ecto_corrector, pcl::PclCellWithNormals<ecto_corrector::Segmenter>,
+ "Segmenter", "Segments a pcl::PointCloud based on depth similarity of neighboring pixels");

0 comments on commit 5770e27

Please sign in to comment.