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);
130 unsigned int i_sub,
unsigned int j_sub,
131 unsigned int nrow_sub,
unsigned int ncol_sub,
135 unsigned int imax = i_sub + nrow_sub ;
136 unsigned int jmax = j_sub + ncol_sub ;
141 nrow_sub = imax-i_sub ;
146 ncol_sub = jmax -j_sub ;
149 S.
resize(nrow_sub, ncol_sub) ;
150 for (i=i_sub ; i < imax ; i++)
151 for (j=j_sub ; j < jmax ; j++)
153 S[i-i_sub][j-j_sub] = I[i][j] ;
172 double dtop = rect.
getTop();
173 double dright = ceil( rect.
getRight() );
174 double dbottom = ceil( rect.
getBottom() );
176 if (dleft < 0.0) dleft = 0.0;
179 if (dright < 0.0) dright = 0.0;
182 if (dtop < 0.0) dtop = 0.0;
185 if (dbottom < 0.0) dbottom = 0.0;
189 unsigned int left = (
unsigned int) dleft;
190 unsigned int top = (
unsigned int) dtop;
191 unsigned int bottom = (
unsigned int) dbottom;
192 unsigned int right = (
unsigned int) dright;
194 unsigned int width = right - left + 1;;
195 unsigned int height = bottom - top + 1;
198 for (
unsigned int i=top ; i <= bottom ; i++) {
199 for (
unsigned int j=left ; j <= right ; j++) {
200 S[i-top][j-left] = I[i][j] ;
218 Type threshold1, Type threshold2,
219 Type value1, Type value2, Type value3)
222 unsigned char *p = I.
bitmap;
224 for (; p < pend; p ++) {
226 if (v < threshold1) *p = value1;
227 else if (v > threshold2) *p = value3;
233 #ifdef VISP_HAVE_PTHREAD
235 #ifndef DOXYGEN_SHOULD_SKIP_THIS
237 class vpUndistortInternalType
245 unsigned int nthreads;
246 unsigned int threadid;
248 vpUndistortInternalType() {};
249 vpUndistortInternalType(
const vpUndistortInternalType<Type> &u) {
255 nthreads = u.nthreads;
256 threadid = u.threadid;
259 static void *vpUndistort_threaded(
void *arg);
264 void *vpUndistortInternalType<Type>::vpUndistort_threaded(
void *arg)
266 vpUndistortInternalType<Type> *undistortSharedData = (vpUndistortInternalType<Type>*)arg;
267 int offset = (int)undistortSharedData->threadid;
268 int width = (
int)undistortSharedData->width;
269 int height = (int)undistortSharedData->height;
270 int nthreads = (
int)undistortSharedData->nthreads;
272 double u0 = undistortSharedData->cam.get_u0();
273 double v0 = undistortSharedData->cam.get_v0();
274 double px = undistortSharedData->cam.get_px();
275 double py = undistortSharedData->cam.get_py();
276 double kud = undistortSharedData->cam.get_kud();
278 double invpx = 1.0/px;
279 double invpy = 1.0/py;
281 double kud_px2 = kud * invpx * invpx;
282 double kud_py2 = kud * invpy * invpy;
284 Type *dst = undistortSharedData->dst+(height/nthreads*offset)*width;
285 Type *src = undistortSharedData->src;
287 for (
double v = height/nthreads*offset;v < height/nthreads*(offset+1) ; v++) {
288 double deltav = v - v0;
290 double fr1 = 1.0 + kud_py2 * deltav * deltav;
292 for (
double u = 0 ; u < width ; u++) {
294 double deltau = u - u0;
296 double fr2 = fr1 + kud_px2 * deltau * deltau;
298 double u_double = deltau * fr2 + u0;
299 double v_double = deltav * fr2 + v0;
304 int u_round = (int) (u_double);
305 int v_round = (int) (v_double);
306 if (u_round < 0.f) u_round = -1;
307 if (v_round < 0.f) v_round = -1;
308 double du_double = (u_double) - (
double) u_round;
309 double dv_double = (v_double) - (
double) v_round;
312 if ( (0 <= u_round) && (0 <= v_round) &&
313 (u_round < ((width) - 1)) && (v_round < ((height) - 1)) ) {
315 const Type* _mp = &src[v_round*width+u_round];
316 v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
318 v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
319 *dst = (Type)(v01 + ((v23 - v01) * dv_double));
328 pthread_exit((
void*) 0);
331 #endif // DOXYGEN_SHOULD_SKIP_THIS
332 #endif // VISP_HAVE_PTHREAD
358 #ifdef VISP_HAVE_PTHREAD
365 undistI.
resize(height, width);
370 if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
376 unsigned int nthreads = 2;
378 pthread_t *callThd =
new pthread_t [nthreads];
379 pthread_attr_init(&attr);
380 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
382 vpUndistortInternalType<Type> *undistortSharedData;
383 undistortSharedData =
new vpUndistortInternalType<Type> [nthreads];
385 for(
unsigned int i=0;i<nthreads;i++) {
388 undistortSharedData[i].src = I.
bitmap;
389 undistortSharedData[i].dst = undistI.
bitmap;
390 undistortSharedData[i].width = I.
getWidth();
391 undistortSharedData[i].height = I.
getHeight();
392 undistortSharedData[i].cam = cam;
393 undistortSharedData[i].nthreads = nthreads;
394 undistortSharedData[i].threadid = i;
395 pthread_create( &callThd[i], &attr,
396 &vpUndistortInternalType<Type>::vpUndistort_threaded,
397 &undistortSharedData[i]);
399 pthread_attr_destroy(&attr);
402 for(
unsigned int i=0;i<nthreads;i++) {
404 pthread_join( callThd[i], NULL);
408 delete [] undistortSharedData;
409 #else // VISP_HAVE_PTHREAD
416 undistI.
resize(height, width);
425 if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
431 double invpx = 1.0/px;
432 double invpy = 1.0/py;
434 double kud_px2 = kud * invpx * invpx;
435 double kud_py2 = kud * invpy * invpy;
437 Type *dst = undistI.
bitmap;
438 for (
double v = 0;v < height ; v++) {
439 double deltav = v - v0;
441 double fr1 = 1.0 + kud_py2 * deltav * deltav;
443 for (
double u = 0 ; u < width ; u++) {
445 double deltau = u - u0;
447 double fr2 = fr1 + kud_px2 * deltau * deltau;
449 double u_double = deltau * fr2 + u0;
450 double v_double = deltav * fr2 + v0;
457 int u_round = (int) (u_double);
458 int v_round = (int) (v_double);
459 if (u_round < 0.f) u_round = -1;
460 if (v_round < 0.f) v_round = -1;
461 double du_double = (u_double) - (
double) u_round;
462 double dv_double = (v_double) - (
double) v_round;
465 if ( (0 <= u_round) && (0 <= v_round) &&
466 (u_round < (((
int)width) - 1)) && (v_round < (((
int)height) - 1)) ) {
468 const Type* _mp = &I[(
unsigned int)v_round][(
unsigned int)u_round];
469 v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
471 v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
472 *dst = (Type)(v01 + ((v23 - v01) * dv_double));
481 #endif // VISP_HAVE_PTHREAD
491 undistI.
resize(height,width);
505 for(
int v = 0 ; v < height; v++){
506 for(
int u = 0; u < height; u++){
509 double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
510 double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
511 undistI[v][u] = I.getPixelBI((
float)u_double,(
float)v_double);
528 unsigned int height = 0, width = 0;
533 newI.
resize(height, width);
535 for ( i = 0; i < height; i++)
537 memcpy(newI.
bitmap+i*width, I.
bitmap+(height-1-i)*width,
551 unsigned int height = 0, width = 0;
559 for ( i = 0; i < height/2; i++)
unsigned int getWidth() const
Type * bitmap
points toward the bitmap
void resize(const unsigned int height, const unsigned int width)
set the size of the image
static double sqr(double x)
Generic class defining intrinsic camera parameters.
unsigned int getHeight() const
Defines a rectangle in the plane.
Definition of the vpImage class member functions.