[ros-vision-opencv] 01/05: Imported Upstream version 1.11.10+ds

Jochen Sprickerhof jspricke-guest at moszumanska.debian.org
Sat Jan 23 14:29:27 UTC 2016


This is an automated email from the git hooks/post-receive script.

jspricke-guest pushed a commit to annotated tag debian/1.11.10+ds-1
in repository ros-vision-opencv.

commit 2cbd01ac25a5e913d77b6a68ee2524271f45c23c
Author: Jochen Sprickerhof <git at jochen.sprickerhof.de>
Date:   Tue Jan 19 10:22:50 2016 +0100

    Imported Upstream version 1.11.10+ds
---
 .travis.yml                                        |   5 +-
 cv_bridge/CHANGELOG.rst                            |  12 ++
 cv_bridge/include/cv_bridge/cv_bridge.h            |   5 +-
 cv_bridge/include/cv_bridge/rgb_colors.h           | 211 +++++++++++++++++++++
 cv_bridge/package.xml                              |   2 +-
 cv_bridge/src/CMakeLists.txt                       |   4 +-
 cv_bridge/src/cv_bridge.cpp                        |  85 +++++----
 cv_bridge/src/rgb_colors.cpp                       | 202 ++++++++++++++++++++
 cv_bridge/test/CMakeLists.txt                      |   2 +-
 cv_bridge/test/test_rgb_colors.cpp                 |  19 ++
 image_geometry/CHANGELOG.rst                       |   3 +
 image_geometry/package.xml                         |   2 +-
 opencv_apps/CHANGELOG.rst                          |  16 ++
 opencv_apps/CMakeLists.txt                         |  17 +-
 opencv_apps/package.xml                            |  11 +-
 .../src/nodelet/contour_moments_nodelet.cpp        |   6 +-
 opencv_apps/src/nodelet/convex_hull_nodelet.cpp    |   2 +-
 opencv_apps/src/nodelet/fback_flow_nodelet.cpp     |   4 +-
 .../src/nodelet/goodfeature_track_nodelet.cpp      |  22 ++-
 opencv_apps/src/nodelet/hough_lines_nodelet.cpp    |   2 +-
 opencv_apps/src/nodelet/lk_flow_nodelet.cpp        |   6 +-
 opencv_apps/src/nodelet/phase_corr_nodelet.cpp     |  19 +-
 .../src/nodelet/segment_objects_nodelet.cpp        |   6 +-
 opencv_apps/src/nodelet/simple_flow_nodelet.cpp    |  21 +-
 .../src/nodelet/watershed_segmentation_nodelet.cpp |  38 +++-
 opencv_apps/test/CMakeLists.txt                    |  36 ++++
 opencv_apps/test/test-camshift.test                |  32 ++++
 opencv_apps/test/test-contour_moments.test         |  24 +++
 opencv_apps/test/test-convex_hull.test             |  24 +++
 opencv_apps/test/test-edge_detection.test          |  54 ++++++
 opencv_apps/test/test-face_detection.test          |  23 +++
 opencv_apps/test/test-fback_flow.test              |  23 +++
 opencv_apps/test/test-find_contours.test           |  24 +++
 opencv_apps/test/test-general_contours.test        |  30 +++
 opencv_apps/test/test-goodfeature_track.test       |  24 +++
 opencv_apps/test/test-hough_circles.test           |  25 +++
 opencv_apps/test/test-hough_lines.test             |  42 ++++
 opencv_apps/test/test-lk_flow.test                 |  25 +++
 opencv_apps/test/test-people_detect.test           |  23 +++
 opencv_apps/test/test-phase_corr.test              |  23 +++
 opencv_apps/test/test-segment_objects.test         |  24 +++
 opencv_apps/test/test-simple_flow.test             |  24 +++
 opencv_apps/test/test-watershed_segmentation.test  |  26 +++
 opencv_tests/CHANGELOG.rst                         |   3 +
 opencv_tests/package.xml                           |   2 +-
 vision_opencv/CHANGELOG.rst                        |   3 +
 vision_opencv/package.xml                          |   2 +-
 47 files changed, 1149 insertions(+), 89 deletions(-)

diff --git a/.travis.yml b/.travis.yml
index 226873f..83dfb40 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -4,6 +4,8 @@ language: generic
 env:
   - OPENCV_VERSION=2 ROS_DISTRO=indigo
   - OPENCV_VERSION=3 ROS_DISTRO=indigo
+  - OPENCV_VERSION=2 ROS_DISTRO=jade
+  - OPENCV_VERSION=3 ROS_DISTRO=jade
 # Install system dependencies, namely ROS.
 before_install:
   # Define some config vars.
@@ -14,7 +16,7 @@ before_install:
   - export CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options
   - export ROS_PARALLEL_JOBS='-j8 -l6'
   # Install ROS
-  - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+  - sudo sh -c "echo \"deb http://packages.ros.org/ros-shadow-fixed/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
   - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
   - sudo apt-get update -qq
   # Install ROS
@@ -41,6 +43,7 @@ before_script:
   - cd ~/catkin_ws/src
   - wstool init
   - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi
+  - if [ $OPENCV_VERSION == 3 ]; then wstool set -y image_pipeline --git https://github.com/ros-perception/image_pipeline; fi # need to recompile image_proc
   - wstool up
 
   # package depdencies: install using rosdep.
diff --git a/cv_bridge/CHANGELOG.rst b/cv_bridge/CHANGELOG.rst
index c894de4..dc2e427 100644
--- a/cv_bridge/CHANGELOG.rst
+++ b/cv_bridge/CHANGELOG.rst
@@ -2,6 +2,18 @@
 Changelog for package cv_bridge
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.11.10 (2016-01-16)
+--------------------
+* fix OpenCV3 build
+* Describe about converting label to bgr image in cvtColorForDisplay
+* Convert label to BGR image to display
+* Add test for rgb_colors.cpp
+* Add rgb_colors util
+* Update doc for converting to BGR in cvtColorForDisplay
+* Convert to BGR from any encoding
+* Refactor: sensor_msgs::image_encodings -> enc
+* Contributors: Kentaro Wada, Vincent Rabaud
+
 1.11.9 (2015-11-29)
 -------------------
 * deal with endianness
diff --git a/cv_bridge/include/cv_bridge/cv_bridge.h b/cv_bridge/include/cv_bridge/cv_bridge.h
index b9ada6f..600816a 100644
--- a/cv_bridge/include/cv_bridge/cv_bridge.h
+++ b/cv_bridge/include/cv_bridge/cv_bridge.h
@@ -258,11 +258,14 @@ CvImagePtr cvtColor(const CvImageConstPtr& source,
  *
  * The following rules apply:
  * - if the output encoding is empty, the fact that the input image is mono or multiple-channel is
- * preserved in the ouput image. The bit depth will be 8.
+ * preserved in the ouput image. The bit depth will be 8. it tries to convert to BGR no matter what
+ * encoding image is passed.
  * - if the output encoding is not empty, it must have sensor_msgs::image_encodings::isColor and
  * isMono return true. It must also be 8 bit in depth
  * - if the input encoding is an OpenCV format (e.g. 8UC1), and if we have 1,3 or 4 channels, it is
  * respectively converted to mono, BGR or BGRA.
+ * - if the input encoding is 32SC1, this estimate that image as label image and will convert it as
+ * bgr image with different colors for each label.
  *
  * \param source   A shared_ptr to a sensor_msgs::Image message
  * \param encoding Either an encoding string that returns true in sensor_msgs::image_encodings::isColor
diff --git a/cv_bridge/include/cv_bridge/rgb_colors.h b/cv_bridge/include/cv_bridge/rgb_colors.h
new file mode 100644
index 0000000..1eaa88b
--- /dev/null
+++ b/cv_bridge/include/cv_bridge/rgb_colors.h
@@ -0,0 +1,211 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Original color definition is at scikit-image distributed with
+ * following license disclaimer:
+ *
+ * Copyright (C) 2011, the scikit-image team
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ *   1. Redistributions of source code must retain the above copyright
+ *      notice, this list of conditions and the following disclaimer.
+ *   2. 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.
+ *   3. Neither the name of skimage 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 AUTHOR ``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 AUTHOR 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.
+ *********************************************************************/
+
+#ifndef CV_BRIDGE_RGB_COLORS_H_
+#define CV_BRIDGE_RGB_COLORS_H_
+
+#include <opencv2/opencv.hpp>
+
+
+namespace cv_bridge
+{
+
+namespace rgb_colors
+{
+
+  /**
+   * @brief
+   * 146 rgb colors
+   */
+  enum Colors {
+    ALICEBLUE,
+    ANTIQUEWHITE,
+    AQUA,
+    AQUAMARINE,
+    AZURE,
+    BEIGE,
+    BISQUE,
+    BLACK,
+    BLANCHEDALMOND,
+    BLUE,
+    BLUEVIOLET,
+    BROWN,
+    BURLYWOOD,
+    CADETBLUE,
+    CHARTREUSE,
+    CHOCOLATE,
+    CORAL,
+    CORNFLOWERBLUE,
+    CORNSILK,
+    CRIMSON,
+    CYAN,
+    DARKBLUE,
+    DARKCYAN,
+    DARKGOLDENROD,
+    DARKGRAY,
+    DARKGREEN,
+    DARKGREY,
+    DARKKHAKI,
+    DARKMAGENTA,
+    DARKOLIVEGREEN,
+    DARKORANGE,
+    DARKORCHID,
+    DARKRED,
+    DARKSALMON,
+    DARKSEAGREEN,
+    DARKSLATEBLUE,
+    DARKSLATEGRAY,
+    DARKSLATEGREY,
+    DARKTURQUOISE,
+    DARKVIOLET,
+    DEEPPINK,
+    DEEPSKYBLUE,
+    DIMGRAY,
+    DIMGREY,
+    DODGERBLUE,
+    FIREBRICK,
+    FLORALWHITE,
+    FORESTGREEN,
+    FUCHSIA,
+    GAINSBORO,
+    GHOSTWHITE,
+    GOLD,
+    GOLDENROD,
+    GRAY,
+    GREEN,
+    GREENYELLOW,
+    GREY,
+    HONEYDEW,
+    HOTPINK,
+    INDIANRED,
+    INDIGO,
+    IVORY,
+    KHAKI,
+    LAVENDER,
+    LAVENDERBLUSH,
+    LAWNGREEN,
+    LEMONCHIFFON,
+    LIGHTBLUE,
+    LIGHTCORAL,
+    LIGHTCYAN,
+    LIGHTGOLDENRODYELLOW,
+    LIGHTGRAY,
+    LIGHTGREEN,
+    LIGHTGREY,
+    LIGHTPINK,
+    LIGHTSALMON,
+    LIGHTSEAGREEN,
+    LIGHTSKYBLUE,
+    LIGHTSLATEGRAY,
+    LIGHTSLATEGREY,
+    LIGHTSTEELBLUE,
+    LIGHTYELLOW,
+    LIME,
+    LIMEGREEN,
+    LINEN,
+    MAGENTA,
+    MAROON,
+    MEDIUMAQUAMARINE,
+    MEDIUMBLUE,
+    MEDIUMORCHID,
+    MEDIUMPURPLE,
+    MEDIUMSEAGREEN,
+    MEDIUMSLATEBLUE,
+    MEDIUMSPRINGGREEN,
+    MEDIUMTURQUOISE,
+    MEDIUMVIOLETRED,
+    MIDNIGHTBLUE,
+    MINTCREAM,
+    MISTYROSE,
+    MOCCASIN,
+    NAVAJOWHITE,
+    NAVY,
+    OLDLACE,
+    OLIVE,
+    OLIVEDRAB,
+    ORANGE,
+    ORANGERED,
+    ORCHID,
+    PALEGOLDENROD,
+    PALEGREEN,
+    PALEVIOLETRED,
+    PAPAYAWHIP,
+    PEACHPUFF,
+    PERU,
+    PINK,
+    PLUM,
+    POWDERBLUE,
+    PURPLE,
+    RED,
+    ROSYBROWN,
+    ROYALBLUE,
+    SADDLEBROWN,
+    SALMON,
+    SANDYBROWN,
+    SEAGREEN,
+    SEASHELL,
+    SIENNA,
+    SILVER,
+    SKYBLUE,
+    SLATEBLUE,
+    SLATEGRAY,
+    SLATEGREY,
+    SNOW,
+    SPRINGGREEN,
+    STEELBLUE,
+    TAN,
+    TEAL,
+    THISTLE,
+    TOMATO,
+    TURQUOISE,
+    VIOLET,
+    WHEAT,
+    WHITE,
+    WHITESMOKE,
+    YELLOW,
+    YELLOWGREEN,
+  };
+
+  /**
+   * @brief
+   * get rgb color with enum.
+   */
+  cv::Vec3d getRGBColor(const int color);
+
+} // namespace rgb_colors
+
+}  // namespace cv_bridge
+
+#endif
diff --git a/cv_bridge/package.xml b/cv_bridge/package.xml
index a503c30..fac8a98 100644
--- a/cv_bridge/package.xml
+++ b/cv_bridge/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>cv_bridge</name>
-  <version>1.11.9</version>
+  <version>1.11.10</version>
   <description>
     This contains CvBridge, which converts between ROS
     Image messages and OpenCV images.
diff --git a/cv_bridge/src/CMakeLists.txt b/cv_bridge/src/CMakeLists.txt
index 806cd05..8ec321c 100644
--- a/cv_bridge/src/CMakeLists.txt
+++ b/cv_bridge/src/CMakeLists.txt
@@ -1,6 +1,6 @@
 # add library
 include_directories(./)
-add_library(${PROJECT_NAME} cv_bridge.cpp)
+add_library(${PROJECT_NAME} cv_bridge.cpp rgb_colors.cpp)
 add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES})
 
@@ -35,7 +35,7 @@ if (PYTHON_VERSION_MAJOR VERSION_EQUAL 3)
   add_definitions(-DPYTHON3)
 endif()
 
-if (OpenCV_VERSION VERSION_EQUAL "3")
+if (OpenCV_VERSION_MAJOR VERSION_EQUAL 3)
 add_library(${PROJECT_NAME}_boost module.cpp module_opencv3.cpp)
 else()
 add_library(${PROJECT_NAME}_boost module.cpp module_opencv2.cpp)
diff --git a/cv_bridge/src/cv_bridge.cpp b/cv_bridge/src/cv_bridge.cpp
index 261fa71..e284fd3 100644
--- a/cv_bridge/src/cv_bridge.cpp
+++ b/cv_bridge/src/cv_bridge.cpp
@@ -46,6 +46,7 @@
 #include <sensor_msgs/image_encodings.h>
 
 #include <cv_bridge/cv_bridge.h>
+#include <cv_bridge/rgb_colors.h>
 
 namespace enc = sensor_msgs::image_encodings;
 
@@ -191,15 +192,11 @@ const std::vector<int> getConversionCode(std::string src_encoding, std::string d
 {
   Encoding src_encod = getEncoding(src_encoding);
   Encoding dst_encod = getEncoding(dst_encoding);
-  bool is_src_color_format = sensor_msgs::image_encodings::isColor(src_encoding) ||
-                             sensor_msgs::image_encodings::isMono(src_encoding) ||
-                             sensor_msgs::image_encodings::isBayer(src_encoding) ||
-                             (src_encoding == sensor_msgs::image_encodings::YUV422);
-  bool is_dst_color_format = sensor_msgs::image_encodings::isColor(dst_encoding) ||
-                             sensor_msgs::image_encodings::isMono(dst_encoding) ||
-                             sensor_msgs::image_encodings::isBayer(dst_encoding) ||
-                             (dst_encoding == sensor_msgs::image_encodings::YUV422);
-  bool is_num_channels_the_same = (sensor_msgs::image_encodings::numChannels(src_encoding) == sensor_msgs::image_encodings::numChannels(dst_encoding));
+  bool is_src_color_format = enc::isColor(src_encoding) || enc::isMono(src_encoding) ||
+                             enc::isBayer(src_encoding) || (src_encoding == enc::YUV422);
+  bool is_dst_color_format = enc::isColor(dst_encoding) || enc::isMono(dst_encoding) ||
+                             enc::isBayer(dst_encoding) || (dst_encoding == enc::YUV422);
+  bool is_num_channels_the_same = (enc::numChannels(src_encoding) == enc::numChannels(dst_encoding));
 
   // If we have no color info in the source, we can only convert to the same format which
   // was resolved in the previous condition. Otherwise, fail
@@ -532,26 +529,23 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
   {
     try
     {
-      // If there is a chance we can just share the data, let's try it out
-      if (sensor_msgs::image_encodings::isColor(source->encoding) || sensor_msgs::image_encodings::isMono(source->encoding))
+      // Let's decide upon an output format
+      if (enc::numChannels(source->encoding) == 1)
       {
-        return source;
-      }
-      // Otherwise, let's decide upon an output format
-      if (sensor_msgs::image_encodings::numChannels(source->encoding) == 1)
-      {
-        if ((sensor_msgs::image_encodings::bitDepth(source->encoding) == 8) ||
-            (sensor_msgs::image_encodings::bitDepth(source->encoding) == 16))
-          encoding = sensor_msgs::image_encodings::MONO8;
+        if ((enc::bitDepth(source->encoding) == 8) ||
+            (enc::bitDepth(source->encoding) == 16))
+          encoding = enc::MONO8;
+        else if (enc::bitDepth(source->encoding) == 32)
+          encoding = enc::BGR8;
         else
           throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
       }
       else
       {
         // We choose BGR by default here as we assume people will use OpenCV
-        if ((sensor_msgs::image_encodings::bitDepth(source->encoding) == 8) ||
-            (sensor_msgs::image_encodings::bitDepth(source->encoding) == 16))
-          encoding = sensor_msgs::image_encodings::BGR8;
+        if ((enc::bitDepth(source->encoding) == 8) ||
+            (enc::bitDepth(source->encoding) == 16))
+          encoding = enc::BGR8;
         else
           throw std::runtime_error("Unsupported depth of the source encoding " + encoding);
       }
@@ -564,12 +558,37 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
   }
   else
   {
-    if ((!sensor_msgs::image_encodings::isColor(encoding_out) && !sensor_msgs::image_encodings::isMono(encoding_out)) ||
-        (sensor_msgs::image_encodings::bitDepth(encoding) != 8))
+    if ((!enc::isColor(encoding_out) && !enc::isMono(encoding_out)) ||
+        (enc::bitDepth(encoding) != 8))
       throw Exception("cv_bridge.cvtColorForDisplay() does not have an output encoding that is color or mono, and has is bit in depth");
 
   }
 
+  // Convert label to bgr image
+  if (encoding == sensor_msgs::image_encodings::BGR8 &&
+      sensor_msgs::image_encodings::bitDepth(source->encoding) == 32)
+  {
+    CvImagePtr result(new CvImage());
+    result->header = source->header;
+    result->encoding = encoding;
+    result->image = cv::Mat(source->image.rows, source->image.cols, CV_8UC3);
+    for (size_t j = 0; j < source->image.rows; ++j) {
+      for (size_t i = 0; i < source->image.cols; ++i) {
+        int label = source->image.at<int>(j, i);
+        if (label == -1) {  // background label
+          result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(0, 0, 0);
+        }
+        else
+        {
+          cv::Vec3d rgb = rgb_colors::getRGBColor(label);
+          // result image should be BGR
+          result->image.at<cv::Vec3b>(j, i) = cv::Vec3b(int(rgb[2] * 255), int(rgb[1] * 255), int(rgb[0] * 255));
+        }
+      }
+    }
+    return result;
+  }
+
   // Perform scaling if asked for
   if (do_dynamic_scaling)
   {
@@ -579,7 +598,7 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
       CvImagePtr result(new CvImage());
       result->header = source->header;
       result->encoding = encoding;
-      if (sensor_msgs::image_encodings::bitDepth(encoding) == 1)
+      if (enc::bitDepth(encoding) == 1)
       {
         result->image = cv::Mat(source->image.size(), CV_8UC1);
         result->image.setTo(255./2.);
@@ -593,11 +612,11 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
 
   if (min_image_value != max_image_value)
   {
-    if (sensor_msgs::image_encodings::numChannels(source->encoding) != 1)
+    if (enc::numChannels(source->encoding) != 1)
       throw Exception("cv_bridge.cvtColorForDisplay() scaling for images with more than one channel is unsupported");
     CvImagePtr img_scaled_8u(new CvImage());
     img_scaled_8u->header = source->header;
-    img_scaled_8u->encoding = sensor_msgs::image_encodings::MONO8;
+    img_scaled_8u->encoding = enc::MONO8;
     cv::Mat(source->image-min_image_value).convertTo(img_scaled_8u->image, CV_8UC1, 255.0 /
       (max_image_value - min_image_value));
     return cvtColor(img_scaled_8u, encoding);
@@ -611,17 +630,17 @@ CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr& source,
 
   // If we get the OpenCV format, if we have 1,3 or 4 channels, we are most likely in mono, BGR or BGRA modes
   if (source->encoding == "CV_8UC1")
-    source_typed->encoding = sensor_msgs::image_encodings::MONO8;
+    source_typed->encoding = enc::MONO8;
   else if (source->encoding == "16UC1")
-    source_typed->encoding = sensor_msgs::image_encodings::MONO16;
+    source_typed->encoding = enc::MONO16;
   else if (source->encoding == "CV_8UC3")
-    source_typed->encoding = sensor_msgs::image_encodings::BGR8;
+    source_typed->encoding = enc::BGR8;
   else if (source->encoding == "CV_8UC4")
-    source_typed->encoding = sensor_msgs::image_encodings::BGRA8;
+    source_typed->encoding = enc::BGRA8;
   else if (source->encoding == "CV_16UC3")
-    source_typed->encoding = sensor_msgs::image_encodings::BGR8;
+    source_typed->encoding = enc::BGR8;
   else if (source->encoding == "CV_16UC4")
-    source_typed->encoding = sensor_msgs::image_encodings::BGRA8;
+    source_typed->encoding = enc::BGRA8;
 
   // If no conversion is needed, don't convert
   if (source_typed->encoding == encoding)
diff --git a/cv_bridge/src/rgb_colors.cpp b/cv_bridge/src/rgb_colors.cpp
new file mode 100644
index 0000000..ca2eaaa
--- /dev/null
+++ b/cv_bridge/src/rgb_colors.cpp
@@ -0,0 +1,202 @@
+// -*- mode: c++ -*-
+/*********************************************************************
+ * Original color definition is at scikit-image distributed with
+ * following license disclaimer:
+ *
+ * Copyright (C) 2011, the scikit-image team
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ *   1. Redistributions of source code must retain the above copyright
+ *      notice, this list of conditions and the following disclaimer.
+ *   2. 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.
+ *   3. Neither the name of skimage 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 AUTHOR ``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 AUTHOR 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 "cv_bridge/rgb_colors.h"
+#include <opencv2/core/core.hpp>
+
+
+namespace cv_bridge
+{
+
+namespace rgb_colors
+{
+
+  cv::Vec3d getRGBColor(const int color)
+  {
+    cv::Vec3d c;
+    switch (color % 146) {
+      case ALICEBLUE: c = cv::Vec3d(0.941, 0.973, 1); break;
+      case ANTIQUEWHITE: c = cv::Vec3d(0.98, 0.922, 0.843); break;
+      case AQUA: c = cv::Vec3d(0, 1, 1); break;
+      case AQUAMARINE: c = cv::Vec3d(0.498, 1, 0.831); break;
+      case AZURE: c = cv::Vec3d(0.941, 1, 1); break;
+      case BEIGE: c = cv::Vec3d(0.961, 0.961, 0.863); break;
+      case BISQUE: c = cv::Vec3d(1, 0.894, 0.769); break;
+      case BLACK: c = cv::Vec3d(0, 0, 0); break;
+      case BLANCHEDALMOND: c = cv::Vec3d(1, 0.922, 0.804); break;
+      case BLUE: c = cv::Vec3d(0, 0, 1); break;
+      case BLUEVIOLET: c = cv::Vec3d(0.541, 0.169, 0.886); break;
+      case BROWN: c = cv::Vec3d(0.647, 0.165, 0.165); break;
+      case BURLYWOOD: c = cv::Vec3d(0.871, 0.722, 0.529); break;
+      case CADETBLUE: c = cv::Vec3d(0.373, 0.62, 0.627); break;
+      case CHARTREUSE: c = cv::Vec3d(0.498, 1, 0); break;
+      case CHOCOLATE: c = cv::Vec3d(0.824, 0.412, 0.118); break;
+      case CORAL: c = cv::Vec3d(1, 0.498, 0.314); break;
+      case CORNFLOWERBLUE: c = cv::Vec3d(0.392, 0.584, 0.929); break;
+      case CORNSILK: c = cv::Vec3d(1, 0.973, 0.863); break;
+      case CRIMSON: c = cv::Vec3d(0.863, 0.0784, 0.235); break;
+      case CYAN: c = cv::Vec3d(0, 1, 1); break;
+      case DARKBLUE: c = cv::Vec3d(0, 0, 0.545); break;
+      case DARKCYAN: c = cv::Vec3d(0, 0.545, 0.545); break;
+      case DARKGOLDENROD: c = cv::Vec3d(0.722, 0.525, 0.0431); break;
+      case DARKGRAY: c = cv::Vec3d(0.663, 0.663, 0.663); break;
+      case DARKGREEN: c = cv::Vec3d(0, 0.392, 0); break;
+      case DARKGREY: c = cv::Vec3d(0.663, 0.663, 0.663); break;
+      case DARKKHAKI: c = cv::Vec3d(0.741, 0.718, 0.42); break;
+      case DARKMAGENTA: c = cv::Vec3d(0.545, 0, 0.545); break;
+      case DARKOLIVEGREEN: c = cv::Vec3d(0.333, 0.42, 0.184); break;
+      case DARKORANGE: c = cv::Vec3d(1, 0.549, 0); break;
+      case DARKORCHID: c = cv::Vec3d(0.6, 0.196, 0.8); break;
+      case DARKRED: c = cv::Vec3d(0.545, 0, 0); break;
+      case DARKSALMON: c = cv::Vec3d(0.914, 0.588, 0.478); break;
+      case DARKSEAGREEN: c = cv::Vec3d(0.561, 0.737, 0.561); break;
+      case DARKSLATEBLUE: c = cv::Vec3d(0.282, 0.239, 0.545); break;
+      case DARKSLATEGRAY: c = cv::Vec3d(0.184, 0.31, 0.31); break;
+      case DARKSLATEGREY: c = cv::Vec3d(0.184, 0.31, 0.31); break;
+      case DARKTURQUOISE: c = cv::Vec3d(0, 0.808, 0.82); break;
+      case DARKVIOLET: c = cv::Vec3d(0.58, 0, 0.827); break;
+      case DEEPPINK: c = cv::Vec3d(1, 0.0784, 0.576); break;
+      case DEEPSKYBLUE: c = cv::Vec3d(0, 0.749, 1); break;
+      case DIMGRAY: c = cv::Vec3d(0.412, 0.412, 0.412); break;
+      case DIMGREY: c = cv::Vec3d(0.412, 0.412, 0.412); break;
+      case DODGERBLUE: c = cv::Vec3d(0.118, 0.565, 1); break;
+      case FIREBRICK: c = cv::Vec3d(0.698, 0.133, 0.133); break;
+      case FLORALWHITE: c = cv::Vec3d(1, 0.98, 0.941); break;
+      case FORESTGREEN: c = cv::Vec3d(0.133, 0.545, 0.133); break;
+      case FUCHSIA: c = cv::Vec3d(1, 0, 1); break;
+      case GAINSBORO: c = cv::Vec3d(0.863, 0.863, 0.863); break;
+      case GHOSTWHITE: c = cv::Vec3d(0.973, 0.973, 1); break;
+      case GOLD: c = cv::Vec3d(1, 0.843, 0); break;
+      case GOLDENROD: c = cv::Vec3d(0.855, 0.647, 0.125); break;
+      case GRAY: c = cv::Vec3d(0.502, 0.502, 0.502); break;
+      case GREEN: c = cv::Vec3d(0, 0.502, 0); break;
+      case GREENYELLOW: c = cv::Vec3d(0.678, 1, 0.184); break;
+      case GREY: c = cv::Vec3d(0.502, 0.502, 0.502); break;
+      case HONEYDEW: c = cv::Vec3d(0.941, 1, 0.941); break;
+      case HOTPINK: c = cv::Vec3d(1, 0.412, 0.706); break;
+      case INDIANRED: c = cv::Vec3d(0.804, 0.361, 0.361); break;
+      case INDIGO: c = cv::Vec3d(0.294, 0, 0.51); break;
+      case IVORY: c = cv::Vec3d(1, 1, 0.941); break;
+      case KHAKI: c = cv::Vec3d(0.941, 0.902, 0.549); break;
+      case LAVENDER: c = cv::Vec3d(0.902, 0.902, 0.98); break;
+      case LAVENDERBLUSH: c = cv::Vec3d(1, 0.941, 0.961); break;
+      case LAWNGREEN: c = cv::Vec3d(0.486, 0.988, 0); break;
+      case LEMONCHIFFON: c = cv::Vec3d(1, 0.98, 0.804); break;
+      case LIGHTBLUE: c = cv::Vec3d(0.678, 0.847, 0.902); break;
+      case LIGHTCORAL: c = cv::Vec3d(0.941, 0.502, 0.502); break;
+      case LIGHTCYAN: c = cv::Vec3d(0.878, 1, 1); break;
+      case LIGHTGOLDENRODYELLOW: c = cv::Vec3d(0.98, 0.98, 0.824); break;
+      case LIGHTGRAY: c = cv::Vec3d(0.827, 0.827, 0.827); break;
+      case LIGHTGREEN: c = cv::Vec3d(0.565, 0.933, 0.565); break;
+      case LIGHTGREY: c = cv::Vec3d(0.827, 0.827, 0.827); break;
+      case LIGHTPINK: c = cv::Vec3d(1, 0.714, 0.757); break;
+      case LIGHTSALMON: c = cv::Vec3d(1, 0.627, 0.478); break;
+      case LIGHTSEAGREEN: c = cv::Vec3d(0.125, 0.698, 0.667); break;
+      case LIGHTSKYBLUE: c = cv::Vec3d(0.529, 0.808, 0.98); break;
+      case LIGHTSLATEGRAY: c = cv::Vec3d(0.467, 0.533, 0.6); break;
+      case LIGHTSLATEGREY: c = cv::Vec3d(0.467, 0.533, 0.6); break;
+      case LIGHTSTEELBLUE: c = cv::Vec3d(0.69, 0.769, 0.871); break;
+      case LIGHTYELLOW: c = cv::Vec3d(1, 1, 0.878); break;
+      case LIME: c = cv::Vec3d(0, 1, 0); break;
+      case LIMEGREEN: c = cv::Vec3d(0.196, 0.804, 0.196); break;
+      case LINEN: c = cv::Vec3d(0.98, 0.941, 0.902); break;
+      case MAGENTA: c = cv::Vec3d(1, 0, 1); break;
+      case MAROON: c = cv::Vec3d(0.502, 0, 0); break;
+      case MEDIUMAQUAMARINE: c = cv::Vec3d(0.4, 0.804, 0.667); break;
+      case MEDIUMBLUE: c = cv::Vec3d(0, 0, 0.804); break;
+      case MEDIUMORCHID: c = cv::Vec3d(0.729, 0.333, 0.827); break;
+      case MEDIUMPURPLE: c = cv::Vec3d(0.576, 0.439, 0.859); break;
+      case MEDIUMSEAGREEN: c = cv::Vec3d(0.235, 0.702, 0.443); break;
+      case MEDIUMSLATEBLUE: c = cv::Vec3d(0.482, 0.408, 0.933); break;
+      case MEDIUMSPRINGGREEN: c = cv::Vec3d(0, 0.98, 0.604); break;
+      case MEDIUMTURQUOISE: c = cv::Vec3d(0.282, 0.82, 0.8); break;
+      case MEDIUMVIOLETRED: c = cv::Vec3d(0.78, 0.0824, 0.522); break;
+      case MIDNIGHTBLUE: c = cv::Vec3d(0.098, 0.098, 0.439); break;
+      case MINTCREAM: c = cv::Vec3d(0.961, 1, 0.98); break;
+      case MISTYROSE: c = cv::Vec3d(1, 0.894, 0.882); break;
+      case MOCCASIN: c = cv::Vec3d(1, 0.894, 0.71); break;
+      case NAVAJOWHITE: c = cv::Vec3d(1, 0.871, 0.678); break;
+      case NAVY: c = cv::Vec3d(0, 0, 0.502); break;
+      case OLDLACE: c = cv::Vec3d(0.992, 0.961, 0.902); break;
+      case OLIVE: c = cv::Vec3d(0.502, 0.502, 0); break;
+      case OLIVEDRAB: c = cv::Vec3d(0.42, 0.557, 0.137); break;
+      case ORANGE: c = cv::Vec3d(1, 0.647, 0); break;
+      case ORANGERED: c = cv::Vec3d(1, 0.271, 0); break;
+      case ORCHID: c = cv::Vec3d(0.855, 0.439, 0.839); break;
+      case PALEGOLDENROD: c = cv::Vec3d(0.933, 0.91, 0.667); break;
+      case PALEGREEN: c = cv::Vec3d(0.596, 0.984, 0.596); break;
+      case PALEVIOLETRED: c = cv::Vec3d(0.686, 0.933, 0.933); break;
+      case PAPAYAWHIP: c = cv::Vec3d(1, 0.937, 0.835); break;
+      case PEACHPUFF: c = cv::Vec3d(1, 0.855, 0.725); break;
+      case PERU: c = cv::Vec3d(0.804, 0.522, 0.247); break;
+      case PINK: c = cv::Vec3d(1, 0.753, 0.796); break;
+      case PLUM: c = cv::Vec3d(0.867, 0.627, 0.867); break;
+      case POWDERBLUE: c = cv::Vec3d(0.69, 0.878, 0.902); break;
+      case PURPLE: c = cv::Vec3d(0.502, 0, 0.502); break;
+      case RED: c = cv::Vec3d(1, 0, 0); break;
+      case ROSYBROWN: c = cv::Vec3d(0.737, 0.561, 0.561); break;
+      case ROYALBLUE: c = cv::Vec3d(0.255, 0.412, 0.882); break;
+      case SADDLEBROWN: c = cv::Vec3d(0.545, 0.271, 0.0745); break;
+      case SALMON: c = cv::Vec3d(0.98, 0.502, 0.447); break;
+      case SANDYBROWN: c = cv::Vec3d(0.98, 0.643, 0.376); break;
+      case SEAGREEN: c = cv::Vec3d(0.18, 0.545, 0.341); break;
+      case SEASHELL: c = cv::Vec3d(1, 0.961, 0.933); break;
+      case SIENNA: c = cv::Vec3d(0.627, 0.322, 0.176); break;
+      case SILVER: c = cv::Vec3d(0.753, 0.753, 0.753); break;
+      case SKYBLUE: c = cv::Vec3d(0.529, 0.808, 0.922); break;
+      case SLATEBLUE: c = cv::Vec3d(0.416, 0.353, 0.804); break;
+      case SLATEGRAY: c = cv::Vec3d(0.439, 0.502, 0.565); break;
+      case SLATEGREY: c = cv::Vec3d(0.439, 0.502, 0.565); break;
+      case SNOW: c = cv::Vec3d(1, 0.98, 0.98); break;
+      case SPRINGGREEN: c = cv::Vec3d(0, 1, 0.498); break;
+      case STEELBLUE: c = cv::Vec3d(0.275, 0.51, 0.706); break;
+      case TAN: c = cv::Vec3d(0.824, 0.706, 0.549); break;
+      case TEAL: c = cv::Vec3d(0, 0.502, 0.502); break;
+      case THISTLE: c = cv::Vec3d(0.847, 0.749, 0.847); break;
+      case TOMATO: c = cv::Vec3d(1, 0.388, 0.278); break;
+      case TURQUOISE: c = cv::Vec3d(0.251, 0.878, 0.816); break;
+      case VIOLET: c = cv::Vec3d(0.933, 0.51, 0.933); break;
+      case WHEAT: c = cv::Vec3d(0.961, 0.871, 0.702); break;
+      case WHITE: c = cv::Vec3d(1, 1, 1); break;
+      case WHITESMOKE: c = cv::Vec3d(0.961, 0.961, 0.961); break;
+      case YELLOW: c = cv::Vec3d(1, 1, 0); break;
+      case YELLOWGREEN: c = cv::Vec3d(0.604, 0.804, 0.196); break;
+    }  // switch
+    return c;
+  }
+
+} // namespace rgb_colors
+
+}  // namespace cv_bridge
diff --git a/cv_bridge/test/CMakeLists.txt b/cv_bridge/test/CMakeLists.txt
index 612176c..c933afa 100644
--- a/cv_bridge/test/CMakeLists.txt
+++ b/cv_bridge/test/CMakeLists.txt
@@ -3,7 +3,7 @@
 # add boost directories for now
 include_directories("../src")
 
-catkin_add_gtest(${PROJECT_NAME}-utest test_endian.cpp utest.cpp utest2.cpp)
+catkin_add_gtest(${PROJECT_NAME}-utest test_endian.cpp utest.cpp utest2.cpp test_rgb_colors.cpp)
 target_link_libraries(${PROJECT_NAME}-utest
   ${PROJECT_NAME}
   ${OpenCV_LIBRARIES}
diff --git a/cv_bridge/test/test_rgb_colors.cpp b/cv_bridge/test/test_rgb_colors.cpp
new file mode 100644
index 0000000..090e667
--- /dev/null
+++ b/cv_bridge/test/test_rgb_colors.cpp
@@ -0,0 +1,19 @@
+#include "cv_bridge/rgb_colors.h"
+#include <opencv2/opencv.hpp>
+#include <gtest/gtest.h>
+
+
+TEST(RGBColors, testGetRGBColor)
+{
+  cv::Vec3d color;
+  // red
+  color = cv_bridge::rgb_colors::getRGBColor(cv_bridge::rgb_colors::RED);
+  EXPECT_EQ(1, color[0]);
+  EXPECT_EQ(0, color[1]);
+  EXPECT_EQ(0, color[2]);
+  // gray
+  color = cv_bridge::rgb_colors::getRGBColor(cv_bridge::rgb_colors::GRAY);
+  EXPECT_EQ(0.502, color[0]);
+  EXPECT_EQ(0.502, color[1]);
+  EXPECT_EQ(0.502, color[2]);
+}
diff --git a/image_geometry/CHANGELOG.rst b/image_geometry/CHANGELOG.rst
index d59ed32..d9f8358 100644
--- a/image_geometry/CHANGELOG.rst
+++ b/image_geometry/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package image_geometry
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.11.10 (2016-01-16)
+--------------------
+
 1.11.9 (2015-11-29)
 -------------------
 * add a condition if D=None
diff --git a/image_geometry/package.xml b/image_geometry/package.xml
index b4912a0..dfa4d4e 100644
--- a/image_geometry/package.xml
+++ b/image_geometry/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>image_geometry</name>
-  <version>1.11.9</version>
+  <version>1.11.10</version>
   <description>
     `image_geometry` contains C++ and Python libraries for interpreting images
     geometrically. It interfaces the calibration parameters in sensor_msgs/CameraInfo
diff --git a/opencv_apps/CHANGELOG.rst b/opencv_apps/CHANGELOG.rst
index 160157c..bdccbe8 100644
--- a/opencv_apps/CHANGELOG.rst
+++ b/opencv_apps/CHANGELOG.rst
@@ -2,6 +2,22 @@
 Changelog for package opencv_apps
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.11.10 (2016-01-16)
+--------------------
+* enable simple_flow on opencv3, https://github.com/ros-perception/vision_opencv/commit/8ed5ff5c48b4c3d270cd8216175cf6a8441cb046 can revert https://github.com/ros-perception/vision_opencv/commit/89a933aef7c11acdb75a17c46bfcb60389b25baa
+* lk_flow_nodeletcpp, fback_flow_nodelet.cpp: need to copy input image to gray
+* opencv_apps: add test programs, this will generate images for wiki
+* fix OpenCV3 build
+* phase_corr: fix display, bigger circle and line
+* goodfeature_track_nodelet.cpp: publish good feature points as corners
+* set image encoding to bgr8
+* convex_hull: draw hull with width 4
+* watershed_segmentatoin_nodelet.cpp: output segmented area as contours and suppot add_seed_points as input of segmentatoin seed
+* src/nodelet/segment_objects_nodelet.cpp: change output image topic name from segmented to image
+* Convert rgb image to bgr in lk_flow
+* [oepncv_apps] fix bug for segment_objects_nodelet.cpp
+* Contributors: Kei Okada, Kentaro Wada, Shingo Kitagawa, Vincent Rabaud
+
 1.11.9 (2015-11-29)
 -------------------
 * Accept grayscale images as input as well
diff --git a/opencv_apps/CMakeLists.txt b/opencv_apps/CMakeLists.txt
index 90d0262..fafec67 100644
--- a/opencv_apps/CMakeLists.txt
+++ b/opencv_apps/CMakeLists.txt
@@ -4,13 +4,6 @@ project(opencv_apps)
 find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp)
 
 find_package(OpenCV REQUIRED)
-if (OpenCV_VERSION VERSION_EQUAL "3")
-  set(${PROJECT_NAME}_EXTRA_CFG)
-  set(${PROJECT_NAME}_EXTRA_FILES)
-else()
-  set(${PROJECT_NAME}_EXTRA_CFG cfg/SimpleFlow.cfg)
-  set(${PROJECT_NAME}_EXTRA_FILES src/nodelet/simple_flow_nodelet.cpp)
-endif()
 
 # generate the dynamic_reconfigure config file
 generate_dynamic_reconfigure_options(
@@ -25,8 +18,8 @@ generate_dynamic_reconfigure_options(
   cfg/PeopleDetect.cfg
   cfg/PhaseCorr.cfg
   cfg/SegmentObjects.cfg
+  cfg/SimpleFlow.cfg
   cfg/WatershedSegmentation.cfg
-  ${${PROJECT_NAME}_EXTRA_CFG}
   )
 
 ## Generate messages in the 'msg' folder
@@ -164,7 +157,7 @@ add_library(${PROJECT_NAME} SHARED
   # rgbdodometry.cpp
   src/nodelet/segment_objects_nodelet.cpp
   ## select3dobj.cpp
-  # src/nodelet/simple_flow_nodelet.cpp
+  src/nodelet/simple_flow_nodelet.cpp
   # squares.cpp
   # starter_imagelist.cpp
   # starter_video.cpp
@@ -294,3 +287,9 @@ install(TARGETS edge_detection_exe hough_lines_exe hough_circles_exe face_detect
 install(FILES nodelet_plugins.xml
         DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
+
+## test
+if(CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS rostest)
+  add_subdirectory(test)
+endif()
diff --git a/opencv_apps/package.xml b/opencv_apps/package.xml
index efd8c45..447b1f3 100644
--- a/opencv_apps/package.xml
+++ b/opencv_apps/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>opencv_apps</name>
-  <version>1.11.9</version>
+  <version>1.11.10</version>
   <description>The opencv_apps package, most of code is taken from https://github.com/Itseez/opencv/tree/master/samples/cpp</description>
 
   <maintainer email="kei.okada at gmail.com">Kei Okada</maintainer>
@@ -27,6 +27,15 @@
   <run_depend>roscpp</run_depend>
   <run_depend>std_srvs</run_depend>
 
+  <test_depend>rostest</test_depend>
+  <test_depend>rosbag</test_depend>
+  <test_depend>rosservice</test_depend>
+  <test_depend>rostopic</test_depend>
+  <test_depend>procps</test_depend>
+  <test_depend>image_proc</test_depend>
+  <test_depend>image_view</test_depend>
+  <test_depend>topic_tools</test_depend>
+
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
   </export>
diff --git a/opencv_apps/src/nodelet/contour_moments_nodelet.cpp b/opencv_apps/src/nodelet/contour_moments_nodelet.cpp
index 5084bc3..3a981d8 100644
--- a/opencv_apps/src/nodelet/contour_moments_nodelet.cpp
+++ b/opencv_apps/src/nodelet/contour_moments_nodelet.cpp
@@ -170,7 +170,7 @@ class ContourMomentsNodelet : public nodelet::Nodelet
       printf("\t Info: Area and Contour Length \n");
       for( size_t i = 0; i< contours.size(); i++ )
       {
-        printf(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f \n", (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength( contours[i], true ) );
+        NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f \n", (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength( contours[i], true ) );
         cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
         cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
         cv::circle( drawing, mc[i], 4, color, -1, 8, 0 );
@@ -215,7 +215,7 @@ class ContourMomentsNodelet : public nodelet::Nodelet
       }
 
       // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, drawing).toImageMsg();
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", drawing).toImageMsg();
       img_pub_.publish(out_img);
       msg_pub_.publish(moments_msg);
     }
@@ -284,7 +284,7 @@ public:
     subscriber_count_ = 0;
     prev_stamp_ = ros::Time(0, 0);
 
-    window_name_ = "Edge Detection Demo";
+    window_name_ = "Contours";
     low_threshold_ = 100; // only for canny
 
     image_transport::SubscriberStatusCallback img_connect_cb    = boost::bind(&ContourMomentsNodelet::img_connectCb, this, _1);
diff --git a/opencv_apps/src/nodelet/convex_hull_nodelet.cpp b/opencv_apps/src/nodelet/convex_hull_nodelet.cpp
index 6f1408d..9b5fc4e 100644
--- a/opencv_apps/src/nodelet/convex_hull_nodelet.cpp
+++ b/opencv_apps/src/nodelet/convex_hull_nodelet.cpp
@@ -161,7 +161,7 @@ class ConvexHullNodelet : public nodelet::Nodelet
       {
         cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
         cv::drawContours( drawing, contours, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
-        cv::drawContours( drawing, hull, (int)i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
+        cv::drawContours( drawing, hull, (int)i, color, 4, 8, std::vector<cv::Vec4i>(), 0, cv::Point() );
 
         opencv_apps::Contour contour_msg;
         for ( size_t j = 0; j < hull[i].size(); j++ ) {
diff --git a/opencv_apps/src/nodelet/fback_flow_nodelet.cpp b/opencv_apps/src/nodelet/fback_flow_nodelet.cpp
index cdad8c2..24dbc6e 100644
--- a/opencv_apps/src/nodelet/fback_flow_nodelet.cpp
+++ b/opencv_apps/src/nodelet/fback_flow_nodelet.cpp
@@ -130,7 +130,7 @@ class FBackFlowNodelet : public nodelet::Nodelet
       if ( frame.channels() > 1 ) {
         cv::cvtColor( frame, gray, cv::COLOR_BGR2GRAY );
       } else {
-        gray = frame;
+        frame.copyTo(gray);
       }
       if( prevgray.data )
       {
@@ -170,7 +170,7 @@ class FBackFlowNodelet : public nodelet::Nodelet
 
 
       // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, cflow).toImageMsg();
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", cflow).toImageMsg();
       img_pub_.publish(out_img);
       msg_pub_.publish(flows_msg);
     }
diff --git a/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp b/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp
index c6d505c..e37704a 100644
--- a/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp
+++ b/opencv_apps/src/nodelet/goodfeature_track_nodelet.cpp
@@ -32,7 +32,7 @@
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughLines_Demo.cpp
+// http://github.com/Itseez/opencv/blob/master/samples/cpp/tutorial_code/TrackingMotion/goodFeaturesToTrack_Demo.cpp
 /**
  * @function goodFeaturesToTrack_Demo.cpp
  * @brief Demo code for detecting corners using Shi-Tomasi method
@@ -49,6 +49,8 @@
 
 #include <dynamic_reconfigure/server.h>
 #include "opencv_apps/GoodfeatureTrackConfig.h"
+#include "opencv_apps/Point2D.h"
+#include "opencv_apps/Point2DArrayStamped.h"
 
 namespace goodfeature_track {
 class GoodfeatureTrackNodelet : public nodelet::Nodelet
@@ -115,8 +117,8 @@ class GoodfeatureTrackNodelet : public nodelet::Nodelet
       cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
 
       // Messages
-      //opencv_apps::LineArrayStamped lines_msg;
-      //lines_msg.header = msg->header;
+      opencv_apps::Point2DArrayStamped corners_msg;
+      corners_msg.header = msg->header;
 
       // Do the work
       cv::Mat src_gray;
@@ -126,6 +128,7 @@ class GoodfeatureTrackNodelet : public nodelet::Nodelet
         cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
       } else {
         src_gray = frame;
+        cv::cvtColor( src_gray, frame, cv::COLOR_GRAY2BGR );
       }
 
       if( debug_view_) {
@@ -176,10 +179,17 @@ class GoodfeatureTrackNodelet : public nodelet::Nodelet
         int c = cv::waitKey(1);
       }
 
+      // Create msgs
+      for( size_t i = 0; i< corners.size(); i++ ) {
+        opencv_apps::Point2D corner;
+        corner.x = corners[i].x;
+        corner.y = corners[i].y;
+        corners_msg.points.push_back(corner);
+      }
       // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg();
       img_pub_.publish(out_img);
-      //msg_pub_.publish(lines_msg);
+      msg_pub_.publish(corners_msg);
     }
     catch (cv::Exception &e)
     {
@@ -254,7 +264,7 @@ public:
     ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&GoodfeatureTrackNodelet::msg_connectCb, this, _1);
     ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&GoodfeatureTrackNodelet::msg_disconnectCb, this, _1);
     img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    //msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);
+    msg_pub_ = local_nh_.advertise<opencv_apps::Point2DArrayStamped>("corners", 1, msg_connect_cb, msg_disconnect_cb);
 
     if( debug_view_ ) {
       subscriber_count_++;
diff --git a/opencv_apps/src/nodelet/hough_lines_nodelet.cpp b/opencv_apps/src/nodelet/hough_lines_nodelet.cpp
index 2c5f437..d12694f 100644
--- a/opencv_apps/src/nodelet/hough_lines_nodelet.cpp
+++ b/opencv_apps/src/nodelet/hough_lines_nodelet.cpp
@@ -221,7 +221,7 @@ class HoughLinesNodelet : public nodelet::Nodelet
       }
 
       // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding,frame).toImageMsg();
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg();
       img_pub_.publish(out_img);
       msg_pub_.publish(lines_msg);
     }
diff --git a/opencv_apps/src/nodelet/lk_flow_nodelet.cpp b/opencv_apps/src/nodelet/lk_flow_nodelet.cpp
index b759c71..c0ae5ed 100644
--- a/opencv_apps/src/nodelet/lk_flow_nodelet.cpp
+++ b/opencv_apps/src/nodelet/lk_flow_nodelet.cpp
@@ -131,6 +131,10 @@ class LKFlowNodelet : public nodelet::Nodelet
     {
       // Convert the image into something opencv can handle.
       cv::Mat image = cv_bridge::toCvShare(msg, msg->encoding)->image;
+      if (msg->encoding == sensor_msgs::image_encodings::RGB8 ||
+          msg->encoding == sensor_msgs::image_encodings::RGB16) {
+        cv::cvtColor(image, image, cv::COLOR_RGB2BGR);
+      }
 
       // Messages
       opencv_apps::FlowArrayStamped flows_msg;
@@ -153,7 +157,7 @@ class LKFlowNodelet : public nodelet::Nodelet
       if ( image.channels() > 1 ) {
         cv::cvtColor( image, gray, cv::COLOR_BGR2GRAY );
       } else {
-        gray = image;
+        image.copyTo(gray);
       }
 
       if( nightMode )
diff --git a/opencv_apps/src/nodelet/phase_corr_nodelet.cpp b/opencv_apps/src/nodelet/phase_corr_nodelet.cpp
index fef1d32..00ae1af 100644
--- a/opencv_apps/src/nodelet/phase_corr_nodelet.cpp
+++ b/opencv_apps/src/nodelet/phase_corr_nodelet.cpp
@@ -32,12 +32,7 @@
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
-// https://github.com/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/HoughLines_Demo.cpp
-/**
- * @file HoughLines_Demo.cpp
- * @brief Demo code for Hough Transform
- * @author OpenCV team
- */
+// https://github.com/Itseez/opencv/blob/master/samples/cpp/phase_corr.cpp
 
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
@@ -120,7 +115,7 @@ class PhaseCorrNodelet : public nodelet::Nodelet
 
       // Do the work
       if ( frame.channels() > 1 ) {
-        cv::cvtColor( frame, curr, cv::COLOR_RGB2GRAY );
+        cv::cvtColor( frame, curr, cv::COLOR_BGR2GRAY );
       } else {
         curr = frame;
       }
@@ -145,16 +140,16 @@ class PhaseCorrNodelet : public nodelet::Nodelet
       cv::Point2d shift = cv::phaseCorrelate(prev64f, curr64f, hann);
       double radius = cv::sqrt(shift.x*shift.x + shift.y*shift.y);
 
-      if(radius > 5)
+      if(radius > 0)
       {
         // draw a circle and line indicating the shift direction...
         cv::Point center(curr.cols >> 1, curr.rows >> 1);
 #ifndef CV_VERSION_EPOCH
-        cv::circle(frame, center, (int)radius, cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
-        cv::line(frame, center, cv::Point(center.x + (int)shift.x, center.y + (int)shift.y), cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
+        cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
+        cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
 #else
-        cv::circle(frame, center, (int)radius, cv::Scalar(0, 255, 0), 3, CV_AA);
-        cv::line(frame, center, cv::Point(center.x + (int)shift.x, center.y + (int)shift.y), cv::Scalar(0, 255, 0), 3, CV_AA);
+        cv::circle(frame, center, (int)(radius*5), cv::Scalar(0, 255, 0), 3, CV_AA);
+        cv::line(frame, center, cv::Point(center.x + (int)(shift.x*5), center.y + (int)(shift.y*5)), cv::Scalar(0, 255, 0), 3, CV_AA);
 #endif
 
         //
diff --git a/opencv_apps/src/nodelet/segment_objects_nodelet.cpp b/opencv_apps/src/nodelet/segment_objects_nodelet.cpp
index ad362cd..6c1012b 100644
--- a/opencv_apps/src/nodelet/segment_objects_nodelet.cpp
+++ b/opencv_apps/src/nodelet/segment_objects_nodelet.cpp
@@ -79,7 +79,7 @@ class SegmentObjectsNodelet : public nodelet::Nodelet
 #ifndef CV_VERSION_EPOCH
   cv::Ptr<cv::BackgroundSubtractorMOG2> bgsubtractor;
 #else
-  cv::BackgroundSubtractorMOG2 bgsubtractor;
+  cv::BackgroundSubtractorMOG bgsubtractor;
 #endif
   bool update_bg_model;
 
@@ -208,7 +208,7 @@ class SegmentObjectsNodelet : public nodelet::Nodelet
       }
 
       // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, out_frame).toImageMsg();
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", out_frame).toImageMsg();
       img_pub_.publish(out_img);
       msg_pub_.publish(contours_msg);
       area_pub_.publish(area_msg);
@@ -297,7 +297,7 @@ public:
     image_transport::SubscriberStatusCallback img_disconnect_cb = boost::bind(&SegmentObjectsNodelet::img_disconnectCb, this, _1);
     ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&SegmentObjectsNodelet::msg_connectCb, this, _1);
     ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&SegmentObjectsNodelet::msg_disconnectCb, this, _1);
-    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("segmented", 1, img_connect_cb, img_disconnect_cb);
+    img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
     msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
     area_pub_ = local_nh_.advertise<std_msgs::Float64>("area", 1, msg_connect_cb, msg_disconnect_cb);
     update_bg_model_service_ = local_nh_.advertiseService("update_bg_model", &SegmentObjectsNodelet::update_bg_model_cb, this);
diff --git a/opencv_apps/src/nodelet/simple_flow_nodelet.cpp b/opencv_apps/src/nodelet/simple_flow_nodelet.cpp
index fdef09a..d454b2f 100644
--- a/opencv_apps/src/nodelet/simple_flow_nodelet.cpp
+++ b/opencv_apps/src/nodelet/simple_flow_nodelet.cpp
@@ -45,6 +45,9 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
+#if CV_MAJOR_VERSION == 3
+#include <opencv2/optflow.hpp>
+#endif
 
 #include <dynamic_reconfigure/server.h>
 #include "opencv_apps/SimpleFlowConfig.h"
@@ -113,7 +116,15 @@ class SimpleFlowNodelet : public nodelet::Nodelet
     try
     {
       // Convert the image into something opencv can handle.
-      cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
+      cv::Mat frame_src = cv_bridge::toCvShare(msg, msg->encoding)->image;
+
+      /// Convert it to gray
+      cv::Mat frame;
+      if ( frame_src.channels() > 1 ) {
+        frame = frame_src;
+      } else {
+        cv::cvtColor( frame_src, frame, cv::COLOR_GRAY2BGR );
+      }
 
       cv::resize(frame, gray, cv::Size(frame.cols/(double)MAX(1,scale_), frame.rows/(double)MAX(1,scale_)), 0, 0, CV_INTER_AREA);
       if(prevGray.empty())
@@ -145,7 +156,11 @@ class SimpleFlowNodelet : public nodelet::Nodelet
       }
 
       float start = (float)cv::getTickCount();
+#if CV_MAJOR_VERSION == 3
+      cv::optflow::calcOpticalFlowSF(gray, prevGray,
+#else
       cv::calcOpticalFlowSF(gray, prevGray,
+#endif
                             flow,
                             3, 2, 4, 4.1, 25.5, 18, 55.0, 25.5, 0.35, 18, 55.0, 25.5, 10);
       NODELET_INFO("calcOpticalFlowSF : %lf sec", (cv::getTickCount() - start) / cv::getTickFrequency());
@@ -160,7 +175,7 @@ class SimpleFlowNodelet : public nodelet::Nodelet
       for (int i= 0; i < rows; ++i) {
         for (int j = 0; j < cols; ++j) {
           cv::Vec2f flow_at_point = flow.at<cv::Vec2f>(i, j);
-          cv::line(frame, cv::Point(scale_col*j, scale_row*i), cv::Point(scale_col*(j+flow_at_point[0]), scale_row*(i+flow_at_point[1])), cv::Scalar::all(255), 1, 8, 0 );
+          cv::line(frame, cv::Point(scale_col*j, scale_row*i), cv::Point(scale_col*(j+flow_at_point[0]), scale_row*(i+flow_at_point[1])), cv::Scalar(0,255,0), 1, 8, 0 );
 
           opencv_apps::Flow flow_msg;
           opencv_apps::Point2D point_msg;
@@ -187,7 +202,7 @@ class SimpleFlowNodelet : public nodelet::Nodelet
 
       cv::swap(prevGray, gray);
       // Publish the image.
-      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg();
+      sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", frame).toImageMsg();
       img_pub_.publish(out_img);
       msg_pub_.publish(flows_msg);
     }
diff --git a/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp b/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp
index abab7fd..fc604b5 100644
--- a/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp
+++ b/opencv_apps/src/nodelet/watershed_segmentation_nodelet.cpp
@@ -47,6 +47,10 @@
 
 #include <dynamic_reconfigure/server.h>
 #include "opencv_apps/WatershedSegmentationConfig.h"
+#include "opencv_apps/Contour.h"
+#include "opencv_apps/ContourArray.h"
+#include "opencv_apps/ContourArrayStamped.h"
+#include "opencv_apps/Point2DArray.h"
 
 namespace watershed_segmentation {
 class WatershedSegmentationNodelet : public nodelet::Nodelet
@@ -55,6 +59,7 @@ class WatershedSegmentationNodelet : public nodelet::Nodelet
   image_transport::Subscriber img_sub_;
   image_transport::CameraSubscriber cam_sub_;
   ros::Publisher msg_pub_;
+  ros::Subscriber add_seed_points_sub_;
 
   boost::shared_ptr<image_transport::ImageTransport> it_;
   ros::NodeHandle nh_, local_nh_;
@@ -127,8 +132,8 @@ class WatershedSegmentationNodelet : public nodelet::Nodelet
       cv::Mat frame = cv_bridge::toCvShare(msg, msg->encoding)->image;
 
       // Messages
-      //opencv_apps::LineArrayStamped lines_msg;
-      //lines_msg.header = msg->header;
+      opencv_apps::ContourArrayStamped contours_msg;
+      contours_msg.header = msg->header;
 
       // Do the work
       //std::vector<cv::Rect> faces;
@@ -195,6 +200,16 @@ class WatershedSegmentationNodelet : public nodelet::Nodelet
         NODELET_WARN("compCount is 0");
         return; //continue;
       }
+      for( size_t i = 0; i< contours.size(); i++ ) {
+        opencv_apps::Contour contour_msg;
+        for ( size_t j = 0; j < contours[i].size(); j++ ) {
+          opencv_apps::Point2D point_msg;
+          point_msg.x = contours[i][j].x;
+          point_msg.y = contours[i][j].y;
+          contour_msg.points.push_back(point_msg);
+        }
+        contours_msg.contours.push_back(contour_msg);
+      }
 
       std::vector<cv::Vec3b> colorTab;
       for( i = 0; i < compCount; i++ )
@@ -244,7 +259,7 @@ class WatershedSegmentationNodelet : public nodelet::Nodelet
       // Publish the image.
       sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, wshed).toImageMsg();
       img_pub_.publish(out_img);
-      //msg_pub_.publish(lines_msg);
+      msg_pub_.publish(contours_msg);
     }
     catch (cv::Exception &e)
     {
@@ -254,6 +269,18 @@ class WatershedSegmentationNodelet : public nodelet::Nodelet
     prev_stamp_ = msg->header.stamp;
   }
 
+  void add_seed_point_cb(const opencv_apps::Point2DArray& msg) {
+    if ( msg.points.size() == 0 ) {
+      markerMask = cv::Scalar::all(0);
+    } else {
+      for(size_t i = 0; i < msg.points.size(); i++ ) {
+        cv::Point pt0(msg.points[i].x, msg.points[i].y);
+        cv::Point pt1(pt0.x + 1, pt0.y + 1);
+        cv::line( markerMask, pt0, pt1, cv::Scalar::all(255), 5, 8, 0 );
+      }
+    }
+  }
+
   void subscribe()
   {
     NODELET_DEBUG("Subscribing to image topic.");
@@ -311,7 +338,7 @@ public:
     subscriber_count_ = 0;
     prev_stamp_ = ros::Time(0, 0);
 
-    window_name_ = "input";
+    window_name_ = "roughly mark the areas to segment on the image";
     segment_name_ = "watershed transform";
     prevPt.x = -1;
     prevPt.y = -1;
@@ -321,7 +348,8 @@ public:
     ros::SubscriberStatusCallback msg_connect_cb    = boost::bind(&WatershedSegmentationNodelet::msg_connectCb, this, _1);
     ros::SubscriberStatusCallback msg_disconnect_cb = boost::bind(&WatershedSegmentationNodelet::msg_disconnectCb, this, _1);
     img_pub_ = image_transport::ImageTransport(local_nh_).advertise("image", 1, img_connect_cb, img_disconnect_cb);
-    //msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);
+    msg_pub_ = local_nh_.advertise<opencv_apps::ContourArrayStamped>("contours", 1, msg_connect_cb, msg_disconnect_cb);
+    add_seed_points_sub_ = local_nh_.subscribe("add_seed_points", 1, &WatershedSegmentationNodelet::add_seed_point_cb, this);
         
     if( debug_view_ ) {
       subscriber_count_++;
diff --git a/opencv_apps/test/CMakeLists.txt b/opencv_apps/test/CMakeLists.txt
new file mode 100644
index 0000000..4511a2e
--- /dev/null
+++ b/opencv_apps/test/CMakeLists.txt
@@ -0,0 +1,36 @@
+# Tests simple calibration dataset
+catkin_download_test_data(face_detector_withface_test_diamondback.bag http://download.ros.org/data/face_detector/face_detector_withface_test_diamondback.bag
+  DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}
+  MD5 59126117e049e69d577b7ee27251a6f8
+  )
+catkin_download_test_data(vslam_tutorial.bag http://download.ros.org/data/vslam_system/vslam_tutorial.bag
+  DESTINATION .
+  MD5 f5aece448b7af00a38a993eb71400806
+  )
+add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag
+  DEPENDS vslam_tutorial.bag
+  COMMAND rosbag reindex vslam_tutorial.bag --output-dir ${CMAKE_CURRENT_SOURCE_DIR}
+)
+add_custom_target(vslam_tutorial_bag DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/vslam_tutorial.bag)
+add_dependencies(tests vslam_tutorial_bag)
+
+#add_rostest(test-apps.test)
+add_rostest(test-edge_detection.test ARGS gui:=false)
+add_rostest(test-hough_lines.test ARGS gui:=false)
+add_rostest(test-hough_circles.test ARGS gui:=false)
+add_rostest(test-find_contours.test ARGS gui:=false)
+add_rostest(test-convex_hull.test ARGS gui:=false)
+add_rostest(test-general_contours.test ARGS gui:=false)
+add_rostest(test-contour_moments.test ARGS gui:=false)
+add_rostest(test-face_detection.test ARGS gui:=false)
+add_rostest(test-goodfeature_track.test ARGS gui:=false)
+add_rostest(test-camshift.test ARGS gui:=false)
+add_rostest(test-fback_flow.test ARGS gui:=false)
+add_rostest(test-lk_flow.test ARGS gui:=false)
+add_rostest(test-people_detect.test ARGS gui:=false)
+add_rostest(test-phase_corr.test ARGS gui:=false)
+add_rostest(test-segment_objects.test ARGS gui:=false)
+add_rostest(test-simple_flow.test ARGS gui:=false)
+add_rostest(test-watershed_segmentation.test ARGS gui:=false)
+
+
diff --git a/opencv_apps/test/test-camshift.test b/opencv_apps/test/test-camshift.test
new file mode 100644
index 0000000..be21796
--- /dev/null
+++ b/opencv_apps/test/test-camshift.test
@@ -0,0 +1,32 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- camshift.cpp -->
+    <node name="camshift" pkg="opencv_apps" type="camshift" args="image:=image_rect_color" >
+      <param name="debug_view" value="$(arg gui)" />
+      <rosparam param="histogram">
+        [0.0, 255.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      </rosparam>
+      <param name="vmin" value="120" />
+      <param name="vmax" value="230" />
+      <param name="smin" value="60" />
+    </node>
+    <node name="camshift_saver_result" pkg="image_view" type="image_saver" args="image:=camshift/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/camshift_result.png"/>
+    </node>
+    <node name="camshift_saver_back_project" pkg="image_view" type="image_saver" args="image:=camshift/back_project" >
+      <param name="filename_format" value="$(find opencv_apps)/test/camshift_back_project.png"/>
+    </node>
+    <param name="camshift_test/topic" value="camshift/track_box" />    <!-- opencv_apps/Point2DArrayStamped -->
+    <test test-name="camshift_test" pkg="rostest" type="hztest" name="camshift_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-contour_moments.test b/opencv_apps/test/test-contour_moments.test
new file mode 100644
index 0000000..fcd9d71
--- /dev/null
+++ b/opencv_apps/test/test-contour_moments.test
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- contour_moments.cpp -->
+    <node name="contour_moments" pkg="opencv_apps" type="contour_moments" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="threshold" value="30" />
+    </node>
+    <node name="contour_moments_saver" pkg="image_view" type="image_saver" args="image:=contour_moments/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/contour_moments.png"/>
+    </node>
+    <param name="contour_moments_test/topic" value="contour_moments/moments" />    <!-- opencv_apps/MomentArrayStamped -->
+    <test test-name="contour_moments_test" pkg="rostest" type="hztest" name="contour_moments_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-convex_hull.test b/opencv_apps/test/test-convex_hull.test
new file mode 100644
index 0000000..0bae9b1
--- /dev/null
+++ b/opencv_apps/test/test-convex_hull.test
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- convex_hull.cpp -->
+    <node name="convex_hull" pkg="opencv_apps" type="convex_hull" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="threshold" value="20" />
+    </node>
+    <node name="convex_hull_saver" pkg="image_view" type="image_saver" args="image:=convex_hull/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/convex_hull.png"/>
+    </node>
+    <param name="convex_hull_test/topic" value="convex_hull/hulls" />    <!-- opencv_apps/ContorArrayStamped -->
+    <test test-name="convex_hull_test" pkg="rostest" type="hztest" name="convex_hull_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-edge_detection.test b/opencv_apps/test/test-edge_detection.test
new file mode 100644
index 0000000..0a84388
--- /dev/null
+++ b/opencv_apps/test/test-edge_detection.test
@@ -0,0 +1,54 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- edge_detection.cpp  -->
+    <!-- 0: Sobel -->
+    <node name="edge_sobel_detection" pkg="opencv_apps" type="edge_detection" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="edge_type" value="0" />
+    </node>
+    <node name="edge_sobel_detection_saver" pkg="image_view" type="image_saver" args="image:=edge_sobel_detection/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/edge_sobel_detection.png"/>
+    </node>
+    <param name="edge_sobel_detection_test/topic" value="edge_sobel_detection/image" />    <!-- opencv_apps/FaceArrayStamped -->
+    <test test-name="edge_sobel_detection_test" pkg="rostest" type="hztest" name="edge_sobel_detection_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+    <!-- 1: Laplace -->
+    <node name="edge_laplace_detection" pkg="opencv_apps" type="edge_detection" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="edge_type" value="1" />
+    </node>
+    <node name="edge_laplace_detection_saver" pkg="image_view" type="image_saver" args="image:=edge_laplace_detection/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/edge_laplace_detection.png"/>
+    </node>
+    <param name="edge_laplace_detection_test/topic" value="edge_laplace_detection/image" />    <!-- opencv_apps/FaceArrayStamped -->
+    <test test-name="edge_laplace_detection_test" pkg="rostest" type="hztest" name="edge_laplace_detection_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+    <!-- 2: Canny -->
+    <node name="edge_canny_detection" pkg="opencv_apps" type="edge_detection" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="edge_type" value="2" />
+      <param name="canny_low_threshold" value="40" />
+    </node>
+    <node name="edge_canny_detection_saver" pkg="image_view" type="image_saver" args="image:=edge_canny_detection/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/edge_canny_detection.png"/>
+    </node>
+    <param name="edge_canny_detection_test/topic" value="edge_canny_detection/image" />    <!-- opencv_apps/FaceArrayStamped -->
+    <test test-name="edge_canny_detection_test" pkg="rostest" type="hztest" name="edge_canny_detection_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-face_detection.test b/opencv_apps/test/test-face_detection.test
new file mode 100644
index 0000000..5d84a3f
--- /dev/null
+++ b/opencv_apps/test/test-face_detection.test
@@ -0,0 +1,23 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- face_detection.cpp -->
+    <node name="face_detection" pkg="opencv_apps" type="face_detection" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="face_detection_saver" pkg="image_view" type="image_saver" args="image:=face_detection/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/face_detection.png"/>
+    </node>
+    <param name="face_detection_test/topic" value="face_detection/faces" />    <!-- opencv_apps/FaceArrayStamped -->
+    <test test-name="face_detection_test" pkg="rostest" type="hztest" name="face_detection_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-fback_flow.test b/opencv_apps/test/test-fback_flow.test
new file mode 100644
index 0000000..398c51e
--- /dev/null
+++ b/opencv_apps/test/test-fback_flow.test
@@ -0,0 +1,23 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
+
+  <group ns="narrow_stereo/left" >
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect" if="$(arg gui)" />
+
+    <!-- fback_flow.cpp -->
+    <node name="fback_flow" pkg="opencv_apps" type="fback_flow" args="image:=image_rect" >
+      <param name="use_camera_info" value="false" />
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="fback_flow_saver" pkg="image_view" type="image_saver" args="image:=fback_flow/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/fback_flow.png"/>
+    </node>
+    <param name="fback_flow_test/topic" value="fback_flow/flows" />    <!-- opencv_apps/FlowArrayStamped -->
+    <test test-name="fback_flow_test" pkg="rostest" type="hztest" name="fback_flow_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-find_contours.test b/opencv_apps/test/test-find_contours.test
new file mode 100644
index 0000000..68b4bca
--- /dev/null
+++ b/opencv_apps/test/test-find_contours.test
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- find_contours.cpp -->
+    <node name="find_contours" pkg="opencv_apps" type="find_contours" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="canny_low_threshold" value="100" />
+    </node>
+    <node name="find_contours_saver" pkg="image_view" type="image_saver" args="image:=find_contours/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/find_contours.png"/>
+    </node>
+    <param name="find_contours_test/topic" value="find_contours/contours" />    <!-- opencv_apps/ContorArrayStamped -->
+    <test test-name="find_contours_test" pkg="rostest" type="hztest" name="find_contours_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-general_contours.test b/opencv_apps/test/test-general_contours.test
new file mode 100644
index 0000000..2466f18
--- /dev/null
+++ b/opencv_apps/test/test-general_contours.test
@@ -0,0 +1,30 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- general_contours.cpp -->
+    <node name="general_contours" pkg="opencv_apps" type="general_contours" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="threshold" value="30" />
+    </node>
+    <node name="general_contours_saver" pkg="image_view" type="image_saver" args="image:=general_contours/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/general_contours.png"/>
+    </node>
+    <param name="general_contours_rectangles_test/topic" value="general_contours/rectangles" />    <!-- opencv_apps/RotatedRectArrayStamped -->
+    <test test-name="general_contours_rectangles_test" pkg="rostest" type="hztest" name="general_contours_rectangles_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+    <param name="general_contours_test_ellipses/topic" value="general_contours/ellipses" />    <!-- opencv_apps/RotatedRectArrayStamped -->
+    <test test-name="general_contours_ellipses_test" pkg="rostest" type="hztest" name="general_contours_test_ellipses" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-goodfeature_track.test b/opencv_apps/test/test-goodfeature_track.test
new file mode 100644
index 0000000..14d363d
--- /dev/null
+++ b/opencv_apps/test/test-goodfeature_track.test
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- goodfeature_track.cpp -->
+    <node name="goodfeature_track" pkg="opencv_apps" type="goodfeature_track" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="max_corners" value="50" />
+    </node>
+    <node name="goodfeature_track_saver" pkg="image_view" type="image_saver" args="image:=goodfeature_track/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/goodfeature_track.png"/>
+    </node>
+    <param name="goodfeature_track_test/topic" value="goodfeature_track/corners" />    <!-- opencv_apps/Point2DArrayStamped -->
+    <test test-name="goodfeature_track_test" pkg="rostest" type="hztest" name="goodfeature_track_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-hough_circles.test b/opencv_apps/test/test-hough_circles.test
new file mode 100644
index 0000000..886f800
--- /dev/null
+++ b/opencv_apps/test/test-hough_circles.test
@@ -0,0 +1,25 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- hough_circles.cpp -->
+    <node name="hough_circles" pkg="opencv_apps" type="hough_circles" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="canny_threshold" value="200" />
+      <param name="accumulator_threshold" value="20" />
+    </node>
+    <node name="hough_circles_saver" pkg="image_view" type="image_saver" args="image:=hough_circles/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/hough_circles.png"/>
+    </node>
+    <param name="hough_circles_test/topic" value="hough_circles/circles" />    <!-- opencv_apps/CircleArrayStamped -->
+    <test test-name="hough_circles_test" pkg="rostest" type="hztest" name="hough_circles_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-hough_lines.test b/opencv_apps/test/test-hough_lines.test
new file mode 100644
index 0000000..f5fc8b9
--- /dev/null
+++ b/opencv_apps/test/test-hough_lines.test
@@ -0,0 +1,42 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- hough_lines.cpp -->
+    <!-- 0: Standard Hough Line -->
+    <node name="standard_hough_lines" pkg="opencv_apps" type="hough_lines" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="hough_type" value="0" />
+      <param name="threshold" value="100" />
+    </node>
+    <node name="standard_hough_lines_saver" pkg="image_view" type="image_saver" args="image:=standard_hough_lines/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/standard_hough_lines.png"/>
+    </node>
+    <param name="standard_hough_lines_test/topic" value="standard_hough_lines/lines" />    <!-- opencv_apps/LineArrayStamped -->
+    <test test-name="standard_hough_lines_test" pkg="rostest" type="hztest" name="standard_hough_lines_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+
+    <!-- 1: Probabilistic Hough Line -->
+    <node name="probabilistic_hough_lines" pkg="opencv_apps" type="hough_lines" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />e
+      <param name="hough_type" value="1" />
+      <param name="threshold" value="100" />
+    </node>
+    <node name="probabilistic_hough_lines_saver" pkg="image_view" type="image_saver" args="image:=probabilistic_hough_lines/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/probabilistic_hough_lines.png"/>
+    </node>
+    <param name="probabilistic_hough_lines_test/topic" value="probabilistic_hough_lines/lines" />    <!-- opencv_apps/LineArrayStamped -->
+    <test test-name="probabilistic_hough_lines_test" pkg="rostest" type="hztest" name="probabilistic_hough_lines_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-lk_flow.test b/opencv_apps/test/test-lk_flow.test
new file mode 100644
index 0000000..d0f64b7
--- /dev/null
+++ b/opencv_apps/test/test-lk_flow.test
@@ -0,0 +1,25 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
+
+  <group ns="narrow_stereo/left" >
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_throttle" if="$(arg gui)" />
+    <node name="image_throttle" pkg="topic_tools" type="throttle" args="messages image_rect 5" />
+    <!-- lk_flow.cpp -->
+    <node name="lk_flow" pkg="opencv_apps" type="lk_flow" args="image:=image_rect_throttle" >
+      <param name="use_camera_info" value="false" />
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="lk_flow_saver" pkg="image_view" type="image_saver" args="image:=lk_flow/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/lk_flow.png"/>
+    </node>
+    <node name="lk_flow_initialize" pkg="rosservice" type="rosservice" args="call --wait lk_flow/initialize_points"
+          launch-prefix="watch" />
+    <param name="lk_flow_test/topic" value="lk_flow/flows" />    <!-- opencv_apps/FlowArrayStamped -->
+    <test test-name="lk_flow_test" pkg="rostest" type="hztest" name="lk_flow_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-people_detect.test b/opencv_apps/test/test-people_detect.test
new file mode 100644
index 0000000..b8ad3a5
--- /dev/null
+++ b/opencv_apps/test/test-people_detect.test
@@ -0,0 +1,23 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- people_detect.cpp -->
+    <node name="people_detect" pkg="opencv_apps" type="people_detect" args="image:=image_rect" >
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="people_detect_saver" pkg="image_view" type="image_saver" args="image:=people_detect/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/people_detect.png"/>
+    </node>
+    <param name="people_detect_test/topic" value="people_detect/found" />    <!-- opencv_apps/RectArrayStamped -->
+    <test test-name="people_detect_test" pkg="rostest" type="hztest" name="people_detect_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-phase_corr.test b/opencv_apps/test/test-phase_corr.test
new file mode 100644
index 0000000..83c1c8d
--- /dev/null
+++ b/opencv_apps/test/test-phase_corr.test
@@ -0,0 +1,23 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
+
+  <group ns="narrow_stereo/left" >
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect" if="$(arg gui)" />
+
+    <!-- phase_corr.cpp -->
+    <node name="phase_corr" pkg="opencv_apps" type="phase_corr" args="image:=image_rect" >
+      <param name="use_camera_info" value="false" />
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="phase_corr_saver" pkg="image_view" type="image_saver" args="image:=phase_corr/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/phase_corr.png"/>
+    </node>
+    <param name="phase_corr_test/topic" value="phase_corr/shift" />    <!-- opencv_apps/Point2DStamped -->
+    <test test-name="phase_corr_test" pkg="rostest" type="hztest" name="phase_corr_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-segment_objects.test b/opencv_apps/test/test-segment_objects.test
new file mode 100644
index 0000000..1c61059
--- /dev/null
+++ b/opencv_apps/test/test-segment_objects.test
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- segment_objects.cpp -->
+    <node name="segment_objects" pkg="opencv_apps" type="segment_objects" args="image:=image_rect" >
+      <param name="use_camera_info" value="false" />
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="segment_objects_saver" pkg="image_view" type="image_saver" args="image:=segment_objects/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/segment_objects.png"/>
+    </node>
+    <param name="segment_objects_test/topic" value="segment_objects/contours" />    <!-- opencv_apps/ContoruArrayStamped -->
+    <test test-name="segment_objects_test" pkg="rostest" type="hztest" name="segment_objects_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-simple_flow.test b/opencv_apps/test/test-simple_flow.test
new file mode 100644
index 0000000..25b05ee
--- /dev/null
+++ b/opencv_apps/test/test-simple_flow.test
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_vslam_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/vslam_tutorial.bag -s 5 --topics /narrow_stereo/left/image_rect" />
+
+  <group ns="narrow_stereo/left" >
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect" if="$(arg gui)" />
+
+    <!-- simple_flow.cpp -->
+    <node name="simple_flow" pkg="opencv_apps" type="simple_flow" args="image:=image_rect" >
+      <param name="use_camera_info" value="false" />
+      <param name="debug_view" value="$(arg gui)" />
+      <param name="scale" value="8" />
+    </node>
+    <node name="simple_flow_saver" pkg="image_view" type="image_saver" args="image:=simple_flow/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/simple_flow.png"/>
+    </node>
+    <param name="simple_flow_test/topic" value="simple_flow/flows" />    <!-- opencv_apps/FlowArrayStamped -->
+    <test test-name="simple_flow_test" pkg="rostest" type="hztest" name="simple_flow_test" >
+      <param name="hz" value="15" />
+      <param name="hzerror" value="14" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_apps/test/test-watershed_segmentation.test b/opencv_apps/test/test-watershed_segmentation.test
new file mode 100644
index 0000000..0d42635
--- /dev/null
+++ b/opencv_apps/test/test-watershed_segmentation.test
@@ -0,0 +1,26 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- watershed_segmentation -->
+    <node name="watershed_segmentation" pkg="opencv_apps" type="watershed_segmentation" args="image:=image_rect_color" output="screen">
+      <param name="use_camera_info" value="false" />
+      <param name="debug_view" value="$(arg gui)" />
+    </node>
+    <node name="pub_add_seed_points" pkg="rostopic" type="rostopic"
+            args="pub -r 1 watershed_segmentation/add_seed_points opencv_apps/Point2DArray '[{x: 100, y: 100},{x: 200, y: 100},{x: 300, y: 100},{x: 400, y: 100},{x: 500, y: 100},{x: 100, y: 200},{x: 200, y: 200},{x: 300, y: 200},{x: 400, y: 200},{x: 500, y: 200},{x: 100, y: 300},{x: 200, y: 300},{x: 300, y: 300},{x: 400, y: 300},{x: 500, y: 300}]' --" />
+    <node name="watershed_segmentation_saver" pkg="image_view" type="image_saver" args="image:=watershed_segmentation/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/watershed_segmentation.png"/>
+    </node>
+    <param name="watershed_segmentation_test/topic" value="watershed_segmentation/contours" />    <!-- opencv_apps/ContourStamped -->
+    <test test-name="watershed_segmentation_test" pkg="rostest" type="hztest" name="watershed_segmentation_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>
diff --git a/opencv_tests/CHANGELOG.rst b/opencv_tests/CHANGELOG.rst
index df63818..9441efb 100644
--- a/opencv_tests/CHANGELOG.rst
+++ b/opencv_tests/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package opencv_tests
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.11.10 (2016-01-16)
+--------------------
+
 1.11.9 (2015-11-29)
 -------------------
 
diff --git a/opencv_tests/package.xml b/opencv_tests/package.xml
index 4874f16..3b7db31 100644
--- a/opencv_tests/package.xml
+++ b/opencv_tests/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>opencv_tests</name>
-  <version>1.11.9</version>
+  <version>1.11.10</version>
   <description>
      opencv_tests
   </description>
diff --git a/vision_opencv/CHANGELOG.rst b/vision_opencv/CHANGELOG.rst
index 512d58b..de069c4 100644
--- a/vision_opencv/CHANGELOG.rst
+++ b/vision_opencv/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package vision_opencv
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.11.10 (2016-01-16)
+--------------------
+
 1.11.9 (2015-11-29)
 -------------------
 * Add opencv_apps to vision_opencv dependency
diff --git a/vision_opencv/package.xml b/vision_opencv/package.xml
index b4c4515..ff5618d 100644
--- a/vision_opencv/package.xml
+++ b/vision_opencv/package.xml
@@ -1,6 +1,6 @@
 <package>
   <name>vision_opencv</name>
-  <version>1.11.9</version>
+  <version>1.11.10</version>
   <description>Packages for interfacing ROS with OpenCV, a library of programming functions for real time computer vision.</description>
   <author>Patrick Mihelich</author>
   <author>James Bowman</author>

-- 
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/ros/ros-vision-opencv.git



More information about the debian-science-commits mailing list