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_(opticalRotation())
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_.cols == 12 || D_.cols == 14) && 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 bool 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 || D_.cols == 12 || D_.cols == 14));
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  return isRectificationMapInitialized();
187 }
188 
189 void CameraModel::setImageSize(const cv::Size & size)
190 {
191  UASSERT((size.height > 0 && size.width > 0) || (size.height == 0 && size.width == 0));
192  imageSize_ = size;
193  double ncx = cx();
194  double ncy = cy();
195  if(ncx==0.0 && imageSize_.width > 0)
196  {
197  ncx = double(imageSize_.width)/2.0-0.5;
198  }
199  if(ncy==0.0 && imageSize_.height > 0)
200  {
201  ncy = double(imageSize_.height)/2.0-0.5;
202  }
203  if(!P_.empty())
204  {
205  P_.at<double>(0,2) = ncx;
206  P_.at<double>(1,2) = ncy;
207  }
208  if(!K_.empty())
209  {
210  K_.at<double>(0,2) = ncx;
211  K_.at<double>(1,2) = ncy;
212  }
213 }
214 
215 bool CameraModel::load(const std::string & filePath)
216 {
217  K_ = cv::Mat();
218  D_ = cv::Mat();
219  R_ = cv::Mat();
220  P_ = cv::Mat();
221  mapX_ = cv::Mat();
222  mapY_ = cv::Mat();
223  name_.clear();
224  imageSize_ = cv::Size();
225 
226  if(UFile::exists(filePath))
227  {
228  try
229  {
230  UINFO("Reading calibration file \"%s\"", filePath.c_str());
231  cv::FileStorage fs(filePath, cv::FileStorage::READ);
232 
233  cv::FileNode n,n2;
234 
235  n = fs["camera_name"];
236  if(n.type() != cv::FileNode::NONE)
237  {
238  name_ = (std::string)n;
239  }
240  else
241  {
242  UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
243  }
244 
245  n = fs["image_width"];
246  n2 = fs["image_height"];
247  if(n.type() != cv::FileNode::NONE)
248  {
249  imageSize_.width = (int)fs["image_width"];
250  imageSize_.height = (int)fs["image_height"];
251  }
252  else
253  {
254  UWARN("Missing \"image_width\" and/or \"image_height\" fields in \"%s\"", filePath.c_str());
255  }
256 
257  // import from ROS calibration format
258  n = fs["camera_matrix"];
259  if(n.type() != cv::FileNode::NONE)
260  {
261  int rows = (int)n["rows"];
262  int cols = (int)n["cols"];
263  std::vector<double> data;
264  n["data"] >> data;
265  UASSERT(rows*cols == (int)data.size());
266  UASSERT(rows == 3 && cols == 3);
267  K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
268  }
269  else
270  {
271  UWARN("Missing \"camera_matrix\" field in \"%s\"", filePath.c_str());
272  }
273 
274  n = fs["distortion_coefficients"];
275  if(n.type() != cv::FileNode::NONE)
276  {
277  int rows = (int)n["rows"];
278  int cols = (int)n["cols"];
279  std::vector<double> data;
280  n["data"] >> data;
281  UASSERT(rows*cols == (int)data.size());
282  UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8 || cols == 12 || cols == 14));
283  D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
284  }
285  else
286  {
287  UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
288  }
289 
290  n = fs["distortion_model"];
291  if(n.type() != cv::FileNode::NONE)
292  {
293  std::string distortionModel = (std::string)n;
294  if(D_.cols>=4 &&
295  (uStrContains(distortionModel, "fisheye") ||
296  uStrContains(distortionModel, "equidistant")))
297  {
298  cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
299  D.at<double>(0,0) = D_.at<double>(0,0);
300  D.at<double>(0,1) = D_.at<double>(0,1);
301  D.at<double>(0,4) = D_.at<double>(0,2);
302  D.at<double>(0,5) = D_.at<double>(0,3);
303  D_ = D;
304  }
305  }
306  else
307  {
308  UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
309  }
310 
311  n = fs["rectification_matrix"];
312  if(n.type() != cv::FileNode::NONE)
313  {
314  int rows = (int)n["rows"];
315  int cols = (int)n["cols"];
316  std::vector<double> data;
317  n["data"] >> data;
318  UASSERT(rows*cols == (int)data.size());
319  UASSERT(rows == 3 && cols == 3);
320  R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
321  }
322  else
323  {
324  UWARN("Missing \"rectification_matrix\" field in \"%s\"", filePath.c_str());
325  }
326 
327  n = fs["projection_matrix"];
328  if(n.type() != cv::FileNode::NONE)
329  {
330  int rows = (int)n["rows"];
331  int cols = (int)n["cols"];
332  std::vector<double> data;
333  n["data"] >> data;
334  UASSERT(rows*cols == (int)data.size());
335  UASSERT(rows == 3 && cols == 4);
336  P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
337  }
338  else
339  {
340  UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
341  }
342 
343  n = fs["local_transform"];
344  if(n.type() != cv::FileNode::NONE)
345  {
346  int rows = (int)n["rows"];
347  int cols = (int)n["cols"];
348  std::vector<float> data;
349  n["data"] >> data;
350  UASSERT(rows*cols == (int)data.size());
351  UASSERT(rows == 3 && cols == 4);
352  localTransform_ = Transform(
353  data[0], data[1], data[2], data[3],
354  data[4], data[5], data[6], data[7],
355  data[8], data[9], data[10], data[11]);
356  }
357  else
358  {
359  UWARN("Missing \"local_transform\" field in \"%s\"", filePath.c_str());
360  }
361 
362  fs.release();
363 
364  if(isValidForRectification())
365  {
366  initRectificationMap();
367  }
368 
369  return true;
370  }
371  catch(const cv::Exception & e)
372  {
373  UERROR("Error reading calibration file \"%s\": %s (Make sure the first line of the yaml file is \"%YAML:1.0\")", filePath.c_str(), e.what());
374  }
375  }
376  else
377  {
378  UWARN("Could not load calibration file \"%s\".", filePath.c_str());
379  }
380  return false;
381 }
382 
383 bool CameraModel::load(const std::string & directory, const std::string & cameraName)
384 {
385  return load(directory+"/"+cameraName+".yaml");
386 }
387 
388 bool CameraModel::save(const std::string & directory) const
389 {
390  if(name_.empty())
391  {
392  UWARN("Camera name is empty, will use general \"camera\" as name.");
393  }
394  std::string filePath = directory+"/"+(name_.empty()?"camera":name_)+".yaml";
395  if(!filePath.empty() && (!K_.empty() || !D_.empty() || !R_.empty() || !P_.empty()))
396  {
397  UINFO("Saving calibration to file \"%s\"", filePath.c_str());
398  cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
399 
400  // export in ROS calibration format
401 
402  if(!name_.empty())
403  {
404  fs << "camera_name" << name_;
405  }
406  if(imageSize_.width>0 && imageSize_.height>0)
407  {
408  fs << "image_width" << imageSize_.width;
409  fs << "image_height" << imageSize_.height;
410  }
411 
412  if(!K_.empty())
413  {
414  fs << "camera_matrix" << "{";
415  fs << "rows" << K_.rows;
416  fs << "cols" << K_.cols;
417  fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
418  fs << "}";
419  }
420 
421  if(!D_.empty())
422  {
423  cv::Mat D = D_;
424  if(D_.cols == 6)
425  {
426  D = cv::Mat(1,4,CV_64FC1);
427  D.at<double>(0,0) = D_.at<double>(0,0);
428  D.at<double>(0,1) = D_.at<double>(0,1);
429  D.at<double>(0,2) = D_.at<double>(0,4);
430  D.at<double>(0,3) = D_.at<double>(0,5);
431  }
432  fs << "distortion_coefficients" << "{";
433  fs << "rows" << D.rows;
434  fs << "cols" << D.cols;
435  fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
436  fs << "}";
437 
438  // compaibility with ROS
439  if(D_.cols == 6)
440  {
441  fs << "distortion_model" << "equidistant"; // equidistant, fisheye
442  }
443  else if(D.cols > 5)
444  {
445  fs << "distortion_model" << "rational_polynomial"; // rad tan
446  }
447  else
448  {
449  fs << "distortion_model" << "plumb_bob"; // rad tan
450  }
451  }
452 
453  if(!R_.empty())
454  {
455  fs << "rectification_matrix" << "{";
456  fs << "rows" << R_.rows;
457  fs << "cols" << R_.cols;
458  fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
459  fs << "}";
460  }
461 
462  if(!P_.empty())
463  {
464  fs << "projection_matrix" << "{";
465  fs << "rows" << P_.rows;
466  fs << "cols" << P_.cols;
467  fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
468  fs << "}";
469  }
470 
471  if(!localTransform_.isNull())
472  {
473  fs << "local_transform" << "{";
474  fs << "rows" << 3;
475  fs << "cols" << 4;
476  fs << "data" << std::vector<float>((float*)localTransform_.data(), ((float*)localTransform_.data())+12);
477  fs << "}";
478  }
479 
480  fs.release();
481 
482  return true;
483  }
484  else
485  {
486  UERROR("Cannot save calibration to \"%s\" because it is empty.", filePath.c_str());
487  }
488  return false;
489 }
490 
491 std::vector<unsigned char> CameraModel::serialize() const
492 {
493  const int headerSize = 11;
494  int header[headerSize] = {
495  RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
496  0, //mono // 3,
497  imageSize_.width, imageSize_.height, // 4,5
498  (int)K_.total(), (int)D_.total(), (int)R_.total(), (int)P_.total(), // 6,7,8,9
499  localTransform_.isNull()?0:localTransform_.size()}; // 10
500  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]);
501  std::vector<unsigned char> data(
502  sizeof(int)*headerSize +
503  sizeof(double)*(K_.total()+D_.total()+R_.total()+P_.total()) +
504  (localTransform_.isNull()?0:sizeof(float)*localTransform_.size()));
505  memcpy(data.data(), header, sizeof(int)*headerSize);
506  int index = sizeof(int)*headerSize;
507  if(!K_.empty())
508  {
509  memcpy(data.data()+index, K_.data, sizeof(double)*(K_.total()));
510  index+=sizeof(double)*(K_.total());
511  }
512  if(!D_.empty())
513  {
514  memcpy(data.data()+index, D_.data, sizeof(double)*(D_.total()));
515  index+=sizeof(double)*(D_.total());
516  }
517  if(!R_.empty())
518  {
519  memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
520  index+=sizeof(double)*(R_.total());
521  }
522  if(!P_.empty())
523  {
524  memcpy(data.data()+index, P_.data, sizeof(double)*(P_.total()));
525  index+=sizeof(double)*(P_.total());
526  }
527  if(!localTransform_.isNull())
528  {
529  memcpy(data.data()+index, localTransform_.data(), sizeof(float)*(localTransform_.size()));
530  index+=sizeof(float)*(localTransform_.size());
531  }
532  return data;
533 }
534 
535 unsigned int CameraModel::deserialize(const std::vector<unsigned char>& data)
536 {
537  return deserialize(data.data(), data.size());
538 }
539 unsigned int CameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
540 {
541  *this = CameraModel();
542  int headerSize = 11;
543  if(dataSize >= sizeof(int)*headerSize)
544  {
545  UASSERT(data != 0);
546  const int * header = (const int *)data;
547  int type = header[3];
548  if(type == 0)
549  {
550  imageSize_.width = header[4];
551  imageSize_.height = header[5];
552  int iK = 6;
553  int iD = 7;
554  int iR = 8;
555  int iP = 9;
556  int iL = 10;
557  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]);
558  unsigned int requiredDataSize = sizeof(int)*headerSize +
559  sizeof(double)*(header[iK]+header[iD]+header[iR]+header[iP]) +
560  sizeof(float)*header[iL];
561  UASSERT_MSG(dataSize >= requiredDataSize,
562  uFormat("dataSize=%d != required=%d (header: version %d.%d.%d %dx%d type=%d K=%d D=%d R=%d P=%d L=%d)",
563  dataSize,
564  requiredDataSize,
565  header[0], header[1], header[2], header[4], header[5], header[3],
566  header[iK], header[iD], header[iR],header[iP], header[iL]).c_str());
567  unsigned int index = sizeof(int)*headerSize;
568  if(header[iK] != 0)
569  {
570  UASSERT(header[iK] == 9);
571  K_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
572  index+=sizeof(double)*(K_.total());
573  }
574  if(header[iD] != 0)
575  {
576  D_ = cv::Mat(1, header[iD], CV_64FC1, (void*)(data+index)).clone();
577  index+=sizeof(double)*(D_.total());
578  }
579  if(header[iR] != 0)
580  {
581  UASSERT(header[iR] == 9);
582  R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
583  index+=sizeof(double)*(R_.total());
584  }
585  if(header[iP] != 0)
586  {
587  UASSERT(header[iP] == 12);
588  P_ = cv::Mat(3, 4, CV_64FC1, (void*)(data+index)).clone();
589  index+=sizeof(double)*(P_.total());
590  }
591  if(header[iL] != 0)
592  {
593  UASSERT(header[iL] == 12);
594  memcpy(localTransform_.data(), data+index, sizeof(float)*localTransform_.size());
595  index+=sizeof(float)*localTransform_.size();
596  }
597  UASSERT(index <= dataSize);
598  return index;
599  }
600  else
601  {
602  UERROR("Serialized calibration is not mono (type=%d), use the appropriate class matching the type to deserialize.", type);
603  }
604  }
605  UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
606  return 0;
607 }
608 
609 CameraModel CameraModel::scaled(double scale) const
610 {
611  CameraModel scaledModel = *this;
612  UASSERT(scale > 0.0);
613  if(this->isValidForProjection())
614  {
615  // has only effect on K and P
616  cv::Mat K;
617  if(!K_.empty())
618  {
619  K = K_.clone();
620  K.at<double>(0,0) *= scale;
621  K.at<double>(1,1) *= scale;
622  K.at<double>(0,2) *= scale;
623  K.at<double>(1,2) *= scale;
624  }
625 
626  cv::Mat P;
627  if(!P_.empty())
628  {
629  P = P_.clone();
630  P.at<double>(0,0) *= scale;
631  P.at<double>(1,1) *= scale;
632  P.at<double>(0,2) *= scale;
633  P.at<double>(1,2) *= scale;
634  P.at<double>(0,3) *= scale;
635  P.at<double>(1,3) *= scale;
636  }
637  scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
638  }
639  else
640  {
641  UWARN("Trying to scale a camera model not valid! Ignoring scaling...");
642  }
643  return scaledModel;
644 }
645 
646 CameraModel CameraModel::roi(const cv::Rect & roi) const
647 {
648  CameraModel roiModel = *this;
649  if(this->isValidForProjection())
650  {
651  // has only effect on cx and cy
652  cv::Mat K;
653  if(!K_.empty())
654  {
655  K = K_.clone();
656  K.at<double>(0,2) -= roi.x;
657  K.at<double>(1,2) -= roi.y;
658  }
659 
660  cv::Mat P;
661  if(!P_.empty())
662  {
663  P = P_.clone();
664  P.at<double>(0,2) -= roi.x;
665  P.at<double>(1,2) -= roi.y;
666  }
667  roiModel = CameraModel(name_, roi.size(), K, D_, R_, P, localTransform_);
668  }
669  else
670  {
671  UWARN("Trying to extract roi from a camera model not valid! Ignoring roi...");
672  }
673  return roiModel;
674 }
675 
676 double CameraModel::fovX() const
677 {
678  return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;
679 }
680 double CameraModel::fovY() const
681 {
682  return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;
683 }
684 
685 double CameraModel::horizontalFOV() const
686 {
687  return fovX()*180.0/CV_PI;
688 }
689 
690 double CameraModel::verticalFOV() const
691 {
692  return fovY()*180.0/CV_PI;
693 }
694 
695 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
696 {
697  UDEBUG("");
698  if(!mapX_.empty() && !mapY_.empty())
699  {
700  cv::Mat rectified;
701  cv::remap(raw, rectified, mapX_, mapY_, interpolation);
702  return rectified;
703  }
704  else
705  {
706  UERROR("Cannot rectify image because the rectify map is not initialized.");
707  return raw.clone();
708  }
709 }
710 
711 //inspired from https://github.com/code-iai/iai_kinect2/blob/master/depth_registration/src/depth_registration_cpu.cpp
712 cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
713 {
714  UDEBUG("");
715  UASSERT(raw.type() == CV_16UC1);
716  if(!mapX_.empty() && !mapY_.empty())
717  {
718  cv::Mat rectified = cv::Mat::zeros(mapX_.rows, mapX_.cols, raw.type());
719  for(int y=0; y<mapX_.rows; ++y)
720  {
721  for(int x=0; x<mapX_.cols; ++x)
722  {
723  cv::Point2f pt(mapX_.at<float>(y,x), mapY_.at<float>(y,x));
724  int xL = (int)floor(pt.x);
725  int xH = (int)ceil(pt.x);
726  int yL = (int)floor(pt.y);
727  int yH = (int)ceil(pt.y);
728  if(xL >= 0 && yL >= 0 && xH < raw.cols && yH < raw.rows)
729  {
730  const unsigned short & pLT = raw.at<unsigned short>(yL, xL);
731  const unsigned short & pRT = raw.at<unsigned short>(yL, xH);
732  const unsigned short & pLB = raw.at<unsigned short>(yH, xL);
733  const unsigned short & pRB = raw.at<unsigned short>(yH, xH);
734  if(pLT > 0 && pRT > 0 && pLB > 0 && pRB > 0)
735  {
736  unsigned short avg = (pLT + pRT + pLB + pRB) / 4;
737  unsigned short thres = 0.01 * avg;
738  if( abs(pLT - avg) < thres &&
739  abs(pRT - avg) < thres &&
740  abs(pLB - avg) < thres &&
741  abs(pRB - avg) < thres)
742  {
743  //bilinear interpolation
744  float a = pt.x - (float)xL;
745  float c = pt.y - (float)yL;
746 
747  //http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
748  rectified.at<unsigned short>(y,x) =
749  (pLT * (1.f - a) + pRT * a) * (1.f - c) +
750  (pLB * (1.f - a) + pRB * a) * c;
751  }
752  }
753  }
754  }
755  }
756  return rectified;
757  }
758  else
759  {
760  UERROR("Cannot rectify image because the rectify map is not initialized.");
761  return raw.clone();
762  }
763 }
764 
765 // resulting 3D point is in /camera_link frame
766 void CameraModel::project(float u, float v, float depth, float & x, float & y, float & z) const
767 {
768  if(depth > 0.0f)
769  {
770  // Fill in XYZ
771  x = (u - cx()) * depth / fx();
772  y = (v - cy()) * depth / fy();
773  z = depth;
774  }
775  else
776  {
777  x = y = z = std::numeric_limits<float>::quiet_NaN();
778  }
779 }
780 // 3D point is in /camera_link frame
781 void CameraModel::reproject(float x, float y, float z, float & u, float & v) const
782 {
783  UASSERT(z!=0.0f);
784  float invZ = 1.0f/z;
785  u = (fx()*x)*invZ + cx();
786  v = (fy()*y)*invZ + cy();
787 }
788 void CameraModel::reproject(float x, float y, float z, int & u, int & v) const
789 {
790  UASSERT(z!=0.0f);
791  float invZ = 1.0f/z;
792  u = (fx()*x)*invZ + cx();
793  v = (fy()*y)*invZ + cy();
794 }
795 
796 bool CameraModel::inFrame(int u, int v) const
797 {
798  return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
799 }
800 
801 std::ostream& operator<<(std::ostream& os, const CameraModel& model)
802 {
803  os << "Name: " << model.name().c_str() << std::endl
804  << "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
805  << "K= " << model.K_raw() << std::endl
806  << "D= " << model.D_raw() << std::endl
807  << "R= " << model.R() << std::endl
808  << "P= " << model.P() << std::endl
809  << "LocalTransform= " << model.localTransform();
810  return os;
811 }
812 
813 } /* namespace rtabmap */
int
int
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
UINFO
#define UINFO(...)
name
cy
const double cy
D
MatrixXcd D
rtabmap::CameraModel::initRectificationMap
bool initRectificationMap()
Definition: CameraModel.cpp:156
fx
const double fx
c
Scalar Scalar * c
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
UDirectory.h
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CameraModel::D_
cv::Mat D_
Definition: CameraModel.h:154
name_
std::string name_
glm::project
GLM_FUNC_DECL detail::tvec3< T, P > project(detail::tvec3< T, P > const &obj, detail::tmat4x4< T, P > const &model, detail::tmat4x4< T, P > const &proj, detail::tvec4< U, P > const &viewport)
type
y
Matrix3f y
os
ofstream os("timeSchurFactors.csv")
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:111
glm::floor
GLM_FUNC_DECL genType floor(genType const &x)
rows
int rows
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UMath.h
Basic mathematics functions.
rtabmap::CameraModel::Tx
double Tx() const
Definition: CameraModel.h:106
n
int n
n2
n2
data
int data[]
eigen_extensions::deserialize
void deserialize(std::istream &strm, Eigen::Matrix< S, T, U > *mat)
Definition: eigen_extensions.h:78
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
UConversion.h
Some conversion functions.
glm::ceil
GLM_FUNC_DECL genType ceil(genType const &x)
eigen_extensions::serialize
void serialize(const Eigen::Matrix< S, T, U > &mat, std::ostream &strm)
Definition: eigen_extensions.h:66
rtabmap::CameraModel::mapX_
cv::Mat mapX_
Definition: CameraModel.h:157
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
UASSERT
#define UASSERT(condition)
rtabmap::CameraModel::mapY_
cv::Mat mapY_
Definition: CameraModel.h:158
z
z
x
x
uStrContains
bool uStrContains(const std::string &string, const std::string &substring)
Definition: UStl.h:785
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:932
glm::detail::operator<<
GLM_FUNC_DECL tvec1< T, P > operator<<(tvec1< T, P > const &v, T const &s)
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
K
K
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
header
std_msgs::Header const * header(const M &m)
rtabmap::Transform
Definition: Transform.h:41
rtabmap::CameraModel::imageSize_
cv::Size imageSize_
Definition: CameraModel.h:152
rtabmap::CameraModel::P_
cv::Mat P_
Definition: CameraModel.h:156
PointMatcherSupport::Parametrizable
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::CameraModel::R_
cv::Mat R_
Definition: CameraModel.h:155
float
float
rtabmap::CameraModel::K_
cv::Mat K_
Definition: CameraModel.h:153
Matrix< Scalar, Dynamic, Dynamic >
cv
Definition: Features2d.h:41
CameraModel.h
rtabmap::CameraModel::CameraModel
CameraModel()
Definition: CameraModel.cpp:40
cx
const double cx
cols
int cols
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
load
NearestNeighbourSearch< T >::Matrix load(const char *fileName)
fy
const double fy
R
R

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07