diff --git a/grid_map/package.xml b/grid_map/package.xml
index b1acbfc4a..8e7575e91 100644
--- a/grid_map/package.xml
+++ b/grid_map/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <exec_depend>grid_map_core</exec_depend>
   <exec_depend>grid_map_ros</exec_depend>
+  <exec_depend>grid_map_cv</exec_depend>
   <exec_depend>grid_map_msgs</exec_depend>
   <exec_depend>grid_map_filters</exec_depend>
   <exec_depend>grid_map_visualization</exec_depend>
diff --git a/grid_map_core/CMakeLists.txt b/grid_map_core/CMakeLists.txt
index c76c790f6..689abbea8 100644
--- a/grid_map_core/CMakeLists.txt
+++ b/grid_map_core/CMakeLists.txt
@@ -111,4 +111,3 @@ catkin_add_gtest(${PROJECT_NAME}-test
 if(TARGET ${PROJECT_NAME}-test)
   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
 endif()
-
diff --git a/grid_map_cv/CHANGELOG.rst b/grid_map_cv/CHANGELOG.rst
new file mode 100644
index 000000000..c97196c7f
--- /dev/null
+++ b/grid_map_cv/CHANGELOG.rst
@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package grid_map_cv
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+* Created package.
diff --git a/grid_map_cv/CMakeLists.txt b/grid_map_cv/CMakeLists.txt
new file mode 100644
index 000000000..b643cf09c
--- /dev/null
+++ b/grid_map_cv/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(grid_map_cv)
+
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  grid_map_core
+)
+
+find_package(OpenCV REQUIRED)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS
+    include
+  LIBRARIES
+  CATKIN_DEPENDS
+    grid_map_core
+  DEPENDS
+    OpenCV
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+#############
+## Install ##
+#############
+
+# Mark cpp header files for installation
+install(
+  DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  FILES_MATCHING PATTERN "*.hpp"
+)
+
+#############
+## Testing ##
+#############
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
+## Add gtest based cpp test target and link libraries
+catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_cv.cpp test/GridMapCvTest.cpp)
diff --git a/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
new file mode 100644
index 000000000..ae6d65fa4
--- /dev/null
+++ b/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp
@@ -0,0 +1,157 @@
+/*
+ * GridMapCvConverter.hpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+#pragma once
+
+#include <grid_map_core/grid_map_core.hpp>
+
+// OpenCV
+#include <opencv/cv.h>
+
+namespace grid_map {
+
+/*!
+ * Conversions between grid maps and OpenCV images.
+ */
+class GridMapCvConverter
+{
+ public:
+  /*!
+   * Default constructor.
+   */
+  GridMapCvConverter();
+
+  /*!
+   * Destructor.
+   */
+  virtual ~GridMapCvConverter();
+
+  /*!
+   * Adds a layer with data from image.
+   * @param[in] image the image to be added. If it is a color image
+   * (bgr or bgra encoding), it will be transformed in a grayscale image.
+   * @param[in] layer the layer that is filled with the image data.
+   * @param[out] gridMap the grid map to be populated.
+   * @param[in](optional) lowerValue value of the layer corresponding to black image pixels.
+   * @param[in](optional) upperValue value of the layer corresponding to white image pixels.
+   * @return true if successful, false otherwise.
+   */
+  template<typename Type_, int NChannels_>
+  static bool addLayerFromImage(const cv::Mat& image, const std::string& layer,
+                                grid_map::GridMap& gridMap, const float lowerValue,
+                                const float upperValue, const double alphaThreshold = 0.5)
+  {
+    if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
+      std::cerr << "Image size does not correspond to grid map size!" << std::endl;
+      return false;
+    }
+
+    bool isColor = false;
+    if (image.channels() >= 3) isColor = true;
+    bool hasAlpha = false;
+    if (image.channels() >= 4) hasAlpha = true;
+
+    cv::Mat imageMono;
+    if (isColor && !hasAlpha) {
+      cv::cvtColor(image, imageMono, CV_BGR2GRAY);
+    } else if (isColor && hasAlpha) {
+      cv::cvtColor(image, imageMono, CV_BGRA2GRAY);
+    } else if (!isColor && !hasAlpha){
+      imageMono = image;
+    } else {
+      std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl;
+      return false;
+    }
+
+    const float mapValueDifference = upperValue - lowerValue;
+    const float maxImageValue = (float)std::numeric_limits<Type_>::max();
+    const Type_ alphaTreshold = (Type_)(alphaThreshold * std::numeric_limits<Type_>::max());
+
+    gridMap.add(layer);
+    grid_map::Matrix& data = gridMap[layer];
+
+    for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
+      const Index index(*iterator);
+
+      // Check for alpha layer.
+      if (hasAlpha) {
+        const Type_ alpha = image.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
+        if (alpha < alphaTreshold) continue;
+      }
+
+      // Compute value.
+      const Type_ imageValue = imageMono.at<Type_>(index(0), index(1));
+      const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
+      data(index(0), index(1)) = mapValue;
+    }
+
+    return true;
+  };
+
+  /*!
+   * Creates a cv mat from a grid map layer.
+   * @param[in] grid map to be added.
+   * @param[in] layer the layer that is converted to the image.
+   * @param[in] encoding the desired encoding of the image.
+   * @param[in] lowerValue the value of the layer corresponding to black image pixels.
+   * @param[in] upperValue the value of the layer corresponding to white image pixels.
+   * @param[out] image the image to be populated.
+   * @return true if successful, false otherwise.
+   */
+  template<typename Type_, int NChannels_>
+  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
+                      const int encoding, const float lowerValue, const float upperValue,
+                      cv::Mat& image)
+  {
+    // Initialize image.
+    if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
+      image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding);
+    } else {
+      std::cerr << "Invalid grid map?" << std::endl;
+      return false;
+    }
+
+    // Get max image value.
+    unsigned int imageMax = (unsigned int)std::numeric_limits<Type_>::max();
+
+    // Clamp outliers.
+    grid_map::GridMap map = gridMap;
+    map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(lowerValue, upperValue));
+    const grid_map::Matrix& data = gridMap[layer];
+
+    // Convert to image.
+    bool isColor = false;
+    if (image.channels() >= 3) isColor = true;
+    bool hasAlpha = false;
+    if (image.channels() >= 4) hasAlpha = true;
+
+    for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
+      const Index index(*iterator);
+      if (std::isfinite(data(index(0), index(1)))) {
+        const float& value = data(index(0), index(1));
+        const Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax);
+        const Index imageIndex(iterator.getUnwrappedIndex());
+        unsigned int channel = 0;
+        image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;
+
+        if (isColor) {
+          image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
+          image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
+        }
+        if (hasAlpha) {
+          image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageMax;
+        }
+      }
+    }
+
+    return true;
+  };
+
+};
+
+} /* namespace */
diff --git a/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp b/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp
new file mode 100644
index 000000000..db22ab82c
--- /dev/null
+++ b/grid_map_cv/include/grid_map_cv/grid_map_cv.hpp
@@ -0,0 +1,12 @@
+/*
+ * grid_map_cv.hpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+#pragma once
+
+#include <grid_map_core/grid_map_core.hpp>
+#include <grid_map_cv/GridMapCvConverter.hpp>
diff --git a/grid_map_cv/package.xml b/grid_map_cv/package.xml
new file mode 100644
index 000000000..c57cf2401
--- /dev/null
+++ b/grid_map_cv/package.xml
@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>grid_map_cv</name>
+  <version>1.2.0</version>
+  <description>Conversions between grid maps and OpenCV images.</description>
+  <maintainer email="pfankhauser@ethz.ch">Péter Fankhauser</maintainer>
+  <license>BSD</license>
+  <url type="website">http://github.com/ethz-asl/grid_map</url>
+  <url type="bugtracker">http://github.com/ethz-asl/grid_map/issues</url>
+  <author email="pfankhauser@ethz.ch">Péter Fankhauser</author>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>grid_map_core</depend>
+  <depend>opencv3</depend>
+</package>
diff --git a/grid_map_cv/test/GridMapCvTest.cpp b/grid_map_cv/test/GridMapCvTest.cpp
new file mode 100644
index 000000000..56daf6d2e
--- /dev/null
+++ b/grid_map_cv/test/GridMapCvTest.cpp
@@ -0,0 +1,15 @@
+/*
+ * GridMapCvTest.cpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Peter Fankhauser, Martin Wermelinger
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+#include "grid_map_cv/grid_map_cv.hpp"
+
+// gtest
+#include <gtest/gtest.h>
+
+using namespace std;
+using namespace grid_map;
diff --git a/grid_map_cv/test/test_grid_map_cv.cpp b/grid_map_cv/test/test_grid_map_cv.cpp
new file mode 100644
index 000000000..e773f4d2c
--- /dev/null
+++ b/grid_map_cv/test/test_grid_map_cv.cpp
@@ -0,0 +1,18 @@
+/*
+ * test_grid_map_cv.cpp
+ *
+ *  Created on: Apr 14, 2016
+ *      Author: Péter Fankhauser
+ *	 Institute: ETH Zurich, Autonomous Systems Lab
+ */
+
+// gtest
+#include <gtest/gtest.h>
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  srand((int)time(0));
+  return RUN_ALL_TESTS();
+}
diff --git a/grid_map_demos/CMakeLists.txt b/grid_map_demos/CMakeLists.txt
index 99eaa8d46..52894f45e 100644
--- a/grid_map_demos/CMakeLists.txt
+++ b/grid_map_demos/CMakeLists.txt
@@ -7,6 +7,7 @@ set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   grid_map_ros
+  grid_map_cv
   grid_map_msgs
   geometry_msgs
   sensor_msgs
diff --git a/grid_map_demos/package.xml b/grid_map_demos/package.xml
index e7c18c50d..b21b8cde7 100644
--- a/grid_map_demos/package.xml
+++ b/grid_map_demos/package.xml
@@ -11,6 +11,7 @@
   <buildtool_depend>catkin</buildtool_depend>
   <depend>roscpp</depend>
   <depend>grid_map_ros</depend>
+  <depend>grid_map_cv</depend>
   <depend>grid_map_loader</depend>
   <depend>grid_map_msgs</depend>
   <depend>grid_map_visualization</depend>
diff --git a/grid_map_demos/src/opencv_demo_node.cpp b/grid_map_demos/src/opencv_demo_node.cpp
index 5e00f9aba..f36f25b54 100644
--- a/grid_map_demos/src/opencv_demo_node.cpp
+++ b/grid_map_demos/src/opencv_demo_node.cpp
@@ -1,6 +1,7 @@
 #include <ros/ros.h>
 #include <grid_map_ros/grid_map_ros.hpp>
-#include <cv_bridge/cv_bridge.h>
+#include <grid_map_cv/grid_map_cv.hpp>
+#include <opencv/cv.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv/highgui.h>
 
@@ -11,9 +12,9 @@ int main(int argc, char** argv)
 {
   // Initialize node and publisher.
   init(argc, argv, "opencv_demo");
-  NodeHandle nh("~");
-  Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
-  const bool useTransparency = false;
+  NodeHandle nodeHandle("~");
+  Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
+  const bool useTransparency = true;
 
   // Create grid map.
   GridMap map({"original", "elevation"});
@@ -44,42 +45,40 @@ int main(int argc, char** argv)
   }
 
   // Convert to CV image.
-  cv_bridge::CvImage originalImage;
-  std::string encoding;
+  cv::Mat originalImage;
   if (useTransparency) {
-    encoding = sensor_msgs::image_encodings::RGBA16;
+    // Note: The template parameters have to be set based on your encoding
+    // of the image. For 8-bit images use `unsigned char`.
+    GridMapCvConverter::toImage<unsigned short, 4>(map, "original", CV_16UC4, 0.0, 0.3, originalImage);
   } else {
-    encoding = sensor_msgs::image_encodings::MONO16;
+    GridMapCvConverter::toImage<unsigned short, 1>(map, "original", CV_16UC1, 0.0, 0.3, originalImage);
   }
-  GridMapRosConverter::toCvImage(map, "original", encoding, 0.0, 0.3, originalImage);
 
   // Create OpenCV window.
   cv::namedWindow("OpenCV Demo");
 
   // Work with copy of image in a loop.
-  while (nh.ok()) {
+  while (nodeHandle.ok()) {
 
     // Initialize.
     ros::Time time = ros::Time::now();
     map.setTimestamp(time.toNSec());
-    cv_bridge::CvImage modifiedImage(originalImage.header, originalImage.encoding);
+    cv::Mat modifiedImage;
     int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec())));
     blurRadius = blurRadius - (blurRadius % 2) + 1;
 
     // Apply Gaussian blur.
-    cv::GaussianBlur(originalImage.image, modifiedImage.image, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
+    cv::GaussianBlur(originalImage, modifiedImage, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
 
     // Visualize as image.
-    cv::imshow("OpenCV Demo", modifiedImage.image);
+    cv::imshow("OpenCV Demo", modifiedImage);
     cv::waitKey(40);
 
     // Convert resulting image to a grid map.
-    // Note: The template parameters have to be set based on your encoding
-    // of the image. For MONO8 encoding use `unsigned char`.
     if (useTransparency) {
-      GridMapRosConverter::addLayerFromImage<unsigned short, 4>(modifiedImage.image, "elevation", map, 0.0, 0.3, 0.3);
+      GridMapCvConverter::addLayerFromImage<unsigned short, 4>(modifiedImage, "elevation", map, 0.0, 0.3, 0.3);
     } else {
-      GridMapRosConverter::addLayerFromImage<unsigned short, 1>(modifiedImage.image, "elevation", map, 0.0, 0.3);
+      GridMapCvConverter::addLayerFromImage<unsigned short, 1>(modifiedImage, "elevation", map, 0.0, 0.3);
     }
 
     // Publish grid map.
diff --git a/grid_map_ros/CMakeLists.txt b/grid_map_ros/CMakeLists.txt
index d701b7dc4..eef0f9f14 100644
--- a/grid_map_ros/CMakeLists.txt
+++ b/grid_map_ros/CMakeLists.txt
@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   grid_map_core
   grid_map_msgs
+  grid_map_cv
   sensor_msgs
   nav_msgs
   std_msgs
@@ -99,7 +100,7 @@ install(
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
 ## Add gtest based cpp test target and link libraries
-catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_ros.cpp test/GridMapTest.cpp)
+catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_ros.cpp test/GridMapRosTest.cpp)
 if(TARGET ${PROJECT_NAME}-test)
   target_link_libraries(${PROJECT_NAME}-test grid_map_ros)
 endif()
diff --git a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp
index 78eb8e80a..f40ec6243 100644
--- a/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp
+++ b/grid_map_ros/include/grid_map_ros/GridMapRosConverter.hpp
@@ -156,58 +156,6 @@ class GridMapRosConverter
                                 grid_map::GridMap& gridMap, const float lowerValue = 0.0,
                                 const float upperValue = 1.0);
 
-  template<typename Type_, int NChannels_>
-  static bool addLayerFromImage(const cv::Mat& cvMat, const std::string& layer,
-                                grid_map::GridMap& gridMap, const float lowerValue,
-                                const float upperValue, const double alphaThreshold = 0.5)
-  {
-    if (gridMap.getSize()(0) != cvMat.rows || gridMap.getSize()(1) != cvMat.cols) {
-      std::cerr << "Image size does not correspond to grid map size!" << std::endl;
-      return false;
-    }
-
-    bool isColor = false;
-    if (cvMat.channels() >= 3) isColor = true;
-    bool hasAlpha = false;
-    if (cvMat.channels() >= 4) hasAlpha = true;
-
-    cv::Mat cvMatMono;
-    if (isColor && !hasAlpha) {
-      cv::cvtColor(cvMat, cvMatMono, CV_BGR2GRAY);
-    } else if (isColor && hasAlpha) {
-      cv::cvtColor(cvMat, cvMatMono, CV_BGRA2GRAY);
-    } else if (!isColor && !hasAlpha){
-      cvMatMono = cvMat;
-    } else {
-      std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl;
-      return false;
-    }
-
-    const float mapValueDifference = upperValue - lowerValue;
-    const float maxImageValue = (float)std::numeric_limits<Type_>::max();
-    const Type_ alphaTreshold = (Type_)(alphaThreshold * std::numeric_limits<Type_>::max());
-
-    gridMap.add(layer);
-    grid_map::Matrix& data = gridMap[layer];
-
-    for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
-      const Index index(*iterator);
-
-      // Check for alpha layer.
-      if (hasAlpha) {
-        const Type_ alpha = cvMat.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
-        if (alpha < alphaTreshold) continue;
-      }
-
-      // Compute value.
-      const Type_ imageValue = cvMatMono.at<Type_>(index(0), index(1));
-      const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
-      data(index(0), index(1)) = mapValue;
-    }
-
-    return true;
-  };
-
   /*!
    * Adds a color layer with data from an image.
    * @param[in] image the image to be added.
@@ -218,6 +166,33 @@ class GridMapRosConverter
   static bool addColorLayerFromImage(const sensor_msgs::Image& image, const std::string& layer,
                                      grid_map::GridMap& gridMap);
 
+  /*!
+   * Creates an image message from a grid map layer.
+   * This conversion sets the corresponding black and white pixel value to the
+   * min. and max. data of the layer data.
+   * @param[in] grid map to be added.
+   * @param[in] layer the layer that is converted to the image.
+   * @param[in] encoding the desired encoding of the image.
+   * @param[out] image the message to be populated.
+   * @return true if successful, false otherwise.
+   */
+  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
+                      const std::string encoding, sensor_msgs::Image& image);
+
+  /*!
+   * Creates an image message from a grid map layer.
+   * @param[in] grid map to be added.
+   * @param[in] layer the layer that is converted to the image.
+   * @param[in] encoding the desired encoding of the image.
+   * @param[in] lowerValue the value of the layer corresponding to black image pixels.
+   * @param[in] upperValue the value of the layer corresponding to white image pixels.
+   * @param[out] image the message to be populated.
+   * @return true if successful, false otherwise.
+   */
+  static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
+                      const std::string encoding, const float lowerValue, const float upperValue,
+                      sensor_msgs::Image& image);
+
   /*!
    * Creates a cv image from a grid map layer.
    * This conversion sets the corresponding black and white pixel value to the
@@ -225,7 +200,7 @@ class GridMapRosConverter
    * @param[in] grid map to be added.
    * @param[in] layer the layer that is converted to the image.
    * @param[in] encoding the desired encoding of the image.
-   * @param[out] cv image to be populated.
+   * @param[out] cvImage the to be populated.
    * @return true if successful, false otherwise.
    */
   static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
@@ -236,74 +211,15 @@ class GridMapRosConverter
  * @param[in] grid map to be added.
  * @param[in] layer the layer that is converted to the image.
  * @param[in] encoding the desired encoding of the image.
- * @param[in] lowerValue value of the layer corresponding to black image pixels.
- * @param[in] upperValue value of the layer corresponding to white image pixels.
- * @param[out] cv image to be populated.
+ * @param[in] lowerValue the value of the layer corresponding to black image pixels.
+ * @param[in] upperValue the value of the layer corresponding to white image pixels.
+ * @param[out] cvImage to be populated.
  * @return true if successful, false otherwise.
  */
   static bool toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
                         const std::string encoding, const float lowerValue,
                         const float upperValue, cv_bridge::CvImage& cvImage);
 
-  /*!
-   * Creates a cv mat from a grid map layer.
-   * @param[in] grid map to be added.
-   * @param[in] layer the layer that is converted to the image.
-   * @param[in] encoding the desired encoding of the image.
-   * @param[in] lowerValue value of the layer corresponding to black image pixels.
-   * @param[in] upperValue value of the layer corresponding to white image pixels.
-   * @param[out] cv mat to be populated.
-   * @return true if successful, false otherwise.
-   */
-  template<typename Type_, int NChannels_>
-  static bool toCvMat(const grid_map::GridMap& gridMap, const std::string& layer,
-                      const int encoding, const float lowerValue, const float upperValue,
-                      cv::Mat& cvMat)
-  {
-    // Initialize image.
-    if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
-      cvMat = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding);
-    } else {
-      ROS_ERROR("Invalid grid map?");
-      return false;
-    }
-
-    // Get max image value.
-    unsigned int imageMax = (unsigned int)std::numeric_limits<Type_>::max();
-
-    // Clamp outliers.
-    grid_map::GridMap map = gridMap;
-    map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(lowerValue, upperValue));
-    const grid_map::Matrix& data = gridMap[layer];
-
-    // Convert to image.
-    bool isColor = false;
-    if (cvMat.channels() >= 3) isColor = true;
-    bool hasAlpha = false;
-    if (cvMat.channels() >= 4) hasAlpha = true;
-
-    for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
-      const Index index(*iterator);
-      if (std::isfinite(data(index(0), index(1)))) {
-        const float& value = data(index(0), index(1));
-        const Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax);
-        const Index imageIndex(iterator.getUnwrappedIndex());
-        unsigned int channel = 0;
-        cvMat.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;
-
-        if (isColor) {
-          cvMat.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
-          cvMat.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
-        }
-        if (hasAlpha) {
-          cvMat.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageMax;
-        }
-      }
-    }
-
-    return true;
-  };
-
   /*!
    * Saves a grid map into a ROS bag. The timestamp of the grid map
    * is used as time for storing the message in the ROS bag. The time
diff --git a/grid_map_ros/package.xml b/grid_map_ros/package.xml
index eeac50fa8..9130c7922 100644
--- a/grid_map_ros/package.xml
+++ b/grid_map_ros/package.xml
@@ -12,6 +12,7 @@
   <depend>roscpp</depend>
   <depend>grid_map_core</depend>
   <depend>grid_map_msgs</depend>
+  <depend>grid_map_cv</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>std_msgs</depend>
diff --git a/grid_map_ros/src/GridMapRosConverter.cpp b/grid_map_ros/src/GridMapRosConverter.cpp
index d930e79f6..f1e2e8a72 100644
--- a/grid_map_ros/src/GridMapRosConverter.cpp
+++ b/grid_map_ros/src/GridMapRosConverter.cpp
@@ -8,6 +8,7 @@
 
 #include "grid_map_ros/GridMapRosConverter.hpp"
 #include "grid_map_ros/GridMapMsgHelpers.hpp"
+#include <grid_map_cv/grid_map_cv.hpp>
 
 // ROS
 #include <sensor_msgs/point_cloud2_iterator.h>
@@ -335,17 +336,17 @@ bool GridMapRosConverter::addLayerFromImage(const sensor_msgs::Image& image,
   const int cvEncoding = cv_bridge::getCvType(image.encoding);
   switch (cvEncoding) {
     case CV_8UC1:
-      return addLayerFromImage<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned char, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue);
     case CV_8UC3:
-      return addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned char, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue);
     case CV_8UC4:
-      return addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned char, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue);
     case CV_16UC1:
-      return addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned short, 1>(cvImage->image, layer, gridMap, lowerValue, upperValue);
     case CV_16UC3:
-      return addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned short, 3>(cvImage->image, layer, gridMap, lowerValue, upperValue);
     case CV_16UC4:
-      return addLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue);
+      return GridMapCvConverter::addLayerFromImage<unsigned short, 4>(cvImage->image, layer, gridMap, lowerValue, upperValue);
     default:
       ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
       return false;
@@ -384,6 +385,25 @@ bool GridMapRosConverter::addColorLayerFromImage(const sensor_msgs::Image& image
   return true;
 }
 
+bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer,
+                                  const std::string encoding, sensor_msgs::Image& image)
+{
+  cv_bridge::CvImage cvImage;
+  if (!toCvImage(gridMap, layer, encoding, cvImage)) return false;
+  cvImage.toImageMsg(image);
+  return true;
+}
+
+bool GridMapRosConverter::toImage(const grid_map::GridMap& gridMap, const std::string& layer,
+                                  const std::string encoding, const float lowerValue,
+                                  const float upperValue, sensor_msgs::Image& image)
+{
+  cv_bridge::CvImage cvImage;
+  if (!toCvImage(gridMap, layer, encoding, lowerValue, upperValue, cvImage)) return false;
+  cvImage.toImageMsg(image);
+  return true;
+}
+
 bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
                                     const std::string encoding, cv_bridge::CvImage& cvImage)
 {
@@ -403,17 +423,17 @@ bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std:
   const int cvEncoding = cv_bridge::getCvType(encoding);
   switch (cvEncoding) {
     case CV_8UC1:
-      return toCvMat<unsigned char, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
+      return GridMapCvConverter::toImage<unsigned char, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
     case CV_8UC3:
-      return toCvMat<unsigned char, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
+      return GridMapCvConverter::toImage<unsigned char, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
     case CV_8UC4:
-      return toCvMat<unsigned char, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
+      return GridMapCvConverter::toImage<unsigned char, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
     case CV_16UC1:
-      return toCvMat<unsigned short, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
+      return GridMapCvConverter::toImage<unsigned short, 1>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
     case CV_16UC3:
-      return toCvMat<unsigned short, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
+      return GridMapCvConverter::toImage<unsigned short, 3>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
     case CV_16UC4:
-      return toCvMat<unsigned short, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
+      return GridMapCvConverter::toImage<unsigned short, 4>(gridMap, layer, cvEncoding, lowerValue, upperValue, cvImage.image);
     default:
       ROS_ERROR("Expected MONO8, MONO16, RGB(A)8, RGB(A)16, BGR(A)8, or BGR(A)16 image encoding.");
       return false;
diff --git a/grid_map_ros/test/GridMapTest.cpp b/grid_map_ros/test/GridMapRosTest.cpp
similarity index 88%
rename from grid_map_ros/test/GridMapTest.cpp
rename to grid_map_ros/test/GridMapRosTest.cpp
index 726a2383e..e7aa1bb98 100644
--- a/grid_map_ros/test/GridMapTest.cpp
+++ b/grid_map_ros/test/GridMapRosTest.cpp
@@ -1,5 +1,5 @@
 /*
- * GridMapTest.cpp
+ * GridMapRosTest.cpp
  *
  *  Created on: Mar 24, 2015
  *      Author: Peter Fankhauser, Martin Wermelinger
@@ -192,17 +192,15 @@ TEST(ImageConversion, roundTripBGRA8)
   const float minValue = -1.0;
   const float maxValue = 1.0;
 
-  // Convert to CV image.
-  cv_bridge::CvImage cvImage;
-  GridMapRosConverter::toCvImage(gridMapInput, "layer", sensor_msgs::image_encodings::BGRA8,
-                                 minValue, maxValue, cvImage);
-  sensor_msgs::Image rosImage;
-  cvImage.toImageMsg(rosImage);
+  // Convert to image message.
+  sensor_msgs::Image image;
+  GridMapRosConverter::toImage(gridMapInput, "layer", sensor_msgs::image_encodings::BGRA8, minValue,
+                               maxValue, image);
 
   // Convert back to grid map.
   GridMap gridMapOutput;
-  GridMapRosConverter::initializeFromImage(rosImage, gridMapInput.getResolution(), gridMapOutput);
-  GridMapRosConverter::addLayerFromImage(rosImage, "layer", gridMapOutput, minValue, maxValue);
+  GridMapRosConverter::initializeFromImage(image, gridMapInput.getResolution(), gridMapOutput);
+  GridMapRosConverter::addLayerFromImage(image, "layer", gridMapOutput, minValue, maxValue);
 
   // Check data.
   const float resolution = (maxValue - minValue) / (float) std::numeric_limits<unsigned char>::max();
@@ -218,17 +216,15 @@ TEST(ImageConversion, roundTripMONO16)
   const float minValue = -1.0;
   const float maxValue = 1.0;
 
-  // Convert to CV image.
-  cv_bridge::CvImage cvImage;
-  GridMapRosConverter::toCvImage(gridMapInput, "layer", sensor_msgs::image_encodings::MONO16,
-                                 minValue, maxValue, cvImage);
-  sensor_msgs::Image rosImage;
-  cvImage.toImageMsg(rosImage);
+  // Convert to image message.
+  sensor_msgs::Image image;
+  GridMapRosConverter::toImage(gridMapInput, "layer", sensor_msgs::image_encodings::MONO16,
+                               minValue, maxValue, image);
 
   // Convert back to grid map.
   GridMap gridMapOutput;
-  GridMapRosConverter::initializeFromImage(rosImage, gridMapInput.getResolution(), gridMapOutput);
-  GridMapRosConverter::addLayerFromImage(rosImage, "layer", gridMapOutput, minValue, maxValue);
+  GridMapRosConverter::initializeFromImage(image, gridMapInput.getResolution(), gridMapOutput);
+  GridMapRosConverter::addLayerFromImage(image, "layer", gridMapOutput, minValue, maxValue);
 
   // Check data.
   // TODO Why is factor 300 necessary?