Visual Servoing Platform  version 3.2.0
calibrate-camera.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  * 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  Settings s;
146  const std::string inputSettingsFile = argc > 1 ? argv[1] : "default.cfg";
147 
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  try {
164  reader.open(I);
165  }
166  catch(const vpException &e) {
167  std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
168  std::cout << "Check if input images name \"" << s.input << "\" set in " << inputSettingsFile << " config file is correct..." << std::endl;
169  return EXIT_FAILURE;
170  }
171 
172 #ifdef VISP_HAVE_X11
173  vpDisplayX d(I);
174 #elif defined VISP_HAVE_GDI
175  vpDisplayGDI d(I);
176 #elif defined VISP_HAVE_GTK
177  vpDisplayGTK d(I);
178 #elif defined VISP_HAVE_OPENCV
179  vpDisplayOpenCV d(I);
180 #endif
181 
182  vpCameraParameters cam;
183 
184  // Initialize camera parameters
185  double px = cam.get_px();
186  double py = cam.get_px();
187  // Set (u0,v0) in the middle of the image
188  double u0 = I.getWidth() / 2;
189  double v0 = I.getHeight() / 2;
190  cam.initPersProjWithoutDistortion(px, py, u0, v0);
191 
192  std::vector<vpPoint> model;
193  std::vector<vpCalibration> calibrator;
194 
195  for (int i = 0; i < s.boardSize.height; i++) {
196  for (int j = 0; j < s.boardSize.width; j++) {
197  model.push_back(vpPoint(j * s.squareSize, i * s.squareSize, 0));
198  }
199  }
200 
201  while (!reader.end()) {
202  reader.acquire(I);
203 
204  long frame_index = reader.getFrameIndex();
206 
207  cv::Mat cvI;
208  std::vector<cv::Point2f> pointBuf;
209  vpImageConvert::convert(I, cvI);
210 
211  bool found = false;
212  switch (s.calibrationPattern) // Find feature points on the input format
213  {
214  case Settings::CHESSBOARD:
215  // std::cout << "Use chessboard " << std::endl;
216  found = findChessboardCorners(cvI, s.boardSize, pointBuf,
217 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
218  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK |
219  cv::CALIB_CB_NORMALIZE_IMAGE);
220 #else
221  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
222  CV_CALIB_CB_NORMALIZE_IMAGE);
223 #endif
224  break;
225  case Settings::CIRCLES_GRID:
226  // std::cout << "Use circle grid " << std::endl;
227  found = findCirclesGrid(cvI, s.boardSize, pointBuf, cv::CALIB_CB_SYMMETRIC_GRID);
228  break;
229  case Settings::UNDEFINED:
230  default:
231  std::cout << "Unkown calibration grid " << std::endl;
232  break;
233  }
234 
235  std::cout << "frame: " << frame_index << ", status: " << found;
236  if (!found)
237  std::cout << ", image rejected" << std::endl;
238  else
239  std::cout << ", image used as input data" << std::endl;
240 
241  if (found) // If done with success,
242  {
243  std::vector<vpImagePoint> data;
244 
245  if (s.calibrationPattern == Settings::CHESSBOARD) {
246  // improve the found corners' coordinate accuracy for chessboard
247  cornerSubPix(cvI, pointBuf, cv::Size(11, 11), cv::Size(-1, -1),
248 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
249  cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
250 #else
251  cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
252 #endif
253  }
254  std::stringstream ss;
255  ss << "image " << frame_index;
256  vpDisplay::setTitle(I, ss.str());
257  for (unsigned int i = 0; i < pointBuf.size(); i++) {
258  vpImagePoint ip(pointBuf[i].y, pointBuf[i].x);
259  data.push_back(ip);
261  }
262 
263  // Calibration on a single mono image
264  vpCalibration calib;
265  calib.setLambda(0.5);
266  calib.clearPoint();
267  for (unsigned int i = 0; i < model.size(); i++) {
268  calib.addPoint(model[i].get_oX(), model[i].get_oY(), model[i].get_oZ(), data[i]);
269  }
271 
272  if (calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS, cMo, cam, false) == EXIT_SUCCESS) {
273  //std::cout << "camera parameters for frame " << frame_index << ": " << cam << std::endl;
274  calibrator.push_back(calib);
275  }
276  }
277 
278  if (found)
279  vpDisplay::displayText(I, 15, 15, "Image processing succeed", vpColor::green);
280  else
281  vpDisplay::displayText(I, 15, 15, "Image processing fails", vpColor::green);
282 
283  if (s.tempo > 10.f) {
284  vpDisplay::displayText(I, 35, 15, "A click to process the next image", vpColor::green);
285  vpDisplay::flush(I);
287  } else {
288  vpDisplay::flush(I);
289  vpTime::wait(s.tempo * 1000);
290  }
291  }
292 
293  // Now we consider the multi image calibration
294  // Calibrate by a non linear method based on virtual visual servoing
295  if (calibrator.empty()) {
296  std::cerr << "Unable to calibrate. Image processing failed !" << std::endl;
297  return 0;
298  }
299 
300  std::stringstream ss_additional_info;
301  ss_additional_info << "<date>" << vpTime::getDateTime() << "</date>";
302  ss_additional_info << "<nb_calibration_images>" << calibrator.size() << "</nb_calibration_images>";
303  ss_additional_info << "<calibration_pattern_type>";
304 
305  switch (s.calibrationPattern) {
306  case Settings::CHESSBOARD:
307  ss_additional_info << "Chessboard";
308  break;
309 
310  case Settings::CIRCLES_GRID:
311  ss_additional_info << "Circles grid";
312  break;
313 
314  case Settings::UNDEFINED:
315  default:
316  ss_additional_info << "Undefined";
317  break;
318  }
319  ss_additional_info << "</calibration_pattern_type>";
320  ss_additional_info << "<board_size>" << s.boardSize.width << "x" << s.boardSize.height << "</board_size>";
321  ss_additional_info << "<square_size>" << s.squareSize << "</square_size>";
322 
323  std::cout << "\nCalibration without distortion in progress on " << calibrator.size() << " images..." << std::endl;
324  double error;
325  if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS, calibrator, cam, error, false) == EXIT_SUCCESS) {
326  std::cout << cam << std::endl;
327  std::cout << "Global reprojection error: " << error << std::endl;
328  ss_additional_info << "<global_reprojection_error><without_distortion>" << error << "</without_distortion>";
329 
330 #ifdef VISP_HAVE_XML2
331  vpXmlParserCamera xml;
332 
333  if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight()) ==
335  std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\""
336  << std::endl;
337  else {
338  std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\""
339  << std::endl;
340  std::cout << "A file with the same name exists. Remove it to be able "
341  "to save the parameters..."
342  << std::endl;
343  }
344 #endif
345  } else {
346  std::cout << "Calibration without distortion failed." << std::endl;
347  return EXIT_FAILURE;
348  }
349 
350  std::cout << "\nCalibration with distortion in progress on " << calibrator.size() << " images..." << std::endl;
352  EXIT_SUCCESS) {
353  std::cout << cam << std::endl;
354  std::cout << "Global reprojection error: " << error << std::endl;
355  ss_additional_info << "<with_distortion>" << error << "</with_distortion></global_reprojection_error>";
356 
357 #ifdef VISP_HAVE_XML2
358  vpXmlParserCamera xml;
359 
360  if (xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight(), ss_additional_info.str()) ==
362  std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\""
363  << std::endl;
364  else {
365  std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\""
366  << std::endl;
367  std::cout << "A file with the same name exists. Remove it to be able "
368  "to save the parameters..."
369  << std::endl;
370  }
371 #endif
372  std::cout << std::endl;
373  for (unsigned int i = 0; i < calibrator.size(); i++)
374  std::cout << "Estimated pose on input data " << i << ": " << vpPoseVector(calibrator[i].cMo_dist).t()
375  << std::endl;
376 
377  } else {
378  std::cout << "Calibration with distortion failed." << std::endl;
379  return EXIT_FAILURE;
380  }
381 
382  return EXIT_SUCCESS;
383  } catch (const vpException &e) {
384  std::cout << "Catch an exception: " << e << std::endl;
385  return EXIT_FAILURE;
386  }
387 }
388 #else
389 int main()
390 {
391  std::cout << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
392  std::cout << "Tip:" << std::endl;
393  std::cout << "- Install OpenCV, configure again ViSP using cmake and build again this example" << std::endl;
394  return EXIT_SUCCESS;
395 }
396 #endif
vpDisplayX
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:150
vpException::getStringMessage
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:91
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:149
vpDisplay::setTitle
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
Definition: vpDisplay_uchar.cpp:1159
vpImageConvert::convert
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition: vpImageConvert.cpp:78
vpCameraParameters
Generic class defining intrinsic camera parameters.
Definition: vpCameraParameters.h:232
vpIoTools::readConfigVar
static bool readConfigVar(const std::string &var, float &value)
Definition: vpIoTools.cpp:1043
vpXmlParserCamera
XML parser to load and save intrinsic camera parameters.
Definition: vpXmlParserCamera.h:187
vpCalibration::computeCalibrationMulti
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
Definition: vpCalibration.cpp:451
vpVideoReader::end
bool end()
Definition: vpVideoReader.h:257
vpDisplayGDI
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
vpTime::getDateTime
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
Definition: vpTime.cpp:344
vpCameraParameters::get_px
double get_px() const
Definition: vpCameraParameters.h:329
vpCalibration::CALIB_VIRTUAL_VS
Definition: vpCalibration.h:79
vpVideoReader::getFrameIndex
long getFrameIndex() const
Definition: vpVideoReader.h:292
vpDisplayOpenCV
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Definition: vpDisplayOpenCV.h:141
vpCalibration::CALIB_VIRTUAL_VS_DIST
Definition: vpCalibration.h:82
vpCalibration::setLambda
static void setLambda(const double &lambda)
set the gain for the virtual visual servoing algorithm
Definition: vpCalibration.h:167
vpVideoReader::open
void open(vpImage< vpRGBa > &I)
Definition: vpVideoReader.cpp:196
vpVideoReader
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
Definition: vpVideoReader.h:170
vpColor::green
static const vpColor green
Definition: vpColor.h:182
vpDisplay::display
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:676
vpDisplayGTK
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
Definition: vpDisplayGTK.h:137
vpDisplay::displayText
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay_uchar.cpp:600
vpIoTools::loadConfigFile
static bool loadConfigFile(const std::string &confFile)
Definition: vpIoTools.cpp:996
vpImage::getHeight
unsigned int getHeight() const
Definition: vpImage.h:177
vpPoseVector
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:91
vpImagePoint
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:87
vpCalibration::computeCalibration
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
Definition: vpCalibration.cpp:355
vpCalibration
Tools for perspective camera calibration.
Definition: vpCalibration.h:70
vpXmlParserCamera::save
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="")
Definition: vpXmlParserCamera.cpp:207
vpDisplay::flush
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay_uchar.cpp:652
vpDisplay::displayCross
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
Definition: vpDisplay_uchar.cpp:179
vpImage< unsigned char >
vpPoint
Class that defines what is a point.
Definition: vpPoint.h:57
vpDisplay::getClick
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
Definition: vpDisplay_uchar.cpp:701
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:91
vpPoseVector::t
vpRowVector t() const
Definition: vpPoseVector.cpp:395
vpCalibration::addPoint
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
Definition: vpCalibration.cpp:143
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpVideoReader::acquire
void acquire(vpImage< vpRGBa > &I)
Definition: vpVideoReader.cpp:265
vpVideoReader::setFileName
void setFileName(const char *filename)
Definition: vpVideoReader.cpp:91
vpColor::red
static const vpColor red
Definition: vpColor.h:179
vpXmlParserCamera::SEQUENCE_OK
Definition: vpXmlParserCamera.h:215
vpCameraParameters::initPersProjWithoutDistortion
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
Definition: vpCameraParameters.cpp:182
vpCalibration::clearPoint
int clearPoint()
Suppress all the point in the array of point.
Definition: vpCalibration.cpp:126
vpImage::getWidth
unsigned int getWidth() const
Definition: vpImage.h:238