Visual Servoing Platform  version 3.1.0
camera_calibration.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 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  * Camera calibration with chessboard or circle calibration grid.
33  *
34  * Authors:
35  * Fabien Spindler
36  *
37  *****************************************************************************/
38 #include <iostream>
39 
40 #include <visp3/core/vpConfig.h>
41 
42 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
43 
44 #include <opencv2/calib3d/calib3d.hpp>
45 #include <opencv2/core/core.hpp>
46 #include <opencv2/highgui/highgui.hpp>
47 #include <opencv2/imgproc/imgproc.hpp>
48 
49 #include <visp3/vision/vpCalibration.h>
50 
51 #include <visp3/core/vpIoTools.h>
52 #include <visp3/core/vpPoint.h>
53 #include <visp3/core/vpXmlParserCamera.h>
54 #include <visp3/gui/vpDisplayD3D.h>
55 #include <visp3/gui/vpDisplayGDI.h>
56 #include <visp3/gui/vpDisplayGTK.h>
57 #include <visp3/gui/vpDisplayOpenCV.h>
58 #include <visp3/gui/vpDisplayX.h>
59 #include <visp3/io/vpVideoReader.h>
60 
61 #ifndef DOXYGEN_SHOULD_SKIP_THIS
62 
63 class Settings
64 {
65 public:
66  Settings()
67  : boardSize(), calibrationPattern(UNDEFINED), squareSize(0.), input(), tempo(0.), goodInput(false), patternToUse()
68  {
69  boardSize = cv::Size(0, 0);
70  calibrationPattern = UNDEFINED;
71  squareSize = 0.025f;
72  goodInput = false;
73  tempo = 1.f;
74  }
75  enum Pattern { UNDEFINED, CHESSBOARD, CIRCLES_GRID };
76 
77  bool read(const std::string &filename) // Read the parameters
78  {
79  // reading configuration file
80  if (!vpIoTools::loadConfigFile(filename))
81  return false;
82  vpIoTools::readConfigVar("BoardSize_Width:", boardSize.width);
83  vpIoTools::readConfigVar("BoardSize_Height:", boardSize.height);
84  vpIoTools::readConfigVar("Square_Size:", squareSize);
85  vpIoTools::readConfigVar("Calibrate_Pattern:", patternToUse);
86  vpIoTools::readConfigVar("Input:", input);
87  vpIoTools::readConfigVar("Tempo:", tempo);
88 
89  std::cout << "grid width : " << boardSize.width << std::endl;
90  std::cout << "grid height: " << boardSize.height << std::endl;
91  std::cout << "square size: " << squareSize << std::endl;
92  std::cout << "pattern : " << patternToUse << std::endl;
93  std::cout << "input seq : " << input << std::endl;
94  std::cout << "tempo : " << tempo << std::endl;
95  interprate();
96  return true;
97  }
98  void interprate()
99  {
100  goodInput = true;
101  if (boardSize.width <= 0 || boardSize.height <= 0) {
102  std::cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << std::endl;
103  goodInput = false;
104  }
105  if (squareSize <= 10e-6) {
106  std::cerr << "Invalid square size " << squareSize << std::endl;
107  goodInput = false;
108  }
109 
110  if (input.empty()) // Check for valid input
111  goodInput = false;
112 
113  calibrationPattern = UNDEFINED;
114  if (patternToUse.compare("CHESSBOARD") == 0)
115  calibrationPattern = CHESSBOARD;
116  else if (patternToUse.compare("CIRCLES_GRID") == 0)
117  calibrationPattern = CIRCLES_GRID;
118  if (calibrationPattern == UNDEFINED) {
119  std::cerr << " Inexistent camera calibration mode: " << patternToUse << std::endl;
120  goodInput = false;
121  }
122  }
123 
124 public:
125  cv::Size boardSize; // The size of the board -> Number of items by width and
126  // height
127  Pattern calibrationPattern; // One of the Chessboard, circles, or asymmetric
128  // circle pattern
129  float squareSize; // The size of a square in your defined unit (point,
130  // millimeter,etc).
131  std::string input; // The input image sequence
132  float tempo; // Tempo in seconds between two images. If > 10 wait a click to
133  // continue
134  bool goodInput;
135 
136 private:
137  std::string patternToUse;
138 };
139 #endif
140 
141 int main(int argc, const char **argv)
142 {
143  try {
144  std::string outputFileName = "camera.xml";
145 
146  Settings s;
147  const std::string inputSettingsFile = argc > 1 ? argv[1] : "default.cfg";
148  if (!s.read(inputSettingsFile)) {
149  std::cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << std::endl;
150  std::cout << std::endl << "Usage: " << argv[0] << " <configuration file>.cfg" << std::endl;
151  return -1;
152  }
153 
154  if (!s.goodInput) {
155  std::cout << "Invalid input detected. Application stopping. " << std::endl;
156  return -1;
157  }
158 
159  // Start the calibration code
161  vpVideoReader reader;
162  reader.setFileName(s.input);
163  reader.open(I);
164 
165 #ifdef VISP_HAVE_X11
166  vpDisplayX d(I);
167 #elif defined VISP_HAVE_GDI
168  vpDisplayGDI d(I);
169 #elif defined VISP_HAVE_GTK
170  vpDisplayGTK d(I);
171 #elif defined VISP_HAVE_OPENCV
172  vpDisplayOpenCV d(I);
173 #endif
174 
175  std::vector<vpPoint> model;
176  std::vector<vpCalibration> calibrator;
177 
178  for (int i = 0; i < s.boardSize.height; i++) {
179  for (int j = 0; j < s.boardSize.width; j++) {
180  model.push_back(vpPoint(j * s.squareSize, i * s.squareSize, 0));
181  }
182  }
183 
184  while (!reader.end()) {
185  reader.acquire(I);
186  long frame_index = reader.getFrameIndex();
188 
189  cv::Mat cvI;
190  std::vector<cv::Point2f> pointBuf;
191  vpImageConvert::convert(I, cvI);
192 
193  bool found = false;
194  switch (s.calibrationPattern) // Find feature points on the input format
195  {
196  case Settings::CHESSBOARD:
197  // std::cout << "Use chessboard " << std::endl;
198  found = findChessboardCorners(cvI, s.boardSize, pointBuf,
199 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
200  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK |
201  cv::CALIB_CB_NORMALIZE_IMAGE);
202 #else
203  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
204  CV_CALIB_CB_NORMALIZE_IMAGE);
205 #endif
206  break;
207  case Settings::CIRCLES_GRID:
208  // std::cout << "Use circle grid " << std::endl;
209  found = findCirclesGrid(cvI, s.boardSize, pointBuf, cv::CALIB_CB_SYMMETRIC_GRID);
210  break;
211  case Settings::UNDEFINED:
212  default:
213  std::cout << "Unkown calibration grid " << std::endl;
214  break;
215  }
216 
217  std::cout << "frame: " << frame_index << ", status: " << found;
218  if (!found)
219  std::cout << ", image rejected" << std::endl;
220  else
221  std::cout << ", image used as input data" << std::endl;
222 
223  if (found) // If done with success,
224  {
225  std::vector<vpImagePoint> data;
226 
227  if (s.calibrationPattern == Settings::CHESSBOARD) {
228  // improve the found corners' coordinate accuracy for chessboard
229  cornerSubPix(cvI, pointBuf, cv::Size(11, 11), cv::Size(-1, -1),
230 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
231  cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
232 #else
233  cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
234 #endif
235  }
236  std::stringstream ss;
237  ss << "image " << frame_index;
238  vpDisplay::setTitle(I, ss.str());
239  for (unsigned int i = 0; i < pointBuf.size(); i++) {
240  vpImagePoint ip(pointBuf[i].y, pointBuf[i].x);
241  data.push_back(ip);
243  }
244 
245  // Calibration on a single mono image
246  vpCalibration calib;
247  calib.setLambda(0.5);
248  calib.clearPoint();
249  for (unsigned int i = 0; i < model.size(); i++) {
250  calib.addPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ(), data[i]);
251  }
253  vpCameraParameters cam;
254 
255  // Set (u0,v0) in the middle of the image
256  double px = cam.get_px();
257  double py = cam.get_px();
258  double u0 = I.getWidth() / 2;
259  double v0 = I.getHeight() / 2;
260  cam.initPersProjWithoutDistortion(px, py, u0, v0);
261 
262  if (calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS, cMo, cam, false) == 0) {
263  // std::cout << "camera parameters: " << cam << std::endl;
264  calibrator.push_back(calib);
265  }
266  }
267 
268  if (found)
269  vpDisplay::displayText(I, 15, 15, "Image processing succeed", vpColor::green);
270  else
271  vpDisplay::displayText(I, 15, 15, "Image processing fails", vpColor::green);
272 
273  if (s.tempo > 10.f) {
274  vpDisplay::displayText(I, 35, 15, "A click to process the next image", vpColor::green);
275  vpDisplay::flush(I);
277  } else {
278  vpDisplay::flush(I);
279  vpTime::wait(s.tempo * 1000);
280  }
281  }
282 
283  // Now we consider the multi image calibration
284  // Calibrate by a non linear method based on virtual visual servoing
285  if (calibrator.empty()) {
286  std::cerr << "Unable to calibrate. Image processing failed !" << std::endl;
287  return 0;
288  }
289 
290  std::stringstream ss_additional_info;
291  ss_additional_info << "<date>" << vpTime::getDateTime() << "</date>";
292  ss_additional_info << "<nb_calibration_images>" << calibrator.size() << "</nb_calibration_images>";
293  ss_additional_info << "<calibration_pattern_type>";
294 
295  switch (s.calibrationPattern) {
296  case Settings::CHESSBOARD:
297  ss_additional_info << "Chessboard";
298  break;
299 
300  case Settings::CIRCLES_GRID:
301  ss_additional_info << "Circles grid";
302  break;
303 
304  case Settings::UNDEFINED:
305  default:
306  ss_additional_info << "Undefined";
307  break;
308  }
309  ss_additional_info << "</calibration_pattern_type>";
310  ss_additional_info << "<board_size>" << s.boardSize.width << "x" << s.boardSize.height << "</board_size>";
311  ss_additional_info << "<square_size>" << s.squareSize << "</square_size>";
312 
313  std::cout << "\nCalibration without distortion in progress on " << calibrator.size() << " images..." << std::endl;
314  vpCameraParameters cam;
315  double error;
316  if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS, calibrator, cam, error, false) == 0) {
317  std::cout << cam << std::endl;
318  std::cout << "Global reprojection error: " << error << std::endl;
319  ss_additional_info << "<global_reprojection_error><without_distortion>" << error << "</without_distortion>";
320 
321 #ifdef VISP_HAVE_XML2
322  vpXmlParserCamera xml;
323 
324  if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight()) ==
326  std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\""
327  << std::endl;
328  else {
329  std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\""
330  << std::endl;
331  std::cout << "A file with the same name exists. Remove it to be able "
332  "to save the parameters..."
333  << std::endl;
334  }
335 #endif
336  } else
337  std::cout << "Calibration without distortion failed." << std::endl;
338 
339  std::cout << "\nCalibration with distortion in progress on " << calibrator.size() << " images..." << std::endl;
341  0) {
342  std::cout << cam << std::endl;
343  std::cout << "Global reprojection error: " << error << std::endl;
344  ss_additional_info << "<with_distortion>" << error << "</with_distortion></global_reprojection_error>";
345 
346 #ifdef VISP_HAVE_XML2
347  vpXmlParserCamera xml;
348 
349  if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight(), ss_additional_info.str()) ==
351  std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\""
352  << std::endl;
353  else {
354  std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\""
355  << std::endl;
356  std::cout << "A file with the same name exists. Remove it to be able "
357  "to save the parameters..."
358  << std::endl;
359  }
360 #endif
361  std::cout << std::endl;
362  for (unsigned int i = 0; i < calibrator.size(); i++)
363  std::cout << "Estimated pose on input data " << i << ": " << vpPoseVector(calibrator[i].cMo_dist).t()
364  << std::endl;
365 
366  } else
367  std::cout << "Calibration with distortion failed." << std::endl;
368 
369  return 0;
370  } catch (vpException &e) {
371  std::cout << "Catch an exception: " << e << std::endl;
372  return 1;
373  }
374 }
375 #else
376 int main() { std::cout << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl; }
377 #endif
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:150
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:151
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
error that can be emited by ViSP classes.
Definition: vpException.h:71
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
Tools for perspective camera calibration.
Definition: vpCalibration.h:71
XML parser to load and save intrinsic camera parameters.
static const vpColor green
Definition: vpColor.h:183
static void flush(const vpImage< unsigned char > &I)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
Definition: vpTime.cpp:345
static const vpColor red
Definition: vpColor.h:180
Class that defines what is a point.
Definition: vpPoint.h:58
static void setLambda(const double &lambda)
set the gain for the virtual visual servoing algorithm
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
void open(vpImage< vpRGBa > &I)
int clearPoint()
Suppress all the point in the array of point.
static void display(const vpImage< unsigned char > &I)
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Generic class defining intrinsic camera parameters.
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:138
void acquire(vpImage< vpRGBa > &I)
void setFileName(const char *filename)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
long getFrameIndex() const
unsigned int getHeight() const
Definition: vpImage.h:178
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:92
vpRowVector t() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
static bool readConfigVar(const std::string &var, float &value)
Definition: vpIoTools.cpp:936
int save(const vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const unsigned int image_width=0, const unsigned int image_height=0, const std::string &additionalInfo="")
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
unsigned int getWidth() const
Definition: vpImage.h:229
static bool loadConfigFile(const std::string &confFile)
Definition: vpIoTools.cpp:889