43 #ifndef vpImageTools_H
44 #define vpImageTools_H
54 #include <visp/vpImage.h>
56 #ifdef VISP_HAVE_PTHREAD
60 #include <visp/vpImageException.h>
61 #include <visp/vpMath.h>
62 #include <visp/vpRect.h>
63 #include <visp/vpCameraParameters.h>
85 unsigned int i_sub,
unsigned int j_sub,
86 unsigned int nrow_sub,
unsigned int ncol_sub,
95 Type threshold1, Type threshold2,
96 Type value1, Type value2, Type value3);
134 unsigned int i_sub,
unsigned int j_sub,
135 unsigned int nrow_sub,
unsigned int ncol_sub,
139 unsigned int imax = i_sub + nrow_sub ;
140 unsigned int jmax = j_sub + ncol_sub ;
145 nrow_sub = imax-i_sub ;
150 ncol_sub = jmax -j_sub ;
153 S.
resize(nrow_sub, ncol_sub) ;
154 for (i=i_sub ; i < imax ; i++)
155 for (j=j_sub ; j < jmax ; j++)
157 S[i-i_sub][j-j_sub] = I[i][j] ;
176 double dtop = rect.
getTop();
177 double dright = ceil( rect.
getRight() );
178 double dbottom = ceil( rect.
getBottom() );
180 if (dleft < 0.0) dleft = 0.0;
183 if (dright < 0.0) dright = 0.0;
186 if (dtop < 0.0) dtop = 0.0;
189 if (dbottom < 0.0) dbottom = 0.0;
193 unsigned int left = (
unsigned int) dleft;
194 unsigned int top = (
unsigned int) dtop;
195 unsigned int bottom = (
unsigned int) dbottom;
196 unsigned int right = (
unsigned int) dright;
198 unsigned int width = right - left + 1;;
199 unsigned int height = bottom - top + 1;
202 for (
unsigned int i=top ; i <= bottom ; i++) {
203 for (
unsigned int j=left ; j <= right ; j++) {
204 S[i-top][j-left] = I[i][j] ;
222 Type threshold1, Type threshold2,
223 Type value1, Type value2, Type value3)
226 unsigned char *p = I.
bitmap;
228 for (; p < pend; p ++) {
230 if (v < threshold1) *p = value1;
231 else if (v > threshold2) *p = value3;
237 #ifdef VISP_HAVE_PTHREAD
239 #ifndef DOXYGEN_SHOULD_SKIP_THIS
241 class vpUndistortInternalType
249 unsigned int nthreads;
250 unsigned int threadid;
252 vpUndistortInternalType() {};
253 vpUndistortInternalType(
const vpUndistortInternalType<Type> &u) {
259 nthreads = u.nthreads;
260 threadid = u.threadid;
263 static void *vpUndistort_threaded(
void *arg);
268 void *vpUndistortInternalType<Type>::vpUndistort_threaded(
void *arg)
270 vpUndistortInternalType<Type> *undistortSharedData = (vpUndistortInternalType<Type>*)arg;
271 int offset = (int)undistortSharedData->threadid;
272 int width = (
int)undistortSharedData->width;
273 int height = (int)undistortSharedData->height;
274 int nthreads = (
int)undistortSharedData->nthreads;
276 double u0 = undistortSharedData->cam.get_u0();
277 double v0 = undistortSharedData->cam.get_v0();
278 double px = undistortSharedData->cam.get_px();
279 double py = undistortSharedData->cam.get_py();
280 double kud = undistortSharedData->cam.get_kud();
282 double invpx = 1.0/px;
283 double invpy = 1.0/py;
285 double kud_px2 = kud * invpx * invpx;
286 double kud_py2 = kud * invpy * invpy;
288 Type *dst = undistortSharedData->dst+(height/nthreads*offset)*width;
289 Type *src = undistortSharedData->src;
291 for (
double v = height/nthreads*offset;v < height/nthreads*(offset+1) ; v++) {
292 double deltav = v - v0;
294 double fr1 = 1.0 + kud_py2 * deltav * deltav;
296 for (
double u = 0 ; u < width ; u++) {
298 double deltau = u - u0;
300 double fr2 = fr1 + kud_px2 * deltau * deltau;
302 double u_double = deltau * fr2 + u0;
303 double v_double = deltav * fr2 + v0;
308 int u_round = (int) (u_double);
309 int v_round = (int) (v_double);
310 if (u_round < 0.f) u_round = -1;
311 if (v_round < 0.f) v_round = -1;
312 double du_double = (u_double) - (
double) u_round;
313 double dv_double = (v_double) - (
double) v_round;
316 if ( (0 <= u_round) && (0 <= v_round) &&
317 (u_round < ((width) - 1)) && (v_round < ((height) - 1)) ) {
319 const Type* _mp = &src[v_round*width+u_round];
320 v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
322 v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
323 *dst = (Type)(v01 + ((v23 - v01) * dv_double));
332 pthread_exit((
void*) 0);
335 #endif // DOXYGEN_SHOULD_SKIP_THIS
336 #endif // VISP_HAVE_PTHREAD
362 #ifdef VISP_HAVE_PTHREAD
369 undistI.
resize(height, width);
374 if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
380 unsigned int nthreads = 2;
382 pthread_t *callThd =
new pthread_t [nthreads];
383 pthread_attr_init(&attr);
384 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
386 vpUndistortInternalType<Type> *undistortSharedData;
387 undistortSharedData =
new vpUndistortInternalType<Type> [nthreads];
389 for(
unsigned int i=0;i<nthreads;i++) {
392 undistortSharedData[i].src = I.
bitmap;
393 undistortSharedData[i].dst = undistI.
bitmap;
394 undistortSharedData[i].width = I.
getWidth();
395 undistortSharedData[i].height = I.
getHeight();
396 undistortSharedData[i].cam = cam;
397 undistortSharedData[i].nthreads = nthreads;
398 undistortSharedData[i].threadid = i;
399 pthread_create( &callThd[i], &attr,
400 &vpUndistortInternalType<Type>::vpUndistort_threaded,
401 &undistortSharedData[i]);
403 pthread_attr_destroy(&attr);
406 for(
unsigned int i=0;i<nthreads;i++) {
408 pthread_join( callThd[i], NULL);
412 delete [] undistortSharedData;
413 #else // VISP_HAVE_PTHREAD
420 undistI.
resize(height, width);
429 if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
435 double invpx = 1.0/px;
436 double invpy = 1.0/py;
438 double kud_px2 = kud * invpx * invpx;
439 double kud_py2 = kud * invpy * invpy;
441 Type *dst = undistI.
bitmap;
442 for (
double v = 0;v < height ; v++) {
443 double deltav = v - v0;
445 double fr1 = 1.0 + kud_py2 * deltav * deltav;
447 for (
double u = 0 ; u < width ; u++) {
449 double deltau = u - u0;
451 double fr2 = fr1 + kud_px2 * deltau * deltau;
453 double u_double = deltau * fr2 + u0;
454 double v_double = deltav * fr2 + v0;
461 int u_round = (int) (u_double);
462 int v_round = (int) (v_double);
463 if (u_round < 0.f) u_round = -1;
464 if (v_round < 0.f) v_round = -1;
465 double du_double = (u_double) - (
double) u_round;
466 double dv_double = (v_double) - (
double) v_round;
469 if ( (0 <= u_round) && (0 <= v_round) &&
470 (u_round < (((
int)width) - 1)) && (v_round < (((
int)height) - 1)) ) {
472 const Type* _mp = &I[(
unsigned int)v_round][(
unsigned int)u_round];
473 v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
475 v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
476 *dst = (Type)(v01 + ((v23 - v01) * dv_double));
485 #endif // VISP_HAVE_PTHREAD
495 undistI.
resize(height,width);
509 for(
int v = 0 ; v < height; v++){
510 for(
int u = 0; u < height; u++){
513 double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
514 double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
515 undistI[v][u] = I.getPixelBI((
float)u_double,(
float)v_double);
532 unsigned int height = 0, width = 0;
537 newI.
resize(height, width);
539 for ( i = 0; i < height; i++)
541 memcpy(newI.
bitmap+i*width, I.
bitmap+(height-1-i)*width,
555 unsigned int height = 0, width = 0;
563 for ( i = 0; i < height/2; i++)