CameraModel.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/CameraModel.h>
29 #include <rtabmap/core/Version.h>
30 #include <rtabmap/utilite/ULogger.h>
31 #include <rtabmap/utilite/UDirectory.h>
32 #include <rtabmap/utilite/UFile.h>
33 #include <rtabmap/utilite/UConversion.h>
34 #include <rtabmap/utilite/UMath.h>
35 #include <rtabmap/utilite/UStl.h>
36 #include <opencv2/imgproc/imgproc.hpp>
37 
38 namespace rtabmap {
39 
40 CameraModel::CameraModel() :
41  localTransform_(0,0,1,0, -1,0,0,0, 0,-1,0,0)
42 {
43 
44 }
45 
46 CameraModel::CameraModel(
47  const std::string & cameraName,
48  const cv::Size & imageSize,
49  const cv::Mat & K,
50  const cv::Mat & D,
51  const cv::Mat & R,
52  const cv::Mat & P,
53  const Transform & localTransform) :
54  name_(cameraName),
55  imageSize_(imageSize),
56  K_(K),
57  D_(D),
58  R_(R),
59  P_(P),
60  localTransform_(localTransform)
61 {
62  UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
63  UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
64  UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
65  UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
66 }
67 
68 CameraModel::CameraModel(
69  double fx,
70  double fy,
71  double cx,
72  double cy,
73  const Transform & localTransform,
74  double Tx,
75  const cv::Size & imageSize) :
76  imageSize_(imageSize),
77  K_(cv::Mat::eye(3, 3, CV_64FC1)),
78  localTransform_(localTransform)
79 {
80  UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
81  UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
82  UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
83  UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
84  UASSERT(!localTransform.isNull());
85 
86  if(cx==0.0 && imageSize.width > 0)
87  {
88  cx = double(imageSize.width)/2.0-0.5;
89  }
90  if(cy==0.0 && imageSize.height > 0)
91  {
92  cy = double(imageSize.height)/2.0-0.5;
93  }
94 
95  if(Tx != 0.0)
96  {
97  P_ = cv::Mat::eye(3, 4, CV_64FC1),
98  P_.at<double>(0,0) = fx;
99  P_.at<double>(1,1) = fy;
100  P_.at<double>(0,2) = cx;
101  P_.at<double>(1,2) = cy;
102  P_.at<double>(0,3) = Tx;
103  }
104 
105  K_.at<double>(0,0) = fx;
106  K_.at<double>(1,1) = fy;
107  K_.at<double>(0,2) = cx;
108  K_.at<double>(1,2) = cy;
109 }
110 
111 CameraModel::CameraModel(
112  const std::string & name,
113  double fx,
114  double fy,
115  double cx,
116  double cy,
117  const Transform & localTransform,
118  double Tx,
119  const cv::Size & imageSize) :
120  name_(name),
121  imageSize_(imageSize),
122  K_(cv::Mat::eye(3, 3, CV_64FC1)),
123  localTransform_(localTransform)
124 {
125  UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
126  UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
127  UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
128  UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
129  UASSERT(!localTransform.isNull());
130 
131  if(cx==0.0 && imageSize.width > 0)
132  {
133  cx = double(imageSize.width)/2.0-0.5;
134  }
135  if(cy==0.0 && imageSize.height > 0)
136  {
137  cy = double(imageSize.height)/2.0-0.5;
138  }
139 
140  if(Tx != 0.0)
141  {
142  P_ = cv::Mat::eye(3, 4, CV_64FC1),
143  P_.at<double>(0,0) = fx;
144  P_.at<double>(1,1) = fy;
145  P_.at<double>(0,2) = cx;
146  P_.at<double>(1,2) = cy;
147  P_.at<double>(0,3) = Tx;
148  }
149 
150  K_.at<double>(0,0) = fx;
151  K_.at<double>(1,1) = fy;
152  K_.at<double>(0,2) = cx;
153  K_.at<double>(1,2) = cy;
154 }
155 
156 void CameraModel::initRectificationMap()
157 {
158  UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
159  UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
160  UASSERT(R_.rows == 3 && R_.cols == 3);
161  UASSERT(P_.rows == 3 && P_.cols == 4);
162  // init rectification map
163  UINFO("Initialize rectify map");
164  if(D_.cols == 6)
165  {
166 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
167  // Equidistant / FishEye
168  // get only k parameters (k1,k2,p1,p2,k3,k4)
169  cv::Mat D(1, 4, CV_64FC1);
170  D.at<double>(0,0) = D_.at<double>(0,0);
171  D.at<double>(0,1) = D_.at<double>(0,1);
172  D.at<double>(0,2) = D_.at<double>(0,4);
173  D.at<double>(0,3) = D_.at<double>(0,5);
174  cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
175  }
176  else
177 #else
178  UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
179  CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
180  }
181 #endif
182  {
183  // RadialTangential
184  cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
185  }
186 }
187 
188 void CameraModel::setImageSize(const cv::Size & size)
189 {
190  UASSERT((size.height > 0 && size.width > 0) || (size.height == 0 && size.width == 0));
191  imageSize_ = size;
192  double ncx = cx();
193  double ncy = cy();
194  if(ncx==0.0 && imageSize_.width > 0)
195  {
196  ncx = double(imageSize_.width)/2.0-0.5;
197  }
198  if(ncy==0.0 && imageSize_.height > 0)
199  {
200  ncy = double(imageSize_.height)/2.0-0.5;
201  }
202  if(!P_.empty())
203  {
204  P_.at<double>(0,2) = ncx;
205  P_.at<double>(1,2) = ncy;
206  }
207  if(!K_.empty())
208  {
209  K_.at<double>(0,2) = ncx;
210  K_.at<double>(1,2) = ncy;
211  }
212 }
213 
214 bool CameraModel::load(const std::string & filePath)
215 {
216  K_ = cv::Mat();
217  D_ = cv::Mat();
218  R_ = cv::Mat();
219  P_ = cv::Mat();
220  mapX_ = cv::Mat();
221  mapY_ = cv::Mat();
222  name_.clear();
223  imageSize_ = cv::Size();
224 
225  if(UFile::exists(filePath))
226  {
227  try
228  {
229  UINFO("Reading calibration file \"%s\"", filePath.c_str());
230  cv::FileStorage fs(filePath, cv::FileStorage::READ);
231 
232  cv::FileNode n,n2;
233 
234  n = fs["camera_name"];
235  if(n.type() != cv::FileNode::NONE)
236  {
237  name_ = (int)n;
238  }
239  else
240  {
241  UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
242  }
243 
244  n = fs["image_width"];
245  n2 = fs["image_height"];
246  if(n.type() != cv::FileNode::NONE)
247  {
248  imageSize_.width = (int)fs["image_width"];
249  imageSize_.height = (int)fs["image_height"];
250  }
251  else
252  {
253  UWARN("Missing \"image_width\" and/or \"image_height\" fields in \"%s\"", filePath.c_str());
254  }
255 
256  // import from ROS calibration format
257  n = fs["camera_matrix"];
258  if(n.type() != cv::FileNode::NONE)
259  {
260  int rows = (int)n["rows"];
261  int cols = (int)n["cols"];
262  std::vector<double> data;
263  n["data"] >> data;
264  UASSERT(rows*cols == (int)data.size());
265  UASSERT(rows == 3 && cols == 3);
266  K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
267  }
268  else
269  {
270  UWARN("Missing \"camera_matrix\" field in \"%s\"", filePath.c_str());
271  }
272 
273  n = fs["distortion_coefficients"];
274  if(n.type() != cv::FileNode::NONE)
275  {
276  int rows = (int)n["rows"];
277  int cols = (int)n["cols"];
278  std::vector<double> data;
279  n["data"] >> data;
280  UASSERT(rows*cols == (int)data.size());
281  UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
282  D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
283  }
284  else
285  {
286  UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
287  }
288 
289  n = fs["distortion_model"];
290  if(n.type() != cv::FileNode::NONE)
291  {
292  std::string distortionModel = (std::string)n;
293  if(D_.cols>=4 &&
294  (uStrContains(distortionModel, "fisheye") ||
295  uStrContains(distortionModel, "equidistant")))
296  {
297  cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
298  D.at<double>(0,0) = D_.at<double>(0,0);
299  D.at<double>(0,1) = D_.at<double>(0,1);
300  D.at<double>(0,4) = D_.at<double>(0,2);
301  D.at<double>(0,5) = D_.at<double>(0,3);
302  D_ = D;
303  }
304  }
305  else
306  {
307  UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
308  }
309 
310  n = fs["rectification_matrix"];
311  if(n.type() != cv::FileNode::NONE)
312  {
313  int rows = (int)n["rows"];
314  int cols = (int)n["cols"];
315  std::vector<double> data;
316  n["data"] >> data;
317  UASSERT(rows*cols == (int)data.size());
318  UASSERT(rows == 3 && cols == 3);
319  R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
320  }
321  else
322  {
323  UWARN("Missing \"rectification_matrix\" field in \"%s\"", filePath.c_str());
324  }
325 
326  n = fs["projection_matrix"];
327  if(n.type() != cv::FileNode::NONE)
328  {
329  int rows = (int)n["rows"];
330  int cols = (int)n["cols"];
331  std::vector<double> data;
332  n["data"] >> data;
333  UASSERT(rows*cols == (int)data.size());
334  UASSERT(rows == 3 && cols == 4);
335  P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
336  }
337  else
338  {
339  UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
340  }
341 
342  fs.release();
343 
344  if(isValidForRectification())
345  {
346  initRectificationMap();
347  }
348 
349  return true;
350  }
351  catch(const cv::Exception & e)
352  {
353  UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
354  }
355  }
356  else
357  {
358  UWARN("Could not load calibration file \"%s\".", filePath.c_str());
359  }
360  return false;
361 }
362 
363 bool CameraModel::load(const std::string & directory, const std::string & cameraName)
364 {
365  return load(directory+"/"+cameraName+".yaml");
366 }
367 
368 bool CameraModel::save(const std::string & directory) const
369 {
370  std::string filePath = directory+"/"+name_+".yaml";
371  if(!filePath.empty() && (!K_.empty() || !D_.empty() || !R_.empty() || !P_.empty()))
372  {
373  UINFO("Saving calibration to file \"%s\"", filePath.c_str());
374  cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
375 
376  // export in ROS calibration format
377 
378  if(!name_.empty())
379  {
380  fs << "camera_name" << name_;
381  }
382  if(imageSize_.width>0 && imageSize_.height>0)
383  {
384  fs << "image_width" << imageSize_.width;
385  fs << "image_height" << imageSize_.height;
386  }
387 
388  if(!K_.empty())
389  {
390  fs << "camera_matrix" << "{";
391  fs << "rows" << K_.rows;
392  fs << "cols" << K_.cols;
393  fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
394  fs << "}";
395  }
396 
397  if(!D_.empty())
398  {
399  cv::Mat D = D_;
400  if(D_.cols == 6)
401  {
402  D = cv::Mat(1,4,CV_64FC1);
403  D.at<double>(0,0) = D_.at<double>(0,0);
404  D.at<double>(0,1) = D_.at<double>(0,1);
405  D.at<double>(0,2) = D_.at<double>(0,4);
406  D.at<double>(0,3) = D_.at<double>(0,5);
407  }
408  fs << "distortion_coefficients" << "{";
409  fs << "rows" << D.rows;
410  fs << "cols" << D.cols;
411  fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
412  fs << "}";
413 
414  // compaibility with ROS
415  if(D_.cols == 6)
416  {
417  fs << "distortion_model" << "equidistant"; // equidistant, fisheye
418  }
419  else if(D.cols > 5)
420  {
421  fs << "distortion_model" << "rational_polynomial"; // rad tan
422  }
423  else
424  {
425  fs << "distortion_model" << "plumb_bob"; // rad tan
426  }
427  }
428 
429  if(!R_.empty())
430  {
431  fs << "rectification_matrix" << "{";
432  fs << "rows" << R_.rows;
433  fs << "cols" << R_.cols;
434  fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
435  fs << "}";
436  }
437 
438  if(!P_.empty())
439  {
440  fs << "projection_matrix" << "{";
441  fs << "rows" << P_.rows;
442  fs << "cols" << P_.cols;
443  fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
444  fs << "}";
445  }
446 
447  fs.release();
448 
449  return true;
450  }
451  else
452  {
453  UERROR("Cannot save calibration to \"%s\" because it is empty.", filePath.c_str());
454  }
455  return false;
456 }
457 
458 std::vector<unsigned char> CameraModel::serialize() const
459 {
460  const int headerSize = 11;
461  int header[headerSize] = {
462  RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
463  0, //mono // 3,
464  imageSize_.width, imageSize_.height, // 4,5
465  (int)K_.total(), (int)D_.total(), (int)R_.total(), (int)P_.total(), // 6,7,8,9
466  localTransform_.isNull()?0:localTransform_.size()}; // 10
467  UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
468  std::vector<unsigned char> data(
469  sizeof(int)*headerSize +
470  sizeof(double)*(K_.total()+D_.total()+R_.total()+P_.total()) +
471  (localTransform_.isNull()?0:sizeof(float)*localTransform_.size()));
472  memcpy(data.data(), header, sizeof(int)*headerSize);
473  int index = sizeof(int)*headerSize;
474  if(!K_.empty())
475  {
476  memcpy(data.data()+index, K_.data, sizeof(double)*(K_.total()));
477  index+=sizeof(double)*(K_.total());
478  }
479  if(!D_.empty())
480  {
481  memcpy(data.data()+index, D_.data, sizeof(double)*(D_.total()));
482  index+=sizeof(double)*(D_.total());
483  }
484  if(!R_.empty())
485  {
486  memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
487  index+=sizeof(double)*(R_.total());
488  }
489  if(!P_.empty())
490  {
491  memcpy(data.data()+index, P_.data, sizeof(double)*(P_.total()));
492  index+=sizeof(double)*(P_.total());
493  }
494  if(!localTransform_.isNull())
495  {
496  memcpy(data.data()+index, localTransform_.data(), sizeof(float)*(localTransform_.size()));
497  index+=sizeof(float)*(localTransform_.size());
498  }
499  return data;
500 }
501 
502 unsigned int CameraModel::deserialize(const std::vector<unsigned char>& data)
503 {
504  return deserialize(data.data(), data.size());
505 }
506 unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
507 {
508  *this = CameraModel();
509  int headerSize = 11;
510  if(dataSize >= sizeof(int)*headerSize)
511  {
512  UASSERT(data != 0);
513  const int * header = (const int *)data;
514  int type = header[3];
515  if(type == 0)
516  {
517  imageSize_.width = header[4];
518  imageSize_.height = header[5];
519  int iK = 6;
520  int iD = 7;
521  int iR = 8;
522  int iP = 9;
523  int iL = 10;
524  UDEBUG("Header: %d %d %d %d %d %d %d %d %d %d %d", header[0],header[1],header[2],header[3],header[4],header[5],header[6],header[7],header[8],header[9],header[10]);
525  unsigned int requiredDataSize = sizeof(int)*headerSize +
526  sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
527  sizeof(float)*header[iL];
528  UASSERT_MSG(dataSize >= requiredDataSize,
529  uFormat("dataSize=%d != required=%d (header: version %d.%d.%d %dx%d type=%d K=%d D=%d R=%d P=%d L=%d)",
530  dataSize,
531  requiredDataSize,
532  header[0], header[1], header[2], header[4], header[5], header[3],
533  header[iK], header[iD], header[iR],header[iP], header[iL]).c_str());
534  unsigned int index = sizeof(int)*headerSize;
535  if(header[iK] != 0)
536  {
537  UASSERT(header[iK] == 9);
538  K_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
539  index+=sizeof(double)*(K_.total());
540  }
541  if(header[iD] != 0)
542  {
543  D_ = cv::Mat(1, header[iD], CV_64FC1, (void*)(data+index)).clone();
544  index+=sizeof(double)*(D_.total());
545  }
546  if(header[iR] != 0)
547  {
548  UASSERT(header[iR] == 9);
549  R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
550  index+=sizeof(double)*(R_.total());
551  }
552  if(header[iP] != 0)
553  {
554  UASSERT(header[iP] == 12);
555  P_ = cv::Mat(3, 4, CV_64FC1, (void*)(data+index)).clone();
556  index+=sizeof(double)*(P_.total());
557  }
558  if(header[iL] != 0)
559  {
560  UASSERT(header[iL] == 12);
561  memcpy(localTransform_.data(), data+index, sizeof(float)*localTransform_.size());
562  index+=sizeof(float)*localTransform_.size();
563  }
564  UASSERT(index <= dataSize);
565  return index;
566  }
567  else
568  {
569  UERROR("Serialized calibration is not mono (type=%d), use the appropriate class matching the type to deserialize.", type);
570  }
571  }
572  UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
573  return 0;
574 }
575 
576 CameraModel CameraModel::scaled(double scale) const
577 {
578  CameraModel scaledModel = *this;
579  UASSERT(scale > 0.0);
580  if(this->isValidForProjection())
581  {
582  // has only effect on K and P
583  cv::Mat K;
584  if(!K_.empty())
585  {
586  K = K_.clone();
587  K.at<double>(0,0) *= scale;
588  K.at<double>(1,1) *= scale;
589  K.at<double>(0,2) *= scale;
590  K.at<double>(1,2) *= scale;
591  }
592 
593  cv::Mat P;
594  if(!P_.empty())
595  {
596  P = P_.clone();
597  P.at<double>(0,0) *= scale;
598  P.at<double>(1,1) *= scale;
599  P.at<double>(0,2) *= scale;
600  P.at<double>(1,2) *= scale;
601  P.at<double>(0,3) *= scale;
602  P.at<double>(1,3) *= scale;
603  }
604  scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
605  }
606  else
607  {
608  UWARN("Trying to scale a camera model not valid! Ignoring scaling...");
609  }
610  return scaledModel;
611 }
612 
613 CameraModel CameraModel::roi(const cv::Rect & roi) const
614 {
615  CameraModel roiModel = *this;
616  if(this->isValidForProjection())
617  {
618  // has only effect on cx and cy
619  cv::Mat K;
620  if(!K_.empty())
621  {
622  K = K_.clone();
623  K.at<double>(0,2) -= roi.x;
624  K.at<double>(1,2) -= roi.y;
625  }
626 
627  cv::Mat P;
628  if(!P_.empty())
629  {
630  P = P_.clone();
631  P.at<double>(0,2) -= roi.x;
632  P.at<double>(1,2) -= roi.y;
633  }
634  roiModel = CameraModel(name_, roi.size(), K, D_, R_, P, localTransform_);
635  }
636  else
637  {
638  UWARN("Trying to extract roi from a camera model not valid! Ignoring roi...");
639  }
640  return roiModel;
641 }
642 
643 double CameraModel::fovX() const
644 {
645  return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;
646 }
647 double CameraModel::fovY() const
648 {
649  return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;
650 }
651 
652 double CameraModel::horizontalFOV() const
653 {
654  return fovX()*180.0/CV_PI;
655 }
656 
657 double CameraModel::verticalFOV() const
658 {
659  return fovY()*180.0/CV_PI;
660 }
661 
662 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
663 {
664  UDEBUG("");
665  if(!mapX_.empty() && !mapY_.empty())
666  {
667  cv::Mat rectified;
668  cv::remap(raw, rectified, mapX_, mapY_, interpolation);
669  return rectified;
670  }
671  else
672  {
673  UERROR("Cannot rectify image because the rectify map is not initialized.");
674  return raw.clone();
675  }
676 }
677 
678 //inspired from https://github.com/code-iai/iai_kinect2/blob/master/depth_registration/src/depth_registration_cpu.cpp
679 cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
680 {
681  UDEBUG("");
682  UASSERT(raw.type() == CV_16UC1);
683  if(!mapX_.empty() && !mapY_.empty())
684  {
685  cv::Mat rectified = cv::Mat::zeros(mapX_.rows, mapX_.cols, raw.type());
686  for(int y=0; y<mapX_.rows; ++y)
687  {
688  for(int x=0; x<mapX_.cols; ++x)
689  {
690  cv::Point2f pt(mapX_.at<float>(y,x), mapY_.at<float>(y,x));
691  int xL = (int)floor(pt.x);
692  int xH = (int)ceil(pt.x);
693  int yL = (int)floor(pt.y);
694  int yH = (int)ceil(pt.y);
695  if(xL >= 0 && yL >= 0 && xH < raw.cols && yH < raw.rows)
696  {
697  const unsigned short & pLT = raw.at<unsigned short>(yL, xL);
698  const unsigned short & pRT = raw.at<unsigned short>(yL, xH);
699  const unsigned short & pLB = raw.at<unsigned short>(yH, xL);
700  const unsigned short & pRB = raw.at<unsigned short>(yH, xH);
701  if(pLT > 0 && pRT > 0 && pLB > 0 && pRB > 0)
702  {
703  unsigned short avg = (pLT + pRT + pLB + pRB) / 4;
704  unsigned short thres = 0.01 * avg;
705  if( abs(pLT - avg) < thres &&
706  abs(pRT - avg) < thres &&
707  abs(pLB - avg) < thres &&
708  abs(pRB - avg) < thres)
709  {
710  //bilinear interpolation
711  float a = pt.x - (float)xL;
712  float c = pt.y - (float)yL;
713 
714  //http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
715  rectified.at<unsigned short>(y,x) =
716  (pLT * (1.f - a) + pRT * a) * (1.f - c) +
717  (pLB * (1.f - a) + pRB * a) * c;
718  }
719  }
720  }
721  }
722  }
723  return rectified;
724  }
725  else
726  {
727  UERROR("Cannot rectify image because the rectify map is not initialized.");
728  return raw.clone();
729  }
730 }
731 
732 // resulting 3D point is in /camera_link frame
733 void CameraModel::project(float u, float v, float depth, float & x, float & y, float & z) const
734 {
735  if(depth > 0.0f)
736  {
737  // Fill in XYZ
738  x = (u - cx()) * depth / fx();
739  y = (v - cy()) * depth / fy();
740  z = depth;
741  }
742  else
743  {
744  x = y = z = std::numeric_limits<float>::quiet_NaN();
745  }
746 }
747 // 3D point is in /camera_link frame
748 void CameraModel::reproject(float x, float y, float z, float & u, float & v) const
749 {
750  UASSERT(z!=0.0f);
751  float invZ = 1.0f/z;
752  u = (fx()*x)*invZ + cx();
753  v = (fy()*y)*invZ + cy();
754 }
755 void CameraModel::reproject(float x, float y, float z, int & u, int & v) const
756 {
757  UASSERT(z!=0.0f);
758  float invZ = 1.0f/z;
759  u = (fx()*x)*invZ + cx();
760  v = (fy()*y)*invZ + cy();
761 }
762 
763 bool CameraModel::inFrame(int u, int v) const
764 {
765  return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
766 }
767 
768 std::ostream& operator<<(std::ostream& os, const CameraModel& model)
769 {
770  os << "Name: " << model.name() << std::endl
771  << "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
772  << "K= " << model.K_raw() << std::endl
773  << "D= " << model.D_raw() << std::endl
774  << "R= " << model.R() << std::endl
775  << "P= " << model.P() << std::endl
776  << "LocalTransform= " << model.localTransform();
777  return os;
778 }
779 
780 } /* namespace rtabmap */
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
rtabmap::CameraModel::K_raw
cv::Mat K_raw() const
Definition: CameraModel.h:108
rtabmap::CameraModel::setImageSize
void setImageSize(const cv::Size &size)
Definition: CameraModel.cpp:188
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::CameraModel::name
const std::string & name() const
Definition: CameraModel.h:100
f
f
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
rtabmap::CameraModel::inFrame
bool inFrame(int u, int v) const
Definition: CameraModel.cpp:763
rtabmap::CameraModel::localTransform_
Transform localTransform_
Definition: CameraModel.h:158
rtabmap::CameraModel::serialize
std::vector< unsigned char > serialize() const
Definition: CameraModel.cpp:458
rtabmap::CameraModel::save
bool save(const std::string &directory) const
Definition: CameraModel.cpp:368
glm::e
GLM_FUNC_DECL genType e()
glm::scale
GLM_FUNC_DECL detail::tmat4x4< T, P > scale(detail::tmat4x4< T, P > const &m, detail::tvec3< T, P > const &v)
rtabmap::CameraModel::verticalFOV
double verticalFOV() const
Definition: CameraModel.cpp:657
UMath.h
Basic mathematics functions.
rtabmap::CameraModel::load
bool load(const std::string &filePath)
Definition: CameraModel.cpp:214
header
std_msgs::Header * header(M &m)
UConversion.h
Some conversion functions.
cv
Definition: Features2d.h:41
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:787
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:111
rtabmap::CameraModel::R
cv::Mat R() const
Definition: CameraModel.h:112
UASSERT
#define UASSERT(condition)
rtabmap::CameraModel::horizontalFOV
double horizontalFOV() const
Definition: CameraModel.cpp:652
UStl.h
Wrappers of STL for convenient functions.
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::floor
GLM_FUNC_DECL genType floor(genType const &x)
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
rtabmap::CameraModel::reproject
void reproject(float x, float y, float z, float &u, float &v) const
Definition: CameraModel.cpp:748
rtabmap::CameraModel::fovX
double fovX() const
Definition: CameraModel.cpp:643
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
rtabmap::CameraModel::roi
CameraModel roi(const cv::Rect &roi) const
Definition: CameraModel.cpp:613
rtabmap::CameraModel::P
cv::Mat P() const
Definition: CameraModel.h:113
rtabmap::CameraModel::fovY
double fovY() const
Definition: CameraModel.cpp:647
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
rtabmap::CameraModel::project
void project(float u, float v, float depth, float &x, float &y, float &z) const
Definition: CameraModel.cpp:733
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:662
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
rtabmap::CameraModel::rectifyDepth
cv::Mat rectifyDepth(const cv::Mat &raw) const
Definition: CameraModel.cpp:679
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::CameraModel::Tx
double Tx() const
Definition: CameraModel.h:106
UERROR
#define UERROR(...)
rtabmap::operator<<
RTABMAP_EXP std::ostream & operator<<(std::ostream &os, const CameraModel &model)
Definition: CameraModel.cpp:768
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::CameraModel::deserialize
unsigned int deserialize(const std::vector< unsigned char > &data)
Definition: CameraModel.cpp:502
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
rtabmap::CameraModel::D_raw
cv::Mat D_raw() const
Definition: CameraModel.h:109
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
glm::ceil
GLM_FUNC_DECL genType ceil(genType const &x)
rtabmap::CameraModel::scaled
CameraModel scaled(double scale) const
Definition: CameraModel.cpp:576
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58