Visual Servoing Platform version 3.5.0
testUndistortImage.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Authors:
35 * Anthony Saunier
36 *
37 *****************************************************************************/
38
39#include <stdlib.h>
40#include <visp3/core/vpDebug.h>
41#include <visp3/core/vpImage.h>
42#include <visp3/core/vpImageTools.h>
43#include <visp3/core/vpIoTools.h>
44#include <visp3/core/vpRGBa.h>
45#include <visp3/core/vpTime.h>
46#include <visp3/io/vpImageIo.h>
47#include <visp3/io/vpParseArgv.h>
58// List of allowed command line options
59#define GETOPTARGS "cdi:o:t:s:h"
60
61/*
62 Print the program options.
63
64 \param name : Program name.
65 \param badparam : Bad parameter name.
66 \param ipath: Input image path.
67 \param opath : Output image path.
68 \param user : Username.
69 */
70void usage(const char *name, const char *badparam, const std::string &ipath, const std::string &opath,
71 const std::string &user)
72{
73 fprintf(stdout, "\n\
74Read an image from the disk, undistort it \n\
75and save the undistorted image on the disk.\n\
76(grid36-01_undistorted.pgm).\n\
77\n\
78SYNOPSIS\n\
79 %s [-i <input image path>] [-o <output image path>] [-t <nThreads>] [-s <scale>]\n\
80 [-h]\n\
81 ", name);
82
83 fprintf(stdout, "\n\
84OPTIONS: Default\n\
85 -i <input image path> %s\n\
86 Set image input path.\n\
87 From this path read \"calibration/grid36-01.pgm\"\n\
88 image.\n\
89 Setting the VISP_INPUT_IMAGE_PATH environment\n\
90 variable produces the same behaviour than using\n\
91 this option.\n\
92 \n\
93 -o <output image path> %s\n\
94 Set image output path.\n\
95 From this directory, creates the \"%s\"\n\
96 subdirectory depending on the username, where \n\
97 grid36-01_undistorted.pgm output image is written.\n\
98\n\
99 -t <nThreads> \n\
100 Set the number of threads to use for vpImageTools::undistort().\n\
101\n\
102 -s <scale> \n\
103 Resize the image by the specified scale factor.\n\
104\n\
105 -h\n\
106 Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
107
108 if (badparam)
109 fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
110}
111
124bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath,
125 const std::string &user, unsigned int &nThreads, unsigned int &scale)
126{
127 const char *optarg_;
128 int c;
129 while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
130
131 switch (c) {
132 case 'i':
133 ipath = optarg_;
134 break;
135 case 'o':
136 opath = optarg_;
137 break;
138 case 't':
139 nThreads = atoi(optarg_);
140 break;
141 case 's':
142 scale = atoi(optarg_);
143 break;
144 case 'h':
145 usage(argv[0], NULL, ipath, opath, user);
146 return false;
147
148 case 'c':
149 case 'd':
150 break;
151
152 default:
153 usage(argv[0], optarg_, ipath, opath, user);
154 return false;
155 }
156 }
157
158 if ((c == 1) || (c == -1)) {
159 // standalone param or error
160 usage(argv[0], NULL, ipath, opath, user);
161 std::cerr << "ERROR: " << std::endl;
162 std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
163 return false;
164 }
165
166 return true;
167}
168
169int main(int argc, const char **argv)
170{
171 try {
172 std::string env_ipath;
173 std::string opt_ipath;
174 std::string opt_opath;
175 std::string ipath;
176 std::string opath;
177 std::string filename;
178 std::string username;
179 unsigned int nThreads = 2;
180 unsigned int scale = 1;
181
182 // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
183 // environment variable value
185
186 // Set the default input path
187 if (!env_ipath.empty())
188 ipath = env_ipath;
189
190// Set the default output path
191#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
192 opt_opath = "/tmp";
193#elif defined(_WIN32)
194 opt_opath = "C:\\temp";
195#endif
196
197 // Get the user login name
198 vpIoTools::getUserName(username);
199
200 // Read the command line options
201 if (getOptions(argc, argv, opt_ipath, opt_opath, username, nThreads, scale) == false) {
202 exit(-1);
203 }
204
205 // Get the option values
206 if (!opt_ipath.empty())
207 ipath = opt_ipath;
208 if (!opt_opath.empty())
209 opath = opt_opath;
210
211 // Append to the output path string, the login name of the user
212 opath = vpIoTools::createFilePath(opath, username);
213
214 // Test if the output path exist. If no try to create it
215 if (vpIoTools::checkDirectory(opath) == false) {
216 try {
217 // Create the dirname
219 } catch (...) {
220 usage(argv[0], NULL, ipath, opt_opath, username);
221 std::cerr << std::endl << "ERROR:" << std::endl;
222 std::cerr << " Cannot create " << opath << std::endl;
223 std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
224 exit(-1);
225 }
226 }
227
228 // Compare ipath and env_ipath. If they differ, we take into account
229 // the input path comming from the command line option
230 if (opt_ipath.empty()) {
231 if (ipath != env_ipath) {
232 std::cout << std::endl << "WARNING: " << std::endl;
233 std::cout << " Since -i <visp image path=" << ipath << "> "
234 << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
235 << " we skip the environment variable." << std::endl;
236 }
237 }
238
239 // Test if an input path is set
240 if (opt_ipath.empty() && env_ipath.empty()) {
241 usage(argv[0], NULL, ipath, opt_opath, username);
242 std::cerr << std::endl << "ERROR:" << std::endl;
243 std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
244 << " environment variable to specify the location of the " << std::endl
245 << " image path where test images are located." << std::endl
246 << std::endl;
247 exit(-1);
248 }
249
250 //
251 // Here starts really the test
252 //
253 vpImage<vpRGBa> I, I_; // Input image
255 vpImage<vpRGBa> U; // undistorted output image
256 vpImage<unsigned char> U_gray; // undistorted output image
257 vpImage<vpRGBa> U_remap; // undistorted output image
258 vpImage<unsigned char> U_remap_gray; // undistorted output image
259
261 cam.initPersProjWithDistortion(600, 600, 320, 240, -0.17, 0.17);
262 // Read the input grey image from the disk
263 filename = vpIoTools::createFilePath(ipath, "calibration/grid36-01.pgm");
264 std::cout << "Read image: " << filename << std::endl;
265 vpImageIo::read(I_, filename);
266 if (scale > 1) {
267 std::cout << "Scale the image by a factor of " << scale << std::endl;
268 vpImageTools::resize(I_, I, I_.getWidth()*scale, I_.getHeight()*scale);
269 } else {
270 I = I_;
271 }
272 std::cout << "Input image: " << I.getWidth() << "x" << I.getHeight() << std::endl;
273 vpImageConvert::convert(I, I_gray);
274
275 std::cout << "Nb threads to use for vpImageTools::undistort(): " << nThreads << std::endl;
276
277 double t_undistort = 0.0, t_remap = 0.0;
278 {
279 std::cout << "\nUndistortion in process (color image)... " << std::endl;
280 vpImageTools::undistort(I, cam, U, nThreads);
281
282 double begintime = vpTime::measureTimeMs();
283
284 // For the test, to have a significant time measure we repeat the
285 // undistortion 10 times
286 for (unsigned int i = 0; i < 10; i++)
287 // Create the undistorted image
288 vpImageTools::undistort(I, cam, U, nThreads);
289
290 double endtime = vpTime::measureTimeMs();
291 t_undistort = endtime - begintime;
292
293 std::cout << "Time for 10 color image undistortion (ms): " << t_undistort << std::endl;
294 }
295
296 {
297 std::cout << "Undistortion in process with remap (color image)... " << std::endl;
298
299 double begintime = vpTime::measureTimeMs();
300
301 // For the test, to have a significant time measure we repeat the
302 // undistortion 10 times
303 vpArray2D<int> mapU, mapV;
304 vpArray2D<float> mapDu, mapDv;
305 for (unsigned int i = 0; i < 10; i++) {
306 if (i == 0) {
307 vpImageTools::initUndistortMap(cam, I.getWidth(), I.getHeight(), mapU, mapV, mapDu, mapDv);
308 }
309 vpImageTools::remap(I, mapU, mapV, mapDu, mapDv, U_remap);
310 }
311
312 double endtime = vpTime::measureTimeMs();
313 t_remap = endtime - begintime;
314
315 std::cout << "Time for 10 color image undistortion with remap (ms): " << t_remap << std::endl;
316 std::cout << "Speed-up: " << t_undistort / t_remap << "X" << std::endl;
317 }
318
319 {
320 std::cout << "\nUndistortion in process (gray image)... " << std::endl;
321 vpImageTools::undistort(I_gray, cam, U_gray, nThreads);
322
323 double begintime = vpTime::measureTimeMs();
324
325 // For the test, to have a significant time measure we repeat the
326 // undistortion 100 times
327 for (unsigned int i = 0; i < 100; i++)
328 // Create the undistorted image
329 vpImageTools::undistort(I_gray, cam, U_gray, nThreads);
330
331 double endtime = vpTime::measureTimeMs();
332 t_undistort = endtime - begintime;
333
334 std::cout << "Time for 100 gray image undistortion (ms): " << t_undistort << std::endl;
335 }
336
337 {
338 std::cout << "Undistortion in process with remap (gray image)... " << std::endl;
339
340 double begintime = vpTime::measureTimeMs();
341
342 // For the test, to have a significant time measure we repeat the
343 // undistortion 100 times
344 vpArray2D<int> mapU, mapV;
345 vpArray2D<float> mapDu, mapDv;
346 for (unsigned int i = 0; i < 10; i++) {
347 if (i == 0) {
348 vpImageTools::initUndistortMap(cam, I.getWidth(), I.getHeight(), mapU, mapV, mapDu, mapDv);
349 }
350 vpImageTools::remap(I_gray, mapU, mapV, mapDu, mapDv, U_remap_gray);
351 }
352
353 double endtime = vpTime::measureTimeMs();
354 t_remap = endtime - begintime;
355
356 std::cout << "Time for 100 gray image undistortion with remap (ms): " << t_remap << std::endl;
357 std::cout << "Speed-up: " << t_undistort / t_remap << "X" << std::endl;
358 }
359
360 // Write the undistorted images on the disk
361 filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted.ppm"));
362 std::cout << "\nWrite undistorted image: " << filename << std::endl;
363 vpImageIo::write(U, filename);
364
365 filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted.pgm"));
366 std::cout << "Write undistorted image: " << filename << std::endl;
367 vpImageIo::write(U_gray, filename);
368
369 filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_remap.ppm"));
370 std::cout << "\nWrite undistorted image with remap: " << filename << std::endl;
371 vpImageIo::write(U_remap, filename);
372
373 filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_remap.pgm"));
374 std::cout << "Write undistorted image with remap: " << filename << std::endl;
375 vpImageIo::write(U_remap_gray, filename);
376
377 // Compute images difference
378 vpImage<vpRGBa> U_diff_abs;
379 vpImageTools::imageDifferenceAbsolute(U, U_remap, U_diff_abs);
380 double mean_diff = 0.0;
381 for (unsigned int i = 0; i < U_diff_abs.getHeight(); i++) {
382 for (unsigned int j = 0; j < U_diff_abs.getWidth(); j++) {
383 mean_diff += U_diff_abs[i][j].R;
384 mean_diff += U_diff_abs[i][j].G;
385 mean_diff += U_diff_abs[i][j].B;
386 mean_diff += U_diff_abs[i][j].A;
387 }
388 }
389 double remap_mean_error = mean_diff / (4*U_diff_abs.getSize());
390 std::cout << "U_diff_abs mean value: " << remap_mean_error << std::endl;
391 const double remap_error_threshold = 0.5;
392 if (remap_mean_error > remap_error_threshold) {
393 std::cerr << "Issue with vpImageTools::remap() with vpRGBa image" << std::endl;
394 return 1;
395 }
396
397 vpImage<unsigned char> U_diff_gray_abs;
398 vpImageTools::imageDifferenceAbsolute(U_gray, U_remap_gray, U_diff_gray_abs);
399 double remap_mean_error_gray = U_diff_gray_abs.getSum() / U_diff_gray_abs.getSize();
400 std::cout << "U_diff_gray_abs mean value: " << remap_mean_error_gray << std::endl;
401 if (remap_mean_error_gray > remap_error_threshold) {
402 std::cerr << "Issue with vpImageTools::remap() with unsigned char image" << std::endl;
403 return 1;
404 }
405
406 // Write the undistorted difference images on the disk
407 vpImage<vpRGBa> U_diff;
408 vpImage<unsigned char> U_diff_gray;
409 vpImageTools::imageDifference(U, U_remap, U_diff);
410 filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_diff.ppm"));
411 std::cout << "\nWrite undistorted image: " << filename << std::endl;
412 vpImageIo::write(U_diff, filename);
413
414 vpImageTools::imageDifference(U_gray, U_remap_gray, U_diff_gray);
415 filename = vpIoTools::path(vpIoTools::createFilePath(opath, "grid36-01_undistorted_diff.pgm"));
416 std::cout << "Write undistorted image: " << filename << std::endl;
417 vpImageIo::write(U_diff_gray, filename);
418
419 return 0;
420 } catch (const vpException &e) {
421 std::cout << "Catch an exception: " << e << std::endl;
422 return 1;
423 }
424}
Implementation of a generic 2D array used as base class for matrices and vectors.
Definition: vpArray2D.h:132
Generic class defining intrinsic camera parameters.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emited by ViSP classes.
Definition: vpException.h:72
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:149
static void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:293
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:645
static void imageDifferenceAbsolute(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, vpImage< unsigned char > &Idiff)
double getSum() const
Definition: vpImage.h:1579
unsigned int getWidth() const
Definition: vpImage.h:246
unsigned int getSize() const
Definition: vpImage.h:227
unsigned int getHeight() const
Definition: vpImage.h:188
static std::string getViSPImagesDataPath()
Definition: vpIoTools.cpp:1365
static std::string path(const std::string &pathname)
Definition: vpIoTools.cpp:1005
static bool checkDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:420
static std::string getUserName()
Definition: vpIoTools.cpp:316
static std::string createFilePath(const std::string &parent, const std::string &child)
Definition: vpIoTools.cpp:1670
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:570
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:69
VISP_EXPORT double measureTimeMs()