module.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <iostream>
00036 #include <boost/python.hpp>
00037 #include <cv_bridge/cv_bridge.h>
00038 #include <Python.h>
00039 
00040 #include <numpy/ndarrayobject.h>
00041 
00042 #include <opencv/cv.h>
00043 
00044 PyObject *mod_opencv;
00045 
00046 using namespace cv;
00047 namespace bp = boost::python;
00048 
00049 // These are sucky, sketchy versions of the real things in OpenCV Python,
00050 // inferior in every way.
00051 
00052 struct cvmat_t {
00053   PyObject_HEAD
00054   CvMat *a;
00055   PyObject *data;
00056   size_t offset;
00057 };
00058 
00059 static int failmsg(const char *fmt, ...)
00060 {
00061   char str[1000];
00062 
00063   va_list ap;
00064   va_start(ap, fmt);
00065   vsnprintf(str, sizeof(str), fmt, ap);
00066   va_end(ap);
00067 
00068   PyErr_SetString(PyExc_TypeError, str);
00069   return 0;
00070 }
00071 
00072 static int is_cvmat(PyObject *o)
00073 {
00074   return 1;
00075 }
00076 
00077 static int convert_to_CvMat1(PyObject *o, CvMat **dst, const char *name)
00078 {
00079   cvmat_t *m = (cvmat_t*)o;
00080   void *buffer;
00081   Py_ssize_t buffer_len;
00082 
00083   if (!is_cvmat(o)) {
00084     return failmsg("Argument '%s' must be CvMat", name);
00085   } else {
00086     m->a->refcount = NULL;
00087     if (m->data && PyString_Check(m->data)) {
00088       char *ptr = PyString_AsString(m->data) + m->offset;
00089       cvSetData(m->a, ptr, m->a->step);
00090       *dst = m->a;
00091       return 1;
00092     } else if (m->data && PyObject_AsWriteBuffer(m->data, &buffer, &buffer_len) == 0) {
00093       cvSetData(m->a, (void*)((char*)buffer + m->offset), m->a->step);
00094       *dst = m->a;
00095       return 1;
00096     } else {
00097       return failmsg("CvMat argument '%s' has no data", name);
00098     }
00099   }
00100 }
00101 
00102 static PyObject *FROM_CvMat(CvMat *m)
00103 {
00104   PyObject *creatematheader  = PyObject_GetAttrString(mod_opencv, "CreateMatHeader");
00105   PyObject *setdata  = PyObject_GetAttrString(mod_opencv, "SetData");
00106   PyObject *args;
00107 
00108   args = Py_BuildValue("iii", m->rows, m->cols, CV_MAT_TYPE(m->type));
00109   PyObject *pym = PyObject_CallObject(creatematheader, args);
00110   Py_DECREF(args);
00111 
00112   args = Py_BuildValue("Os#i", pym, m->data.ptr, m->rows * m->step, m->step);
00113   Py_DECREF(PyObject_CallObject(setdata, args));
00114   Py_DECREF(args);
00115 
00116   Py_DECREF(creatematheader);
00117   Py_DECREF(setdata);
00118 
00119   return pym;
00120 }
00121 
00122 bp::object
00123 cvtColorWrap(bp::object obj_in, const std::string & encoding_in, const std::string & encoding_out) {
00124   // Convert the Python input to an image
00125   CvMat *cv_mat_in;
00126   convert_to_CvMat1(obj_in.ptr(), &cv_mat_in, "image");
00127 
00128   cv::Mat mat_in(cv_mat_in);
00129 
00130   // Call cv_bridge for color conversion
00131   cv_bridge::CvImagePtr cv_image(new cv_bridge::CvImage(std_msgs::Header(), encoding_in, mat_in));
00132 
00133   cv_bridge::CvImagePtr res = cv_bridge::cvtColor(cv_image, encoding_out);
00134 
00135   CvMat m(res->image);
00136 
00137   return bp::object(boost::python::handle<>(FROM_CvMat(&m)));
00138 }
00139 
00140 
00141 // Taken from http://stackoverflow.com/questions/19136944/call-c-opencv-functions-from-python-send-a-cv-mat-to-c-dll-which-is-usi
00142 
00143 
00144 static size_t REFCOUNT_OFFSET = ( size_t )&((( PyObject* )0)->ob_refcnt ) +
00145 ( 0x12345678 != *( const size_t* )"\x78\x56\x34\x12\0\0\0\0\0" )*sizeof( int );
00146 
00147 
00148 static inline PyObject* pyObjectFromRefcount( const int* refcount )
00149 {
00150 return ( PyObject* )(( size_t )refcount - REFCOUNT_OFFSET );
00151 }
00152 
00153 static inline int* refcountFromPyObject( const PyObject* obj )
00154 {
00155 return ( int* )(( size_t )obj + REFCOUNT_OFFSET );
00156 }
00157 
00158 class NumpyAllocator : public cv::MatAllocator
00159 {
00160 public:
00161 NumpyAllocator( ) { }
00162 ~NumpyAllocator( ) { }
00163 
00164 void allocate( int dims, const int* sizes, int type, int*& refcount,
00165 uchar*& datastart, uchar*& data, size_t* step );
00166 
00167 void deallocate( int* refcount, uchar* datastart, uchar* data );
00168 };
00169 
00170 void doImport( )
00171 {
00172     import_array( );
00173 }
00174 
00175 void NumpyAllocator::allocate( int dims, const int* sizes, int type, int*& refcount, uchar*& datastart, uchar*& data, size_t* step )
00176 {
00177     int depth = CV_MAT_DEPTH( type );
00178     int cn = CV_MAT_CN( type );
00179     const int f = ( int )( sizeof( size_t )/8 );
00180     int typenum = depth == CV_8U ? NPY_UBYTE : depth == CV_8S ? NPY_BYTE :
00181                   depth == CV_16U ? NPY_USHORT : depth == CV_16S ? NPY_SHORT :
00182                   depth == CV_32S ? NPY_INT : depth == CV_32F ? NPY_FLOAT :
00183                   depth == CV_64F ? NPY_DOUBLE : f*NPY_ULONGLONG + (f^1)*NPY_UINT;
00184     int i;
00185     npy_intp _sizes[CV_MAX_DIM+1];
00186     for( i = 0; i < dims; i++ )
00187         _sizes[i] = sizes[i];
00188     if( cn > 1 )
00189     {
00190     /*if( _sizes[dims-1] == 1 )
00191          _sizes[dims-1] = cn;
00192     else*/
00193         _sizes[dims++] = cn;
00194     }
00195     PyObject* o = PyArray_SimpleNew( dims, _sizes, typenum );
00196     if( !o )
00197     CV_Error_(CV_StsError, ("The numpy array of typenum=%d, ndims=%d can not be created", typenum, dims));
00198     refcount = refcountFromPyObject(o);
00199     npy_intp* _strides = PyArray_STRIDES(o);
00200     for( i = 0; i < dims - (cn > 1); i++ )
00201         step[i] = (size_t)_strides[i];
00202     datastart = data = (uchar*)PyArray_DATA(o);
00203 
00204 }
00205 
00206 void NumpyAllocator::deallocate( int* refcount, uchar* datastart, uchar* data )
00207 {
00208     if( !refcount )
00209        return;
00210     PyObject* o = pyObjectFromRefcount(refcount);
00211     Py_INCREF(o);
00212     Py_DECREF(o);
00213 }
00214 
00215 // Declare the object
00216 NumpyAllocator g_numpyAllocator;
00217 
00218 int convert_to_CvMat2(const PyObject* o, cv::Mat& m)
00219 {
00220     // to avoid PyArray_Check() to crash even with valid array
00221     doImport( );
00222 
00223     if(!o || o == Py_None)
00224     {
00225         if( !m.data )
00226             m.allocator = &g_numpyAllocator;
00227         return true;
00228     }
00229 
00230     if( !PyArray_Check(o) )
00231     {
00232         failmsg("Not a numpy array");
00233         return false;
00234     }
00235 
00236     // NPY_LONG (64 bit) is converted to CV_32S (32 bit)
00237     int typenum = PyArray_TYPE(o);
00238     int type = typenum == NPY_UBYTE ? CV_8U : typenum == NPY_BYTE ? CV_8S :
00239         typenum == NPY_USHORT ? CV_16U : typenum == NPY_SHORT ? CV_16S :
00240         typenum == NPY_INT || typenum == NPY_LONG ? CV_32S :
00241         typenum == NPY_FLOAT ? CV_32F :
00242         typenum == NPY_DOUBLE ? CV_64F : -1;
00243 
00244     if( type < 0 )
00245     {
00246         failmsg("data type = %d is not supported", typenum);
00247         return false;
00248     }
00249 
00250     int ndims = PyArray_NDIM(o);
00251     if(ndims >= CV_MAX_DIM)
00252     {
00253         failmsg("dimensionality (=%d) is too high", ndims);
00254         return false;
00255     }
00256 
00257     int size[CV_MAX_DIM+1];
00258     size_t step[CV_MAX_DIM+1], elemsize = CV_ELEM_SIZE1(type);
00259     const npy_intp* _sizes = PyArray_DIMS(o);
00260     const npy_intp* _strides = PyArray_STRIDES(o);
00261     bool transposed = false;
00262 
00263     for(int i = 0; i < ndims; i++)
00264     {
00265         size[i] = (int)_sizes[i];
00266         step[i] = (size_t)_strides[i];
00267     }
00268 
00269     if( ndims == 0 || step[ndims-1] > elemsize ) {
00270         size[ndims] = 1;
00271         step[ndims] = elemsize;
00272         ndims++;
00273     }
00274 
00275     if( ndims >= 2 && step[0] < step[1] )
00276     {
00277         std::swap(size[0], size[1]);
00278         std::swap(step[0], step[1]);
00279         transposed = true;
00280     }
00281 
00282     if( ndims == 3 && size[2] <= CV_CN_MAX && step[1] == elemsize*size[2] )
00283     {
00284         ndims--;
00285         type |= CV_MAKETYPE(0, size[2]);
00286     }
00287 
00288     if( ndims > 2 )
00289     {
00290         failmsg("more than 2 dimensions");
00291         return false;
00292     }
00293 
00294     m = cv::Mat(ndims, size, type, PyArray_DATA(o), step);
00295 
00296     if( m.data )
00297     {
00298         m.refcount = refcountFromPyObject(o);
00299         m.addref(); // protect the original numpy array from deallocation
00300         // (since Mat destructor will decrement the reference counter)
00301     };
00302     m.allocator = &g_numpyAllocator;
00303 
00304     if( transposed )
00305     {
00306         cv::Mat tmp;
00307         tmp.allocator = &g_numpyAllocator;
00308         transpose(m, tmp);
00309         m = tmp;
00310     }
00311     return true;
00312 }
00313 
00314 bp::object
00315 cvtColor2Wrap(bp::object obj_in, const std::string & encoding_in, const std::string & encoding_out) {
00316   // Convert the Python input to an image
00317   cv::Mat mat_in;
00318   convert_to_CvMat2(obj_in.ptr(), mat_in);
00319 
00320   // Call cv_bridge for color conversion
00321   cv_bridge::CvImagePtr cv_image(new cv_bridge::CvImage(std_msgs::Header(), encoding_in, mat_in));
00322 
00323   cv::Mat mat = cv_bridge::cvtColor(cv_image, encoding_out)->image;
00324 
00325   npy_intp dims[] = {mat.rows, mat.cols, mat.channels()};
00326 
00327   PyObject *res = 0 ;
00328   if (mat.depth() == CV_8U)
00329       res = PyArray_SimpleNew(3, dims, NPY_UBYTE);
00330   else if (mat.depth() == CV_8S)
00331       res = PyArray_SimpleNew(3, dims, NPY_BYTE);
00332   else if (mat.depth() == CV_16S)
00333       res = PyArray_SimpleNew(3, dims, NPY_SHORT);
00334   else if (mat.depth() == CV_16U)
00335       res = PyArray_SimpleNew(3, dims, NPY_USHORT);
00336   else if (mat.depth() == CV_32S)
00337       res = PyArray_SimpleNew(3, dims, NPY_INT);
00338   else if (mat.depth() == CV_32F)
00339       res = PyArray_SimpleNew(3, dims, NPY_CFLOAT);
00340   else if (mat.depth() == CV_64F)
00341       res = PyArray_SimpleNew(3, dims, NPY_CDOUBLE);
00342 
00343   std::memcpy(PyArray_DATA((PyArrayObject*)res), mat.data, mat.step*mat.rows);
00344 
00345   return bp::object(boost::python::handle<>(res));
00346 }
00347 
00348 BOOST_PYTHON_MODULE(cv_bridge_boost)
00349 {
00350   mod_opencv = PyImport_ImportModule("cv");
00351 
00352   // Wrap the function to get encodings as OpenCV types
00353   boost::python::def("getCvType", cv_bridge::getCvType);
00354   boost::python::def("cvtColor", cvtColorWrap);
00355   boost::python::def("cvtColor2", cvtColor2Wrap);
00356 }


cv_bridge
Author(s): Patrick Mihelich, James Bowman
autogenerated on Mon Oct 6 2014 08:47:23