Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testUndistortImage.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test for image undistortion.
33  */
34 
35 #include <stdlib.h>
36 #include <visp3/core/vpDebug.h>
37 #include <visp3/core/vpImage.h>
38 #include <visp3/core/vpImageTools.h>
39 #include <visp3/core/vpIoTools.h>
40 #include <visp3/core/vpRGBa.h>
41 #include <visp3/core/vpTime.h>
42 #include <visp3/io/vpImageIo.h>
43 #include <visp3/io/vpParseArgv.h>
44 
53 // List of allowed command line options
54 #define GETOPTARGS "cdi:o:t:s:h"
55 
56 /*
57  Print the program options.
58 
59  \param name : Program name.
60  \param badparam : Bad parameter name.
61  \param ipath: Input image path.
62  \param opath : Output image path.
63  \param user : Username.
64  */
65 void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath,
66  const std::string &user)
67 {
68 #if VISP_HAVE_DATASET_VERSION >= 0x030600
69  std::string ext("png");
70 #else
71  std::string ext("pgm");
72 #endif
73  fprintf(stdout, "\n\
74 Read an image from the disk, undistort it \n\
75 and save the undistorted image on the disk.\n\
76 (grid36-01_undistorted.pgm).\n\
77 \n\
78 SYNOPSIS\n\
79  %s [-i <input image path>] [-o <output image path>] [-t <nThreads>] [-s <scale>]\n\
80  [-h]\n\
81  ",
82  name);
83 
84  fprintf(stdout, "\n\
85 OPTIONS: Default\n\
86  -i <input image path> %s\n\
87  Set image input path.\n\
88  From this path read \"calibration/grid36-01.%s\"\n\
89  image.\n\
90  Setting the VISP_INPUT_IMAGE_PATH environment\n\
91  variable produces the same behaviour than using\n\
92  this option.\n\
93  \n\
94  -o <output image path> %s\n\
95  Set image output path.\n\
96  From this directory, creates the \"%s\"\n\
97  subdirectory depending on the username, where \n\
98  grid36-01_undistorted.pgm output image is written.\n\
99 \n\
100  -t <nThreads> \n\
101  Set the number of threads to use for vpImageTools::undistort().\n\
102 \n\
103  -s <scale> \n\
104  Resize the image by the specified scale factor.\n\
105 \n\
106  -h\n\
107  Print the help.\n\n",
108  ext.c_str(), ipath.c_str(), opath.c_str(), user.c_str());
109 
110  if (badparam)
111  fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
112 }
113 
126 bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user,
127  unsigned int &nThreads, unsigned int &scale)
128 {
129  const char *optarg_;
130  int c;
131  while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
132 
133  switch (c) {
134  case 'i':
135  ipath = optarg_;
136  break;
137  case 'o':
138  opath = optarg_;
139  break;
140  case 't':
141  nThreads = atoi(optarg_);
142  break;
143  case 's':
144  scale = atoi(optarg_);
145  break;
146  case 'h':
147  usage(argv[0], nullptr, ipath, opath, user);
148  return false;
149 
150  case 'c':
151  case 'd':
152  break;
153 
154  default:
155  usage(argv[0], optarg_, ipath, opath, user);
156  return false;
157  }
158  }
159 
160  if ((c == 1) || (c == -1)) {
161  // standalone param or error
162  usage(argv[0], nullptr, ipath, opath, user);
163  std::cerr << "ERROR: " << std::endl;
164  std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
165  return false;
166  }
167 
168  return true;
169 }
170 
171 int main(int argc, const char **argv)
172 {
173  try {
174  std::string env_ipath;
175  std::string opt_ipath;
176  std::string opt_opath;
177  std::string ipath;
178  std::string opath;
179  std::string filename;
180  std::string username;
181  unsigned int nThreads = 2;
182  unsigned int scale = 1;
183 
184 #if VISP_HAVE_DATASET_VERSION >= 0x030600
185  std::string ext("png");
186 #else
187  std::string ext("pgm");
188 #endif
189 
190  // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
191  // environment variable value
192  env_ipath = vpIoTools::getViSPImagesDataPath();
193 
194  // Set the default input path
195  if (!env_ipath.empty())
196  ipath = env_ipath;
197 
198  // Set the default output path
199 #if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
200  opt_opath = "/tmp";
201 #elif defined(_WIN32)
202  opt_opath = "C:\\temp";
203 #endif
204 
205  // Get the user login name
206  vpIoTools::getUserName(username);
207 
208  // Read the command line options
209  if (getOptions(argc, argv, opt_ipath, opt_opath, username, nThreads, scale) == false) {
210  return EXIT_FAILURE;
211  }
212 
213  // Get the option values
214  if (!opt_ipath.empty())
215  ipath = opt_ipath;
216  if (!opt_opath.empty())
217  opath = opt_opath;
218 
219  // Append to the output path string, the login name of the user
220  opath = vpIoTools::createFilePath(opath, username);
221 
222  // Test if the output path exist. If no try to create it
223  if (vpIoTools::checkDirectory(opath) == false) {
224  try {
225  // Create the dirname
227  }
228  catch (...) {
229  usage(argv[0], nullptr, ipath, opt_opath, username);
230  std::cerr << std::endl << "ERROR:" << std::endl;
231  std::cerr << " Cannot create " << opath << std::endl;
232  std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
233  return EXIT_FAILURE;
234  }
235  }
236 
237  // Compare ipath and env_ipath. If they differ, we take into account
238  // the input path coming from the command line option
239  if (opt_ipath.empty()) {
240  if (ipath != env_ipath) {
241  std::cout << std::endl << "WARNING: " << std::endl;
242  std::cout << " Since -i <visp image path=" << ipath << "> "
243  << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
244  << " we skip the environment variable." << std::endl;
245  }
246  }
247 
248  // Test if an input path is set
249  if (opt_ipath.empty() && env_ipath.empty()) {
250  usage(argv[0], nullptr, ipath, opt_opath, username);
251  std::cerr << std::endl << "ERROR:" << std::endl;
252  std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
253  << " environment variable to specify the location of the " << std::endl
254  << " image path where test images are located." << std::endl
255  << std::endl;
256  return EXIT_FAILURE;
257  }
258 
259  //
260  // Here starts really the test
261  //
262  vpImage<vpRGBa> I, I_; // Input image
263  vpImage<unsigned char> I_gray;
264  vpImage<vpRGBa> U; // undistorted output image
265  vpImage<unsigned char> U_gray; // undistorted output image
266  vpImage<vpRGBa> U_remap; // undistorted output image
267  vpImage<unsigned char> U_remap_gray; // undistorted output image
268 
269  vpCameraParameters cam;
270  cam.initPersProjWithDistortion(600, 600, 320, 240, -0.17, 0.17);
271  // Read the input grey image from the disk
272  filename = vpIoTools::createFilePath(ipath, "calibration/grid36-01." + ext);
273  std::cout << "Read image: " << filename << std::endl;
274  vpImageIo::read(I_, filename);
275  if (scale > 1) {
276  std::cout << "Scale the image by a factor of " << scale << std::endl;
277  vpImageTools::resize(I_, I, I_.getWidth() * scale, I_.getHeight() * scale);
278  }
279  else {
280  I = I_;
281  }
282  std::cout << "Input image: " << I.getWidth() << "x" << I.getHeight() << std::endl;
283  vpImageConvert::convert(I, I_gray);
284 
285  std::cout << "Nb threads to use for vpImageTools::undistort(): " << nThreads << std::endl;
286 
287  double t_undistort = 0.0, t_remap = 0.0;
288  {
289  std::cout << "\nUndistortion in process (color image)... " << std::endl;
290  vpImageTools::undistort(I, cam, U, nThreads);
291 
292  double begintime = vpTime::measureTimeMs();
293 
294  // For the test, to have a significant time measure we repeat the
295  // undistortion 10 times
296  for (unsigned int i = 0; i < 10; i++)
297  // Create the undistorted image
298  vpImageTools::undistort(I, cam, U, nThreads);
299 
300  double endtime = vpTime::measureTimeMs();
301  t_undistort = endtime - begintime;
302 
303  std::cout << "Time for 10 color image undistortion (ms): " << t_undistort << std::endl;
304  }
305 
306  {
307  std::cout << "Undistortion in process with remap (color image)... " << std::endl;
308 
309  double begintime = vpTime::measureTimeMs();
310 
311  // For the test, to have a significant time measure we repeat the
312  // undistortion 10 times
313  vpArray2D<int> mapU, mapV;
314  vpArray2D<float> mapDu, mapDv;
315  for (unsigned int i = 0; i < 10; i++) {
316  if (i == 0) {
317  vpImageTools::initUndistortMap(cam, I.getWidth(), I.getHeight(), mapU, mapV, mapDu, mapDv);
318  }
319  vpImageTools::remap(I, mapU, mapV, mapDu, mapDv, U_remap);
320  }
321 
322  double endtime = vpTime::measureTimeMs();
323  t_remap = endtime - begintime;
324 
325  std::cout << "Time for 10 color image undistortion with remap (ms): " << t_remap << std::endl;
326  std::cout << "Speed-up: " << t_undistort / t_remap << "X" << std::endl;
327  }
328 
329  {
330  std::cout << "\nUndistortion in process (gray image)... " << std::endl;
331  vpImageTools::undistort(I_gray, cam, U_gray, nThreads);
332 
333  double begintime = vpTime::measureTimeMs();
334 
335  // For the test, to have a significant time measure we repeat the
336  // undistortion 100 times
337  for (unsigned int i = 0; i < 100; i++)
338  // Create the undistorted image
339  vpImageTools::undistort(I_gray, cam, U_gray, nThreads);
340 
341  double endtime = vpTime::measureTimeMs();
342  t_undistort = endtime - begintime;
343 
344  std::cout << "Time for 100 gray image undistortion (ms): " << t_undistort << std::endl;
345  }
346 
347  {
348  std::cout << "Undistortion in process with remap (gray image)... " << std::endl;
349 
350  double begintime = vpTime::measureTimeMs();
351 
352  // For the test, to have a significant time measure we repeat the
353  // undistortion 100 times
354  vpArray2D<int> mapU, mapV;
355  vpArray2D<float> mapDu, mapDv;
356  for (unsigned int i = 0; i < 10; i++) {
357  if (i == 0) {
358  vpImageTools::initUndistortMap(cam, I.getWidth(), I.getHeight(), mapU, mapV, mapDu, mapDv);
359  }
360  vpImageTools::remap(I_gray, mapU, mapV, mapDu, mapDv, U_remap_gray);
361  }
362 
363  double endtime = vpTime::measureTimeMs();
364  t_remap = endtime - begintime;
365 
366  std::cout << "Time for 100 gray image undistortion with remap (ms): " << t_remap << std::endl;
367  std::cout << "Speed-up: " << t_undistort / t_remap << "X" << std::endl;
368  }
369 
370  // Write the undistorted images on the disk
371  filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_color.png"));
372  std::cout << "\nWrite undistorted image: " << filename << std::endl;
373  vpImageIo::write(U, filename);
374 
375  filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_gray.png"));
376  std::cout << "Write undistorted image: " << filename << std::endl;
377  vpImageIo::write(U_gray, filename);
378 
379  filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_remap_color.png"));
380  std::cout << "\nWrite undistorted image with remap: " << filename << std::endl;
381  vpImageIo::write(U_remap, filename);
382 
383  filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_remap_gray.png"));
384  std::cout << "Write undistorted image with remap: " << filename << std::endl;
385  vpImageIo::write(U_remap_gray, filename);
386 
387  // Compute images difference
388  vpImage<vpRGBa> U_diff_abs;
389  vpImageTools::imageDifferenceAbsolute(U, U_remap, U_diff_abs);
390  double mean_diff = 0.0;
391  for (unsigned int i = 0; i < U_diff_abs.getHeight(); i++) {
392  for (unsigned int j = 0; j < U_diff_abs.getWidth(); j++) {
393  mean_diff += U_diff_abs[i][j].R;
394  mean_diff += U_diff_abs[i][j].G;
395  mean_diff += U_diff_abs[i][j].B;
396  mean_diff += U_diff_abs[i][j].A;
397  }
398  }
399  double remap_mean_error = mean_diff / (4 * U_diff_abs.getSize());
400  std::cout << "U_diff_abs mean value: " << remap_mean_error << std::endl;
401  const double remap_error_threshold = 0.5;
402  if (remap_mean_error > remap_error_threshold) {
403  std::cerr << "Issue with vpImageTools::remap() with vpRGBa image" << std::endl;
404  return EXIT_FAILURE;
405  }
406 
407  vpImage<unsigned char> U_diff_gray_abs;
408  vpImageTools::imageDifferenceAbsolute(U_gray, U_remap_gray, U_diff_gray_abs);
409  double remap_mean_error_gray = U_diff_gray_abs.getSum() / U_diff_gray_abs.getSize();
410  std::cout << "U_diff_gray_abs mean value: " << remap_mean_error_gray << std::endl;
411  if (remap_mean_error_gray > remap_error_threshold) {
412  std::cerr << "Issue with vpImageTools::remap() with unsigned char image" << std::endl;
413  return EXIT_FAILURE;
414  }
415 
416  // Write the undistorted difference images on the disk
417  vpImage<vpRGBa> U_diff;
418  vpImage<unsigned char> U_diff_gray;
419  vpImageTools::imageDifference(U, U_remap, U_diff);
420  filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_diff_color.png"));
421  std::cout << "\nWrite undistorted image: " << filename << std::endl;
422  vpImageIo::write(U_diff, filename);
423 
424  vpImageTools::imageDifference(U_gray, U_remap_gray, U_diff_gray);
425  filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_diff_gray.png"));
426  std::cout << "Write undistorted image: " << filename << std::endl;
427  vpImageIo::write(U_diff_gray, filename);
428 
429  return EXIT_SUCCESS;
430  }
431  catch (const vpException &e) {
432  std::cout << "Catch an exception: " << e << std::endl;
433  return EXIT_FAILURE;
434  }
435 }
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:125
Generic class defining intrinsic camera parameters.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:143
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:287
static void imageDifference(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height, vpArray2D< int > &mapU, vpArray2D< int > &mapV, vpArray2D< float > &mapDu, vpArray2D< float > &mapDv)
static void resize(const vpImage< Type > &I, vpImage< Type > &Ires, unsigned int width, unsigned int height, const vpImageInterpolationType &method=INTERPOLATION_NEAREST, unsigned int nThreads=0)
static void remap(const vpImage< unsigned char > &I, const vpArray2D< int > &mapU, const vpArray2D< int > &mapV, const vpArray2D< float > &mapDu, const vpArray2D< float > &mapDv, vpImage< unsigned char > &Iundist)
static void undistort(const vpImage< Type > &I, const vpCameraParameters &cam, vpImage< Type > &newI, unsigned int nThreads=2)
Definition: vpImageTools.h:646
static void imageDifferenceAbsolute(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
unsigned int getWidth() const
Definition: vpImage.h:242
double getSum() const
Definition: vpImage.h:1860
unsigned int getSize() const
Definition: vpImage.h:223
unsigned int getHeight() const
Definition: vpImage.h:184
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1786
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:1421
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:818
static std::string getUserName()
Definition: vpIoTools.cpp:711
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:2147
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:972
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
VISP_EXPORT double measureTimeMs()