niftkNiftyCalVideoCalibrationManager.h 15.7 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14
/*=============================================================================

  NifTK: A software platform for medical image computing.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

15 16
#ifndef niftkNiftyCalVideoCalibrationManager_h
#define niftkNiftyCalVideoCalibrationManager_h
17 18 19 20 21 22

#include "niftkNiftyCalExports.h"
#include <itkObject.h>
#include <itkObjectFactoryBase.h>
#include <mitkDataStorage.h>
#include <mitkDataNode.h>
23
#include <mitkPointSet.h>
24
#include <niftkPointUtilities.h>
25
#include <niftkIPoint2DDetector.h>
26 27
#include <cv.h>
#include <list>
28

29 30
namespace niftk
{
31 32 33 34 35

/**
 * \class NiftyCalVideoCalibrationManager
 * \brief Manager class to perform video calibration as provided by NiftyCal.
 *
36
 * Note: This class is stateful and not thread safe.
37
 *
38 39 40 41
 * This class was originally intended to work with Zhang's method, which requires
 * N (typically 5-10) views of a calibration pattern. However, it was modified to
 * work with Tsai's method (1 view, coplanar or non-coplanar). So, there may
 * be some residual technical debt to tidy up and simplify.
42 43 44 45 46
 */
class NIFTKNIFTYCAL_EXPORT NiftyCalVideoCalibrationManager : public itk::Object
{
public:

47 48
  enum CalibrationPatterns
  {
49
    // Order must match that in niftk::CameraCalViewPreferencePage
50
    CHESS_BOARD,
51
    CIRCLE_GRID,
52 53
    APRIL_TAGS,
    TEMPLATE_MATCHING_CIRCLES,
54
    TEMPLATE_MATCHING_RINGS,
55 56
    TEMPLATE_MATCHING_NON_COPLANAR_CIRCLES,
    TEMPLATE_MATCHING_NON_COPLANAR_RINGS
57 58 59 60
  };

  enum HandEyeMethod
  {
61
    // Order must match that in niftk::CameraCalViewPreferencePage
62
    TSAI_1989,
63
    SHAHIDI_2002,
64 65
    MALTI_2013,
    NON_LINEAR_EXTRINSIC
66 67
  };

68
  const static bool                DefaultDoIterative;
69
  const static bool                DefaultDo3DOptimisation;
70
  const static bool                DefaultDoClustering;
71
  const static unsigned int        DefaultNumberOfSnapshotsForCalibrating;
72 73
  const static double              DefaultScaleFactorX;
  const static double              DefaultScaleFactorY;
74 75
  const static unsigned int        DefaultGridSizeX;
  const static unsigned int        DefaultGridSizeY;
76 77 78
  const static CalibrationPatterns DefaultCalibrationPattern;
  const static HandEyeMethod       DefaultHandEyeMethod;
  const static std::string         DefaultTagFamily;
79
  const static unsigned int        DefaultMinimumNumberOfPoints;
80
  const static bool                DefaultUpdateNodes;
81
  const static bool                DefaultModelIsStationary;
82
  const static bool                DefaultCameraIsStationary;
83
  const static bool                DefaultSaveOutputRegardlessOfCalibration;
84
  const static bool                DefaultResetCalibrationIfNodeChanges;
85

86 87 88
  mitkClassMacroItkParent(NiftyCalVideoCalibrationManager, itk::Object);
  itkNewMacro(NiftyCalVideoCalibrationManager);

89
  void SetDataStorage(const mitk::DataStorage::Pointer storage);
90

91 92
  void SetLeftImageNode(mitk::DataNode::Pointer node);
  mitk::DataNode::Pointer GetLeftImageNode() const;
93

94 95
  void SetRightImageNode(mitk::DataNode::Pointer node);
  mitk::DataNode::Pointer GetRightImageNode() const;
96

97
  void SetTrackingTransformNode(mitk::DataNode::Pointer node);
98 99
  itkGetMacro(TrackingTransformNode, mitk::DataNode::Pointer);

100 101
  itkSetMacro(ModelTransformNode, mitk::DataNode::Pointer);
  itkGetMacro(ModelTransformNode, mitk::DataNode::Pointer);
102

103 104
  itkSetMacro(NumberOfSnapshotsForCalibrating, unsigned int);
  itkGetMacro(NumberOfSnapshotsForCalibrating, unsigned int);
105

106 107
  itkSetMacro(DoIterative, bool);
  itkGetMacro(DoIterative, bool);
108

109 110 111
  itkSetMacro(Do3DOptimisation, bool);
  itkGetMacro(Do3DOptimisation, bool);

112 113 114
  itkSetMacro(ModelIsStationary, bool);
  itkGetMacro(ModelIsStationary, bool);

115 116 117
  itkSetMacro(CameraIsStationary, bool);
  itkGetMacro(CameraIsStationary, bool);

118 119
  itkSetMacro(SaveOutputRegardlessOfCalibration, bool);
  itkGetMacro(SaveOutputRegardlessOfCalibration, bool);
120

121 122 123
  itkSetMacro(ResetCalibrationIfNodeChanges, bool);
  itkGetMacro(ResetCalibrationIfNodeChanges, bool);

124 125 126
  itkSetMacro(DoClustering, bool);
  itkGetMacro(DoClustering, bool);

127 128 129
  itkSetMacro(UpdateNodes, bool);
  itkGetMacro(UpdateNodes, bool);

130 131
  itkSetMacro(ScaleFactorX, double);
  itkGetMacro(ScaleFactorX, double);
132

133 134
  itkSetMacro(ScaleFactorY, double);
  itkGetMacro(ScaleFactorY, double);
135

136 137
  itkSetMacro(GridSizeX, unsigned int);
  itkGetMacro(GridSizeX, unsigned int);
138

139 140 141 142 143
  itkSetMacro(GridSizeY, unsigned int);
  itkGetMacro(GridSizeY, unsigned int);

  itkSetMacro(MinimumNumberOfPoints, unsigned int);
  itkGetMacro(MinimumNumberOfPoints, unsigned int);
144

145 146
  itkSetMacro(CalibrationPattern, CalibrationPatterns);
  itkGetMacro(CalibrationPattern, CalibrationPatterns);
147

148 149
  itkSetMacro(HandeyeMethod, HandEyeMethod);
  itkGetMacro(HandeyeMethod, HandEyeMethod);
150

151 152 153
  itkSetMacro(TagFamily, std::string);
  itkGetMacro(TagFamily, std::string);

154 155
  void SetModelFileName(const std::string& fileName);
  itkGetMacro(ModelFileName, std::string);
156

157 158
  void SetOutputPrefixName(const std::string& dirName);
  itkGetMacro(OutputPrefixName, std::string);
159

160 161
  void SetReferenceDataFileNames(const std::string& imageFileName,
                                 const std::string& pointsFileName);
162
  itkGetMacro(ReferenceImageFileName, std::string);
163
  itkGetMacro(ReferencePointsFileName, std::string);
164

165 166 167
  void SetTemplateImageFileName(const std::string& fileName);
  itkGetMacro(TemplateImageFileName, std::string);

168 169
  void SetModelTransformFileName(const std::string& fileName);
  itkGetMacro(ModelTransformFileName, std::string);
170

171 172 173
  itkGetMacro(CalibrationResult, std::string);
  itkGetMacro(CalibrationErrorMessage, std::string);

174 175
  bool isStereo() const;

176
  unsigned int GetNumberOfSnapshots() const;
177

178 179 180 181
  /**
   * \brief Clears down the internal points and image arrays,
   * so calibration will restart with zero data.
   */
182
  void Restart();
183 184 185 186 187

  /**
   * \brief Grabs images and tracking, and runs the point extraction.
   * \return Returns true if successful and false otherwise.
   */
188
  bool Grab();
189

190 191 192
  /**
   * \brief Removes the last grabbed snapshot.
   */
193
  void UnGrab();
194 195 196 197 198 199 200

  /**
   * \brief Performs the actual calibration.
   *
   * This can be mono, stereo, iterative and include hand-eye,
   * depending on the configuration parameters stored in this class.
   *
Matt Clarkson's avatar
Matt Clarkson committed
201
   * \return bool true if it ran to completion, false otherwise.
202 203
   *
   * Note that even if calibration ran to completion, it doesn't mean its a good calibration.
204
   */
205
  bool Calibrate();
206 207 208

  /**
   * \brief Saves a bunch of standard (from a NifTK perspective)
209
   * calibration files to the output dir, overwriting existing files.
210
   */
211
  void Save(bool isSuccessful);
212

213 214 215 216 217 218
  /**
   * \brief To update the camera to world in mitk::DataStorage so
   * we can watch the current calibration live in real-time.
   */
  void UpdateCameraToWorldPosition();

219 220 221 222 223
  /**
   * \brief Transforms m_ModelPoints into m_ModelPointsToVisualise;
   */
  void UpdateVisualisedPoints();

224 225 226 227 228 229
  /**
   * \brief Loads our NifTK standard named calibration files from disk,
   * overwriting all the existing, intrinsic, distortion, hand-eye etc.
   */
  void LoadCalibrationFromDirectory(const std::string& dirName);

230 231 232 233 234 235 236 237 238 239
protected:

  NiftyCalVideoCalibrationManager(); // Purposefully hidden.
  virtual ~NiftyCalVideoCalibrationManager(); // Purposefully hidden.

  NiftyCalVideoCalibrationManager(const NiftyCalVideoCalibrationManager&); // Purposefully not implemented.
  NiftyCalVideoCalibrationManager& operator=(const NiftyCalVideoCalibrationManager&); // Purposefully not implemented.

private:

240 241 242
  /**
   * \brief Extracts mitk::Image from imageNode, converts to OpenCV and makes grey-scale.
   */
243
  void ConvertImage(mitk::DataNode::Pointer imageNode, cv::Mat& outputImage);
244 245 246 247 248 249 250

  /**
   * \brief Runs the niftk::IPoint2DDetector on the image.
   *
   * The preferences page will determine the current preferred method of extraction.
   * e.g. Chessboard, grid of circles, AprilTags.
   */
251
  bool ExtractPoints(int imageIndex, const cv::Mat& image);
252 253 254 255

  /**
   * \brief Converts OpenCV rotation vectors and translation vectors to matrices.
   */
256 257 258
  std::list<cv::Matx44d> ExtractCameraMatrices(int imageIndex);

  /**
259
   * \brief Extracts a set of tracking matrices.
260
   */
261
  std::list<cv::Matx44d> ExtractTrackingMatrices(bool shahidiOverride = false);
262 263 264 265 266

  /**
   * \brief Extracts a set of model (chessboard to tracker) matrices.
   */
  std::list<cv::Matx44d> ExtractModelMatrices();
267

268 269 270
  /**
   * \brief Converts a list of matrices to a vector.
   */
271 272
  std::vector<cv::Mat> ConvertMatrices(const std::list<cv::Matx44d>& list);

273
  /**
274
   * \brief Actually does Tsai's 1989 hand-eye calibration for imageIndex=0=left, imageIndex=1=right camera.
275
   */
276
  cv::Matx44d DoTsaiHandEye(int imageIndex);
277 278

  /**
279
   * \brief Actually does Shahidi 2002 hand-eye calibration for imageIndex=0=left, imageIndex=1=right camera.
280
   */
281
  cv::Matx44d DoShahidiHandEye(int imageIndex);
282 283

  /**
284
   * \brief Actually does Malti's 2013 hand-eye calibration for imageIndex=0=left, imageIndex=1=right camera.
285
   */
286
  cv::Matx44d DoMaltiHandEye(int imageIndex);
287

288 289 290
  /**
   * \brief Actually does a full non-linear calibration of all extrinsic parameters.
   */
291
  cv::Matx44d DoFullExtrinsicHandEye(int imageIndex);
292

293 294 295 296
  /**
   * \brief Bespoke method to calculate independent leftHandEye and rightHandEye,
   * by optimising all parameters in stereo, simultaneously.
   */
297
  void DoFullExtrinsicHandEyeInStereo(cv::Matx44d& leftHandEye, cv::Matx44d& rightHandEye);
298

299 300 301
  /**
   * \brief Saves list of images that were used for calibration.
   */
302 303 304
  void SaveImages(const std::string& prefix,
                  const std::list<std::pair<std::shared_ptr<niftk::IPoint2DDetector>, cv::Mat> >&
                  );
305 306 307 308

  /**
   * \brief Saves list of points that were used for calibration.
   */
309
  void SavePoints(const std::string& prefix, const std::list<niftk::PointSet>& points);
310

311 312 313 314
  /**
   * \brief Utility method to apply intrinsic and distortion parameters
   * to an image by setting a mitk::CameraIntrinsics property.
   */
315 316 317 318 319
  void SetIntrinsicsOnImage(const cv::Mat& intrinsics,
                            const cv::Mat& distortion,
                            const std::string& propertyName,
                            mitk::DataNode::Pointer image);

320 321 322 323 324 325 326 327 328
  /**
   * \brief Utility method to apply a stereo (left-to-right) transformation
   * to an image node, by setting a property containing the matrix.
   *
   * Note: NifTK standard is Right-to-Left, whereas NiftyCal and OpenCV
   * by default use Left-To-Right. So, here, this method is private
   * as we specifically handle data in this class, which by NiftyCal and OpenCV
   * convention use Left-to-Right.
   */
329 330 331 332 333 334
  void SetStereoExtrinsicsOnImage(const cv::Mat& leftToRightRotationMatrix,
                                  const cv::Mat& leftToRightTranslationVector,
                                  const std::string& propertyName,
                                  mitk::DataNode::Pointer image
                                  );

335 336 337 338
  /**
   * \brief Combined method to update properties on image nodes, and to
   * move the model points to the correct location in space.
   */
339 340
  void UpdateDisplayNodes();

341
  cv::Matx44d GetInitialHandEye(int imageIndex);
342
  cv::Matx44d GetInitialModelToWorld();
343 344
  double      GetStereoRMSReconstructionError(const cv::Matx44d& handEye);
  double      GetMonoRMSReconstructionError(const cv::Matx44d& handEye);
345
  cv::Matx44d GetModelToWorld(const cv::Matx44d& handEye);
346

347 348
  typedef mitk::GenericProperty<itk::Matrix<float, 4, 4> > MatrixProperty;

349
  // Data from Plugin/DataStorage.
350 351 352
  mitk::DataStorage::Pointer                     m_DataStorage;
  mitk::DataNode::Pointer                        m_ImageNode[2];
  mitk::DataNode::Pointer                        m_TrackingTransformNode;
353
  mitk::DataNode::Pointer                        m_ModelTransformNode;
354

355
  // Data from preferences.
356
  bool                                           m_DoIterative;
357
  bool                                           m_Do3DOptimisation;
358
  bool                                           m_DoClustering;
359
  bool                                           m_ModelIsStationary;
360
  bool                                           m_CameraIsStationary;
361
  bool                                           m_SaveOutputRegardlessOfCalibration;
362
  bool                                           m_ResetCalibrationIfNodeChanges;
363
  unsigned int                                   m_NumberOfSnapshotsForCalibrating;
364 365 366
  std::string                                    m_ModelFileName;
  double                                         m_ScaleFactorX;
  double                                         m_ScaleFactorY;
367 368
  unsigned int                                   m_GridSizeX;
  unsigned int                                   m_GridSizeY;
369 370 371
  CalibrationPatterns                            m_CalibrationPattern;
  HandEyeMethod                                  m_HandeyeMethod;
  std::string                                    m_TagFamily;
372
  std::string                                    m_OutputPrefixName;
373
  std::string                                    m_OutputDirName;
374
  std::string                                    m_ModelTransformFileName;
375 376
  std::string                                    m_ReferenceImageFileName;
  std::string                                    m_ReferencePointsFileName;
377
  bool                                           m_UpdateNodes;
378
  unsigned int                                   m_MinimumNumberOfPoints;
379
  std::string                                    m_TemplateImageFileName;
380
  std::string                                    m_CalibrationDirName;
381

382
  // Data used for temporary storage
383
  cv::Mat                                        m_TmpImage[2];
384

385
  // Data used for calibration.
386 387
  cv::Size2i                                     m_ImageSize;
  std::pair< cv::Mat, niftk::PointSet>           m_ReferenceDataForIterativeCalib;
388
  cv::Mat                                        m_TemplateImage;
389 390
  cv::Matx44d                                    m_StaticModelTransform;
  cv::Matx44d                                    m_ModelTransform;
391 392 393 394
  niftk::Model3D                                 m_ModelPoints;
  mitk::PointSet::Pointer                        m_ModelPointsToVisualise;
  mitk::DataNode::Pointer                        m_ModelPointsToVisualiseDataNode;
  std::list<niftk::PointSet>                     m_Points[2];
395 396 397 398
  std::list<
    std::pair<
      std::shared_ptr<niftk::IPoint2DDetector>,
      cv::Mat>
399
    >                                            m_OriginalImages[2];
400 401 402 403
  std::list<
    std::pair<
      std::shared_ptr<niftk::IPoint2DDetector>,
      cv::Mat>
404 405 406
    >                                            m_ImagesForWarping[2];
  std::list<cv::Matx44d>                         m_TrackingMatrices;
  std::vector<mitk::DataNode::Pointer>           m_TrackingMatricesDataNodes;
407
  std::list<cv::Matx44d>                         m_ModelTrackingMatrices;
408

409
  // Calibration result
410 411 412 413 414 415
  cv::Mat                                        m_Intrinsic[2];
  cv::Mat                                        m_Distortion[2];
  std::vector<cv::Mat>                           m_Rvecs[2];
  std::vector<cv::Mat>                           m_Tvecs[2];
  cv::Mat                                        m_EssentialMatrix;
  cv::Mat                                        m_FundamentalMatrix;
416 417
  cv::Mat                                        m_LeftToRightRotationMatrix;
  cv::Mat                                        m_LeftToRightTranslationVector;
418
  std::vector<cv::Matx44d>                       m_HandEyeMatrices[2];
419
  cv::Matx44d                                    m_ModelToWorld;
420
  std::string                                    m_CalibrationResult;
421
  std::string                                    m_CalibrationErrorMessage;
422

423 424 425 426 427
}; // end class

} // end namespace

#endif