ViSP  2.10.0
camera_calibration.cpp
1 /****************************************************************************
2  *
3  * $Id: camera_calibration.cpp 5126 2015-01-05 22:07:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Camera calibration with chessboard or circle calibration grid.
36  *
37  * Authors:
38  * Fabien Spindler
39  *
40  *****************************************************************************/
41 #include <iostream>
42 
43 #include <visp/vpConfig.h>
44 
45 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
46 
47 #include <opencv2/core/core.hpp>
48 #include <opencv2/imgproc/imgproc.hpp>
49 #include <opencv2/calib3d/calib3d.hpp>
50 #include <opencv2/highgui/highgui.hpp>
51 
52 #include <visp/vpCalibration.h>
53 
54 #include <visp/vpDisplayX.h>
55 #include <visp/vpDisplayGDI.h>
56 #include <visp/vpDisplayOpenCV.h>
57 #include <visp/vpDisplayD3D.h>
58 #include <visp/vpDisplayGTK.h>
59 #include <visp/vpIoTools.h>
60 #include <visp/vpPoint.h>
61 #include <visp/vpVideoReader.h>
62 #include <visp/vpXmlParserCamera.h>
63 
64 #ifndef DOXYGEN_SHOULD_SKIP_THIS
65 
66 class Settings
67 {
68 public:
69  Settings()
70  : boardSize(), calibrationPattern(UNDEFINED), squareSize(0.), input(), tempo(0.), goodInput(false), patternToUse()
71  {
72  boardSize = cv::Size(0, 0);
73  calibrationPattern = UNDEFINED;
74  squareSize = 0.025f;
75  goodInput = false;
76  tempo = 1.f;
77 
78  }
79  enum Pattern { UNDEFINED, CHESSBOARD, CIRCLES_GRID};
80 
81  bool read(const std::string &filename) // Read the parameters
82  {
83  // reading configuration file
84  if (! vpIoTools::loadConfigFile(filename) )
85  return false;
86  vpIoTools::readConfigVar("BoardSize_Width:", boardSize.width);
87  vpIoTools::readConfigVar("BoardSize_Height:", boardSize.height);
88  vpIoTools::readConfigVar("Square_Size:", squareSize);
89  vpIoTools::readConfigVar("Calibrate_Pattern:", patternToUse);
90  vpIoTools::readConfigVar("Input:", input);
91  vpIoTools::readConfigVar("Tempo:", tempo);
92 
93  std::cout << "grid width : " << boardSize.width << std::endl;
94  std::cout << "grid height: " << boardSize.height << std::endl;
95  std::cout << "square size: " << squareSize << std::endl;
96  std::cout << "pattern : " << patternToUse << std::endl;
97  std::cout << "input seq : " << input << std::endl;
98  std::cout << "tempo : " << tempo << std::endl;
99  interprate();
100  return true;
101  }
102  void interprate()
103  {
104  goodInput = true;
105  if (boardSize.width <= 0 || boardSize.height <= 0) {
106  std::cerr << "Invalid Board size: " << boardSize.width << " " << boardSize.height << std::endl;
107  goodInput = false;
108  }
109  if (squareSize <= 10e-6) {
110  std::cerr << "Invalid square size " << squareSize << std::endl;
111  goodInput = false;
112  }
113 
114  if (input.empty()) // Check for valid input
115  goodInput = false;
116 
117  calibrationPattern = UNDEFINED;
118  if (patternToUse.compare("CHESSBOARD") == 0) calibrationPattern = CHESSBOARD;
119  else if (patternToUse.compare("CIRCLES_GRID") == 0) calibrationPattern = CIRCLES_GRID;
120  if (calibrationPattern == UNDEFINED) {
121  std::cerr << " Inexistent camera calibration mode: " << patternToUse << std::endl;
122  goodInput = false;
123  }
124  }
125 
126 public:
127  cv::Size boardSize; // The size of the board -> Number of items by width and height
128  Pattern calibrationPattern;// One of the Chessboard, circles, or asymmetric circle pattern
129  float squareSize; // The size of a square in your defined unit (point, millimeter,etc).
130  std::string input; // The input image sequence
131  float tempo; // Tempo in seconds between two images. If > 10 wait a click to continue
132  bool goodInput;
133 
134 private:
135  std::string patternToUse;
136 };
137 #endif
138 
139 int main(int argc, const char ** argv)
140 {
141  try {
142  std::string outputFileName = "camera.xml";
143 
144  Settings s;
145  const std::string inputSettingsFile = argc > 1 ? argv[1] : "default.cfg";
146  if (! s.read(inputSettingsFile) ) {
147  std::cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << std::endl;
148  std::cout << std::endl << "Usage: " << argv[0] << " <configuration file>.cfg" << std::endl;
149  return -1;
150  }
151 
152  if (! s.goodInput)
153  {
154  std::cout << "Invalid input detected. Application stopping. " << std::endl;
155  return -1;
156  }
157 
158  // Start the calibration code
160  vpVideoReader reader;
161  reader.setFileName(s.input);
162  reader.open(I);
163 
164 #ifdef VISP_HAVE_X11
165  vpDisplayX d(I);
166 #elif defined VISP_HAVE_GDI
167  vpDisplayGDI d(I);
168 #elif defined VISP_HAVE_GTK
169  vpDisplayGTK d(I);
170 #elif defined VISP_HAVE_OPENCV
171  vpDisplayOpenCV d(I);
172 #endif
173 
174  std::vector<vpPoint> model;
175  std::vector<vpCalibration> calibrator;
176 
177  for (int i=0; i< s.boardSize.height; i++) {
178  for (int j=0; j< s.boardSize.width; j++) {
179  vpPoint P;
180  P.setWorldCoordinates(j*s.squareSize, i*s.squareSize, 0);
181  model.push_back(P);
182  }
183  }
184 
185  long frame_index;
186  while(! reader.end()) {
187  frame_index = reader.getFrameIndex();
188  reader.acquire(I);
190 
191  cv::Mat cvI;
192  std::vector<cv::Point2f> pointBuf;
193  vpImageConvert::convert(I, cvI);
194 
195  bool found = false;
196  switch( s.calibrationPattern ) // Find feature points on the input format
197  {
198  case Settings::CHESSBOARD:
199  //std::cout << "Use chessboard " << std::endl;
200  found = findChessboardCorners( cvI, s.boardSize, pointBuf,
201 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
202  cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
203 #else
204  CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | 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),
230  cv::Size(-1,-1),
231 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
232  cv::TermCriteria( cv::TermCriteria::EPS+cv::TermCriteria::COUNT, 30, 0.1 ));
233 #else
234  cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
235 #endif
236  }
237  char title[20]; sprintf(title, "image %ld", frame_index);
238  vpDisplay::setTitle(I, title);
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  }
278  else {
279  vpDisplay::flush(I);
280  vpTime::wait(s.tempo*1000);
281  }
282  }
283 
284  // Now we consider the multi image calibration
285  // Calibrate by a non linear method based on virtual visual servoing
286  if (calibrator.empty()) {
287  std::cerr << "Unable to calibrate. Image processing failed !" << std::endl;
288  return 0;
289  }
290 
291  std::cout << "\nCalibration without distorsion in progress on " << calibrator.size() << " images..." << std::endl;
292  vpCameraParameters cam;
293  double error;
294  if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS, calibrator, cam, error, false) == 0) {
295  std::cout << cam << std::endl;
296  std::cout << "Global reprojection error: " << error << std::endl;
297 #ifdef VISP_HAVE_XML2
298  vpXmlParserCamera xml;
299 
300  if(xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight()) == vpXmlParserCamera::SEQUENCE_OK)
301  std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\"" << std::endl;
302  else {
303  std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\"" << std::endl;
304  std::cout << "A file with the same name exists. Remove it to be able to save the parameters..." << std::endl;
305  }
306 #endif
307  }
308  else
309  std::cout << "Calibration without distortion failed." << std::endl;
310 
311  std::cout << "\nCalibration with distorsion in progress on " << calibrator.size() << " images..." << std::endl;
312  if (vpCalibration::computeCalibrationMulti(vpCalibration::CALIB_VIRTUAL_VS_DIST, calibrator, cam, error, false) == 0) {
313  std::cout << cam << std::endl;
314  std::cout << "Global reprojection error: " << error << std::endl;
315 
316 #ifdef VISP_HAVE_XML2
317  vpXmlParserCamera xml;
318 
319  if(xml.save(cam, outputFileName.c_str(), "Camera", I.getWidth(), I.getHeight()) == vpXmlParserCamera::SEQUENCE_OK)
320  std::cout << "Camera parameters without distortion successfully saved in \"" << outputFileName << "\"" << std::endl;
321  else {
322  std::cout << "Failed to save the camera parameters without distortion in \"" << outputFileName << "\"" << std::endl;
323  std::cout << "A file with the same name exists. Remove it to be able to save the parameters..." << std::endl;
324  }
325 #endif
326  std::cout << std::endl;
327  for (unsigned int i=0; i<calibrator.size(); i++)
328  std::cout << "Estimated pose on input data " << i << ": " << vpPoseVector(calibrator[i].cMo_dist).t() << std::endl;
329 
330  }
331  else
332  std::cout << "Calibration with distortion failed." << std::endl;
333 
334  return 0;
335  }
336  catch(vpException e) {
337  std::cout << "Catch an exception: " << e << std::endl;
338  return 1;
339  }
340 }
341 #else
342 int main()
343 {
344  std::cout << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
345 }
346 #endif
long getFrameIndex() const
unsigned int getWidth() const
Definition: vpImage.h:161
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est, bool verbose=false)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
Definition: vpDisplay.cpp:887
Define the X11 console to display images.
Definition: vpDisplayX.h:152
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:76
int addPoint(double X, double Y, double Z, vpImagePoint &ip)
Tools for perspective camera calibration.
Definition: vpCalibration.h:78
static int wait(double t0, double t)
Definition: vpTime.cpp:149
XML parser to load and save intrinsic camera parameters.
static const vpColor green
Definition: vpColor.h:170
static void flush(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:2232
static const vpColor red
Definition: vpColor.h:167
Class that defines what is a point.
Definition: vpPoint.h:65
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 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)
int clearPoint()
Suppress all the point in the array of point.
static void display(const vpImage< unsigned char > &I)
Definition: vpDisplay.cpp:210
The vpDisplayOpenCV allows to display image using the opencv library.
vpRowVector t() const
Transpose of a vector.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
Generic class defining intrinsic camera parameters.
virtual void setTitle(const char *title)=0
The vpDisplayGTK allows to display image using the GTK+ library version 1.2.
Definition: vpDisplayGTK.h:145
void acquire(vpImage< vpRGBa > &I)
void setFileName(const char *filename)
double get_px() const
The pose is a complete representation of every rigid motion in the euclidian space.
Definition: vpPoseVector.h:92
unsigned int getHeight() const
Definition: vpImage.h:152
virtual bool getClick(bool blocking=true)=0
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:93
static bool readConfigVar(const std::string &var, float &value)
Definition: vpIoTools.cpp:791
static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector< vpCalibration > &table_cal, vpCameraParameters &cam, double &globalReprojectionError, bool verbose=false)
static bool loadConfigFile(const std::string &confFile)
Definition: vpIoTools.cpp:740
void setWorldCoordinates(const double ox, const double oy, const double oz)
Set the point world coordinates. We mean here the coordinates of the point in the object frame...
Definition: vpPoint.cpp:74