image.cpp
Go to the documentation of this file.
00001 /****************************************************************************
00002  *
00003  * $Id: image.cpp 3496 2011-11-22 15:14:32Z fnovotny $
00004  *
00005  * This file is part of the ViSP software.
00006  * Copyright (C) 2005 - 2012 by INRIA. All rights reserved.
00007  * 
00008  * This software is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * ("GPL") version 2 as published by the Free Software Foundation.
00011  * See the file LICENSE.txt at the root directory of this source
00012  * distribution for additional information about the GNU GPL.
00013  *
00014  * For using ViSP with software that can not be combined with the GNU
00015  * GPL, please contact INRIA about acquiring a ViSP Professional 
00016  * Edition License.
00017  *
00018  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
00019  * 
00020  * This software was developed at:
00021  * INRIA Rennes - Bretagne Atlantique
00022  * Campus Universitaire de Beaulieu
00023  * 35042 Rennes Cedex
00024  * France
00025  * http://www.irisa.fr/lagadic
00026  *
00027  * If you have questions regarding the use of this file, please contact
00028  * INRIA at visp@inria.fr
00029  * 
00030  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
00031  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
00032  *
00033  * Contact visp@irisa.fr if any conditions of this licensing are
00034  * not clear to you.
00035  *
00036  * Description:
00037  * 
00038  *
00039  * Authors:
00040  * Filip Novotny
00041  * 
00042  *
00043  *****************************************************************************/
00044 
00050 #include <stdexcept>
00051 
00052 #include <sensor_msgs/Image.h>
00053 #include <sensor_msgs/image_encodings.h>
00054 #include <boost/format.hpp>
00055 
00056 #include "visp_bridge/image.h"
00057 
00058 namespace visp_bridge
00059 {
00060 
00061 sensor_msgs::Image toSensorMsgsImage(const vpImage<unsigned char>& src)
00062 {
00063   sensor_msgs::Image dst;
00064   dst.width = src.getWidth();
00065   dst.height = src.getHeight();
00066   dst.encoding = sensor_msgs::image_encodings::MONO8;
00067   dst.step = src.getWidth();
00068   dst.data.resize(dst.height * dst.step);
00069   memcpy(&dst.data[0], src.bitmap, dst.height * dst.step * sizeof(unsigned char));
00070 
00071   return dst;
00072 }
00073 
00074 vpImage<unsigned char> toVispImage(const sensor_msgs::Image& src)
00075 {
00076   using sensor_msgs::image_encodings::MONO8;
00077   using sensor_msgs::image_encodings::RGB8;
00078   using sensor_msgs::image_encodings::RGBA8;
00079   using sensor_msgs::image_encodings::BGR8;
00080   using sensor_msgs::image_encodings::BGRA8;
00081 
00082 
00083   vpImage<unsigned char> dst(src.height, src.width);
00084 
00085   if (src.encoding == MONO8)
00086     memcpy(dst.bitmap, &(src.data[0]), dst.getHeight() * src.step * sizeof(unsigned char));
00087   else if (src.encoding == RGB8 || src.encoding == RGBA8 || src.encoding == BGR8 || src.encoding == BGRA8){
00088     unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);
00089     unsigned cEnd = (src.encoding == RGBA8 || src.encoding == BGRA8) ? nc - 1 : nc;
00090 
00091     for (unsigned i = 0; i < dst.getWidth(); ++i){
00092       for (unsigned j = 0; j < dst.getHeight(); ++j){
00093         int acc = 0;
00094         for (unsigned c = 0; c < cEnd; ++c)
00095           acc += src.data[j * src.step + i * nc + c];
00096         dst[j][i] = acc / nc;
00097       }
00098     }
00099   }
00100   return dst;
00101 }
00102 
00103 vpImage<vpRGBa> toVispImageRGBa(const sensor_msgs::Image& src)
00104 {
00105   using sensor_msgs::image_encodings::MONO8;
00106   using sensor_msgs::image_encodings::RGB8;
00107   using sensor_msgs::image_encodings::RGBA8;
00108   using sensor_msgs::image_encodings::BGR8;
00109   using sensor_msgs::image_encodings::BGRA8;
00110 
00111 
00112   vpImage<vpRGBa> dst(src.height, src.width);
00113 
00114   if (src.encoding == MONO8)
00115     for (unsigned i = 0; i < dst.getWidth(); ++i){
00116       for (unsigned j = 0; j < dst.getHeight(); ++j){
00117 
00118         dst[j][i] = vpRGBa(src.data[j * src.step + i],src.data[j * src.step + i],src.data[j * src.step + i]);
00119       }
00120     }
00121   else{
00122     unsigned nc = sensor_msgs::image_encodings::numChannels(src.encoding);
00123 
00124     for (unsigned i = 0; i < dst.getWidth(); ++i){
00125       for (unsigned j = 0; j < dst.getHeight(); ++j){
00126         dst[j][i] = vpRGBa(src.data[j * src.step + i * nc + 0],src.data[j * src.step + i * nc + 1],src.data[j * src.step + i * nc + 2]);
00127       }
00128     }
00129   }
00130   return dst;
00131 }
00132 
00133 sensor_msgs::Image toSensorMsgsImage(const vpImage<vpRGBa>& src){
00134   sensor_msgs::Image dst;
00135   dst.width = src.getWidth();
00136   dst.height = src.getHeight();
00137   dst.encoding = sensor_msgs::image_encodings::RGB8;
00138   unsigned nc = sensor_msgs::image_encodings::numChannels(dst.encoding);
00139   dst.step = src.getWidth()*nc;
00140 
00141   dst.data.resize(dst.height * dst.step);
00142   for (unsigned i = 0; i < src.getWidth(); ++i){
00143     for (unsigned j = 0; j < src.getHeight(); ++j){
00144       dst.data[j * dst.step + i * nc + 0] = src.bitmap[j * src.getWidth() + i].R;
00145       dst.data[j * dst.step + i * nc + 1] = src.bitmap[j * src.getWidth() + i].G;
00146       dst.data[j * dst.step + i * nc + 2] = src.bitmap[j * src.getWidth() + i].B;
00147       //dst.data[j * dst.step + i * nc + 3] = src.bitmap[j * dst.step + i].A;
00148     }
00149   }
00150   return dst;
00151 }
00152 }


visp_bridge
Author(s): Filip Novotny
autogenerated on Thu Jul 4 2019 19:30:54