StereoCameraModel.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/StereoCameraModel.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 <opencv2/imgproc/imgproc.hpp>
35 
36 #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)))
37 #include <rtabmap/core/stereo/stereoRectifyFisheye.h>
38 #endif
39 
40 namespace rtabmap {
41 
42 StereoCameraModel::StereoCameraModel(
43  const std::string & name,
44  const cv::Size & imageSize1,
45  const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
46  const cv::Size & imageSize2,
47  const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
48  const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
49  const Transform & localTransform) :
50  leftSuffix_("left"),
51  rightSuffix_("right"),
52  left_(name+"_"+leftSuffix_, imageSize1, K1, D1, R1, P1, localTransform),
53  right_(name+"_"+rightSuffix_, imageSize2, K2, D2, R2, P2, localTransform),
54  name_(name),
55  R_(R),
56  T_(T),
57  E_(E),
58  F_(F)
59 {
60  UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
61  UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
62  UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
63  UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
64 }
65 
66 StereoCameraModel::StereoCameraModel(
67  const std::string & name,
68  const CameraModel & leftCameraModel,
69  const CameraModel & rightCameraModel,
70  const cv::Mat & R,
71  const cv::Mat & T,
72  const cv::Mat & E,
73  const cv::Mat & F) :
74  leftSuffix_("left"),
75  rightSuffix_("right"),
76  left_(leftCameraModel),
77  right_(rightCameraModel),
78  name_(name),
79  R_(R),
80  T_(T),
81  E_(E),
82  F_(F)
83 {
84  left_.setName(name+"_"+getLeftSuffix());
85  right_.setName(name+"_"+getRightSuffix());
86  UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
87  UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
88  UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
89  UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
90 
91  if(!R_.empty() && !T_.empty())
92  {
93  UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
94 
95  if(left_.imageWidth() == right_.imageWidth() && left_.imageHeight() == right_.imageHeight())
96  {
97  updateStereoRectification();
98  }
99  }
100 }
101 
102 StereoCameraModel::StereoCameraModel(
103  const std::string & name,
104  const CameraModel & leftCameraModel,
105  const CameraModel & rightCameraModel,
106  const Transform & extrinsics) :
107  leftSuffix_("left"),
108  rightSuffix_("right"),
109  left_(leftCameraModel),
110  right_(rightCameraModel),
111  name_(name)
112 {
113  left_.setName(name+"_"+getLeftSuffix());
114  right_.setName(name+"_"+getRightSuffix());
115 
116  if(!extrinsics.isNull())
117  {
118  UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
119 
120  extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
121  extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
122 
123  if(left_.imageWidth() == right_.imageWidth() && left_.imageHeight() == right_.imageHeight())
124  {
125  updateStereoRectification();
126  }
127  }
128 }
129 
130 StereoCameraModel::StereoCameraModel(
131  double fx,
132  double fy,
133  double cx,
134  double cy,
135  double baseline,
136  const Transform & localTransform,
137  const cv::Size & imageSize) :
138  leftSuffix_("left"),
139  rightSuffix_("right"),
140  left_(fx, fy, cx, cy, localTransform, 0, imageSize),
141  right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
142 {
143 }
144 
145 //minimal to be saved
146 StereoCameraModel::StereoCameraModel(
147  const std::string & name,
148  double fx,
149  double fy,
150  double cx,
151  double cy,
152  double baseline,
153  const Transform & localTransform,
154  const cv::Size & imageSize) :
155  leftSuffix_("left"),
156  rightSuffix_("right"),
157  left_(name+"_"+getLeftSuffix(), fx, fy, cx, cy, localTransform, 0, imageSize),
158  right_(name+"_"+getRightSuffix(), fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
159  name_(name)
160 {
161 }
162 
163 void StereoCameraModel::setName(const std::string & name, const std::string & leftSuffix, const std::string & rightSuffix)
164 {
165  name_=name;
166  leftSuffix_ = leftSuffix;
167  rightSuffix_ = rightSuffix;
168  left_.setName(name_+"_"+getLeftSuffix());
169  right_.setName(name_+"_"+getRightSuffix());
170 }
171 
172 void StereoCameraModel::updateStereoRectification()
173 {
174  cv::Mat R1,R2,P1,P2,Q;
175  #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)))
176  bool fishEye = left_.D_raw().cols == 6;
177  // calibrate extrinsic
178  if(fishEye)
179  {
180  cv::Vec4d D_left(left_.D_raw().at<double>(0,0), left_.D_raw().at<double>(0,1), left_.D_raw().at<double>(0,4), left_.D_raw().at<double>(0,5));
181  cv::Vec4d D_right(right_.D_raw().at<double>(0,0), right_.D_raw().at<double>(0,1), right_.D_raw().at<double>(0,4), right_.D_raw().at<double>(0,5));
182 
183  stereoRectifyFisheye(
184  left_.K_raw(), D_left,
185  right_.K_raw(), D_right,
186  left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
187  cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
188 
189  // Re-zoom to original focal distance
190  if(P1.at<double>(0,0) < 0)
191  {
192  P1.at<double>(0,0) *= -1;
193  P1.at<double>(1,1) *= -1;
194  }
195  if(P2.at<double>(0,0) < 0)
196  {
197  P2.at<double>(0,0) *= -1;
198  P2.at<double>(1,1) *= -1;
199  }
200  if(P2.at<double>(0,3) > 0)
201  {
202  P2.at<double>(0,3) *= -1;
203  }
204  P2.at<double>(0,3) = P2.at<double>(0,3) * left_.K_raw().at<double>(0,0) / P2.at<double>(0,0);
205  P1.at<double>(0,0) = P1.at<double>(1,1) = left_.K_raw().at<double>(0,0);
206  P2.at<double>(0,0) = P2.at<double>(1,1) = left_.K_raw().at<double>(0,0);
207  }
208  else
209 #endif
210  {
211 
212  cv::stereoRectify(
213  left_.K_raw(), left_.D_raw(),
214  right_.K_raw(), right_.D_raw(),
215  left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
216  cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
217  }
218 
219  left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
220  right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
221 }
222 
223 bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
224 {
225  name_ = cameraName;
226  bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix());
227  bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix());
228  if(leftLoaded && rightLoaded)
229  {
230  if(ignoreStereoTransform)
231  {
232  return true;
233  }
234  //load rotation, translation
235  R_ = cv::Mat();
236  T_ = cv::Mat();
237  E_ = cv::Mat();
238  F_ = cv::Mat();
239 
240  std::string filePath = directory+"/"+cameraName+"_pose.yaml";
241  if(UFile::exists(filePath))
242  {
243  UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
244  cv::FileStorage fs(filePath, cv::FileStorage::READ);
245 
246  cv::FileNode n;
247 
248  n = fs["camera_name"];
249  if(n.type() != cv::FileNode::NONE)
250  {
251  name_ = (int)n;
252  }
253  else
254  {
255  UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
256  }
257 
258  // import from ROS calibration format
259  n = fs["rotation_matrix"];
260  if(n.type() != cv::FileNode::NONE)
261  {
262  int rows = (int)n["rows"];
263  int cols = (int)n["cols"];
264  std::vector<double> data;
265  n["data"] >> data;
266  UASSERT(rows*cols == (int)data.size());
267  UASSERT(rows == 3 && cols == 3);
268  R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
269  }
270  else
271  {
272  UWARN("Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
273  }
274 
275  n = fs["translation_matrix"];
276  if(n.type() != cv::FileNode::NONE)
277  {
278  int rows = (int)n["rows"];
279  int cols = (int)n["cols"];
280  std::vector<double> data;
281  n["data"] >> data;
282  UASSERT(rows*cols == (int)data.size());
283  UASSERT(rows == 3 && cols == 1);
284  T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
285  }
286  else
287  {
288  UWARN("Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
289  }
290 
291  n = fs["essential_matrix"];
292  if(n.type() != cv::FileNode::NONE)
293  {
294  int rows = (int)n["rows"];
295  int cols = (int)n["cols"];
296  std::vector<double> data;
297  n["data"] >> data;
298  UASSERT(rows*cols == (int)data.size());
299  UASSERT(rows == 3 && cols == 3);
300  E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
301  }
302  else
303  {
304  UWARN("Missing \"essential_matrix\" field in \"%s\"", filePath.c_str());
305  }
306 
307  n = fs["fundamental_matrix"];
308  if(n.type() != cv::FileNode::NONE)
309  {
310  int rows = (int)n["rows"];
311  int cols = (int)n["cols"];
312  std::vector<double> data;
313  n["data"] >> data;
314  UASSERT(rows*cols == (int)data.size());
315  UASSERT(rows == 3 && cols == 3);
316  F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
317  }
318  else
319  {
320  UWARN("Missing \"fundamental_matrix\" field in \"%s\"", filePath.c_str());
321  }
322 
323  fs.release();
324 
325  return true;
326  }
327  else
328  {
329  UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
330  }
331  }
332  return false;
333 }
334 bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
335 {
336  if(left_.save(directory) && right_.save(directory))
337  {
338  if(ignoreStereoTransform)
339  {
340  return true;
341  }
342  return saveStereoTransform(directory);
343  }
344  return false;
345 }
346 
347 bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
348 {
349  std::string filePath = directory+"/"+name_+"_pose.yaml";
350  if(!filePath.empty() && (!R_.empty() && !T_.empty()))
351  {
352  UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
353  cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
354 
355  // export in ROS calibration format
356 
357  if(!name_.empty())
358  {
359  fs << "camera_name" << name_;
360  }
361 
362  if(!R_.empty())
363  {
364  fs << "rotation_matrix" << "{";
365  fs << "rows" << R_.rows;
366  fs << "cols" << R_.cols;
367  fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
368  fs << "}";
369  }
370 
371  if(!T_.empty())
372  {
373  fs << "translation_matrix" << "{";
374  fs << "rows" << T_.rows;
375  fs << "cols" << T_.cols;
376  fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
377  fs << "}";
378  }
379 
380  if(!E_.empty())
381  {
382  fs << "essential_matrix" << "{";
383  fs << "rows" << E_.rows;
384  fs << "cols" << E_.cols;
385  fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
386  fs << "}";
387  }
388 
389  if(!F_.empty())
390  {
391  fs << "fundamental_matrix" << "{";
392  fs << "rows" << F_.rows;
393  fs << "cols" << F_.cols;
394  fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
395  fs << "}";
396  }
397 
398  fs.release();
399 
400  return true;
401  }
402  else
403  {
404  UERROR("Failed saving stereo extrinsics (they are null):");
405  std::cout << "R= " << R_ << std::endl;
406  std::cout << "T= " << T_ << std::endl;
407  std::cout << "E= " << T_ << std::endl;
408  std::cout << "F= " << F_ << std::endl;
409  }
410  return false;
411 }
412 
413 std::vector<unsigned char> StereoCameraModel::serialize() const
414 {
415  std::vector<unsigned char> leftData = left_.serialize();
416  std::vector<unsigned char> rightData = right_.serialize();
417 
418  const int headerSize = 10;
419  int header[headerSize] = {
420  RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH, // 0,1,2
421  1, //stereo // 3
422  (int)R_.total(), (int)T_.total(), (int)E_.total(), (int)F_.total(), // 4,5,6,7
423  (int)leftData.size(), (int)rightData.size()}; // 8,9
424  UDEBUG("Header: %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]);
425  std::vector<unsigned char> data(
426  sizeof(int)*headerSize +
427  sizeof(double)*(R_.total()+T_.total()+E_.total()+F_.total()) +
428  leftData.size() + rightData.size());
429  memcpy(data.data(), header, sizeof(int)*headerSize);
430  int index = sizeof(int)*headerSize;
431  if(!R_.empty())
432  {
433  memcpy(data.data()+index, R_.data, sizeof(double)*(R_.total()));
434  index+=sizeof(double)*(R_.total());
435  }
436  if(!T_.empty())
437  {
438  memcpy(data.data()+index, T_.data, sizeof(double)*(T_.total()));
439  index+=sizeof(double)*(T_.total());
440  }
441  if(!E_.empty())
442  {
443  memcpy(data.data()+index, E_.data, sizeof(double)*(E_.total()));
444  index+=sizeof(double)*(E_.total());
445  }
446  if(!F_.empty())
447  {
448  memcpy(data.data()+index, F_.data, sizeof(double)*(F_.total()));
449  index+=sizeof(double)*(F_.total());
450  }
451  if(leftData.size())
452  {
453  memcpy(data.data()+index, leftData.data(), leftData.size());
454  index+=leftData.size();
455  }
456  if(rightData.size())
457  {
458  memcpy(data.data()+index, rightData.data(), rightData.size());
459  index+=rightData.size();
460  }
461  return data;
462 }
463 
464 unsigned int StereoCameraModel::deserialize(const std::vector<unsigned char>& data)
465 {
466  return deserialize(data.data(), data.size());
467 }
468 unsigned int StereoCameraModel::deserialize(const unsigned char * data, unsigned int dataSize)
469 {
470  *this = StereoCameraModel();
471  int headerSize = 10;
472  if(dataSize >= sizeof(int)*headerSize)
473  {
474  int iR = 4;
475  int iT = 5;
476  int iE = 6;
477  int iF = 7;
478  int iLeft = 8;
479  int iRight = 9;
480  const int * header = (const int *)data;
481  UDEBUG("Header: %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]);
482  int type = header[3];
483  if(type==1)
484  {
485  unsigned int requiredDataSize = sizeof(int)*headerSize +
486  sizeof(double)*(header[iR]+header[iT]+header[iE]+header[iF]) +
487  header[iLeft] + header[iRight];
488  UASSERT_MSG(dataSize >= requiredDataSize,
489  uFormat("dataSize=%d != required=%d (header: version %d.%d.%d type=%d R=%d T=%d E=%d F=%d Left=%d Right=%d)",
490  dataSize,
491  requiredDataSize,
492  header[0], header[1], header[2], header[3],
493  header[iR], header[iT], header[iE],header[iF], header[iLeft], header[iRight]).c_str());
494 
495  unsigned int index = sizeof(int)*headerSize;
496 
497  if(header[iR] != 0)
498  {
499  UASSERT(header[iR] == 9);
500  R_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
501  index+=sizeof(double)*(R_.total());
502  }
503 
504  if(header[iT] != 0)
505  {
506  UASSERT(header[iT] == 3);
507  T_ = cv::Mat(3, 1, CV_64FC1, (void*)(data+index)).clone();
508  index+=sizeof(double)*(T_.total());
509  }
510 
511  if(header[iE] != 0)
512  {
513  UASSERT(header[iE] == 9);
514  E_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
515  index+=sizeof(double)*(E_.total());
516  }
517 
518  if(header[iF] != 0)
519  {
520  UASSERT(header[iF] == 9);
521  F_ = cv::Mat(3, 3, CV_64FC1, (void*)(data+index)).clone();
522  index+=sizeof(double)*(F_.total());
523  }
524 
525  if(header[iLeft] != 0)
526  {
527  index += left_.deserialize((data+index), header[iLeft]);
528  }
529 
530  if(header[iRight] != 0)
531  {
532  index += right_.deserialize((data+index), header[iRight]);
533  }
534 
535  UASSERT(index <= dataSize);
536 
537  return index;
538  }
539  else
540  {
541  UERROR("Serialized calibration is not stereo (type=%d), use the appropriate class matching the type to deserialize.", type);
542  }
543  }
544  UERROR("Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (int)dataSize);
545  return 0;
546 }
547 
548 void StereoCameraModel::scale(double scale)
549 {
550  left_ = left_.scaled(scale);
551  right_ = right_.scaled(scale);
552 }
553 
554 void StereoCameraModel::roi(const cv::Rect & roi)
555 {
556  left_ = left_.roi(roi);
557  right_ = right_.roi(roi);
558 }
559 
560 float StereoCameraModel::computeDepth(float disparity) const
561 {
562  //depth = baseline * f / (disparity + cx1-cx0);
563  UASSERT(this->isValidForProjection());
564  if(disparity == 0.0f)
565  {
566  return 0.0f;
567  }
568  return baseline() * left().fx() / (disparity + right().cx() - left().cx());
569 }
570 
571 float StereoCameraModel::computeDisparity(float depth) const
572 {
573  // disparity = (baseline * fx / depth) - (cx1-cx0);
574  UASSERT(this->isValidForProjection());
575  if(depth == 0.0f)
576  {
577  return 0.0f;
578  }
579  return baseline() * left().fx() / depth - right().cx() + left().cx();
580 }
581 
582 float StereoCameraModel::computeDisparity(unsigned short depth) const
583 {
584  // disparity = (baseline * fx / depth) - (cx1-cx0);
585  UASSERT(this->isValidForProjection());
586  if(depth == 0)
587  {
588  return 0.0f;
589  }
590  return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
591 }
592 
593 Transform StereoCameraModel::stereoTransform() const
594 {
595  if(!R_.empty() && !T_.empty())
596  {
597  return Transform(
598  R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
599  R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
600  R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
601  }
602  return Transform();
603 }
604 
605 std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model)
606 {
607  os << "Left Camera " << model.left() << std::endl
608  << "Right Camera " << model.right() << std::endl
609  << "Stereo Extrinsics:" << std::endl
610  << "R= " << model.R() << std::endl
611  << "T= " << model.T() << std::endl
612  << "E= " << model.E() << std::endl
613  << "F= "<< model.F() << std::endl
614  << "baseline= " << model.baseline() << std::endl;
615  return os;
616 }
617 
618 } /* namespace rtabmap */
rtabmap::StereoCameraModel::save
bool save(const std::string &directory, bool ignoreStereoTransform=true) const
Definition: StereoCameraModel.cpp:334
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::CameraModel::serialize
std::vector< unsigned char > serialize() const
Definition: CameraModel.cpp:491
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
rtabmap::CameraModel::scaled
CameraModel scaled(double scale) const
Definition: CameraModel.cpp:609
rtabmap::StereoCameraModel::saveStereoTransform
bool saveStereoTransform(const std::string &directory) const
Definition: StereoCameraModel.cpp:347
rtabmap::CameraModel::save
bool save(const std::string &directory) const
Definition: CameraModel.cpp:388
f
f
name_
std::string name_
rtabmap::Transform::rotationMatrix
cv::Mat rotationMatrix() const
Definition: Transform.cpp:238
data
data
rtabmap::StereoCameraModel::deserialize
unsigned int deserialize(const std::vector< unsigned char > &data)
Definition: StereoCameraModel.cpp:464
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::CameraModel::load
bool load(const std::string &filePath)
Definition: CameraModel.cpp:215
header
std_msgs::Header * header(M &m)
UConversion.h
Some conversion functions.
rtabmap::StereoCameraModel::T
const cv::Mat & T() const
Definition: StereoCameraModel.h:111
rtabmap::StereoCameraModel::name
const std::string & name() const
Definition: StereoCameraModel.h:92
rtabmap::StereoCameraModel::serialize
std::vector< unsigned char > serialize() const
Definition: StereoCameraModel.cpp:413
UASSERT
#define UASSERT(condition)
rtabmap::StereoCameraModel::F
const cv::Mat & F() const
Definition: StereoCameraModel.h:113
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:89
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::StereoCameraModel::computeDisparity
float computeDisparity(float depth) const
Definition: StereoCameraModel.cpp:571
rtabmap::CameraModel::D_raw
cv::Mat D_raw() const
Definition: CameraModel.h:109
rtabmap::StereoCameraModel::getLeftSuffix
const std::string & getLeftSuffix() const
Definition: StereoCameraModel.h:125
rtabmap::CameraModel::K_raw
cv::Mat K_raw() const
Definition: CameraModel.h:108
rtabmap::CameraModel::roi
CameraModel roi(const cv::Rect &roi) const
Definition: CameraModel.cpp:646
rtabmap::stereoRectifyFisheye
void stereoRectifyFisheye(cv::InputArray _cameraMatrix1, cv::InputArray _distCoeffs1, cv::InputArray _cameraMatrix2, cv::InputArray _distCoeffs2, cv::Size imageSize, cv::InputArray _Rmat, cv::InputArray _Tmat, cv::OutputArray _Rmat1, cv::OutputArray _Rmat2, cv::OutputArray _Pmat1, cv::OutputArray _Pmat2, cv::OutputArray _Qmat, int flags, double alpha, cv::Size newImageSize)
Definition: stereoRectifyFisheye.h:1181
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
rtabmap::StereoCameraModel::R
const cv::Mat & R() const
Definition: StereoCameraModel.h:110
rtabmap::StereoCameraModel::roi
void roi(const cv::Rect &roi)
Definition: StereoCameraModel.cpp:554
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::Transform::translationMatrix
cv::Mat translationMatrix() const
Definition: Transform.cpp:243
rtabmap::StereoCameraModel::load
bool load(const std::string &directory, const std::string &cameraName, bool ignoreStereoTransform=true)
Definition: StereoCameraModel.cpp:223
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:119
rtabmap::StereoCameraModel::getRightSuffix
const std::string & getRightSuffix() const
Definition: StereoCameraModel.h:126
UDEBUG
#define UDEBUG(...)
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
rtabmap::operator<<
RTABMAP_EXP std::ostream & operator<<(std::ostream &os, const CameraModel &model)
Definition: CameraModel.cpp:801
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::CameraModel::name
const std::string & name() const
Definition: CameraModel.h:100
rtabmap::CameraModel::deserialize
unsigned int deserialize(const std::vector< unsigned char > &data)
Definition: CameraModel.cpp:535
rtabmap::StereoCameraModel::setName
void setName(const std::string &name, const std::string &leftSuffix="left", const std::string &rightSuffix="right")
Definition: StereoCameraModel.cpp:163
trace.model
model
Definition: trace.py:4
rtabmap::StereoCameraModel::E
const cv::Mat & E() const
Definition: StereoCameraModel.h:112
rtabmap::StereoCameraModel::computeDepth
float computeDepth(float disparity) const
Definition: StereoCameraModel.cpp:560
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::CameraModel::setName
void setName(const std::string &name)
Definition: CameraModel.h:99
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:123
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:57