00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00050
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
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
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
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
00191
00192
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
00216 NumpyAllocator g_numpyAllocator;
00217
00218 int convert_to_CvMat2(const PyObject* o, cv::Mat& m)
00219 {
00220
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
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();
00300
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
00317 cv::Mat mat_in;
00318 convert_to_CvMat2(obj_in.ptr(), mat_in);
00319
00320
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
00353 boost::python::def("getCvType", cv_bridge::getCvType);
00354 boost::python::def("cvtColor", cvtColorWrap);
00355 boost::python::def("cvtColor2", cvtColor2Wrap);
00356 }