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/utilite/ULogger.h>
30 #include <rtabmap/utilite/UDirectory.h>
31 #include <rtabmap/utilite/UFile.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <opencv2/imgproc/imgproc.hpp>
34 
35 namespace rtabmap {
36 
37 StereoCameraModel::StereoCameraModel(
38  const std::string & name,
39  const cv::Size & imageSize1,
40  const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
41  const cv::Size & imageSize2,
42  const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
43  const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
44  const Transform & localTransform) :
45  leftSuffix_("left"),
46  rightSuffix_("right"),
47  left_(name+"_"+leftSuffix_, imageSize1, K1, D1, R1, P1, localTransform),
48  right_(name+"_"+rightSuffix_, imageSize2, K2, D2, R2, P2, localTransform),
49  name_(name),
50  R_(R),
51  T_(T),
52  E_(E),
53  F_(F)
54 {
55  UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
56  UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
57  UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
58  UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
59 }
60 
61 StereoCameraModel::StereoCameraModel(
62  const std::string & name,
63  const CameraModel & leftCameraModel,
64  const CameraModel & rightCameraModel,
65  const cv::Mat & R,
66  const cv::Mat & T,
67  const cv::Mat & E,
68  const cv::Mat & F) :
69  leftSuffix_("left"),
70  rightSuffix_("right"),
71  left_(leftCameraModel),
72  right_(rightCameraModel),
73  name_(name),
74  R_(R),
75  T_(T),
76  E_(E),
77  F_(F)
78 {
79  left_.setName(name+"_"+getLeftSuffix());
80  right_.setName(name+"_"+getRightSuffix());
81  UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
82  UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
83  UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
84  UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
85 
86  if(!R_.empty() && !T_.empty())
87  {
88  UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
89 
90  cv::Mat R1,R2,P1,P2,Q;
91  cv::stereoRectify(left_.K_raw(), left_.D_raw(),
92  right_.K_raw(), right_.D_raw(),
93  left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
94  cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
95 
96  left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
97  right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
98  }
99 }
100 
101 StereoCameraModel::StereoCameraModel(
102  const std::string & name,
103  const CameraModel & leftCameraModel,
104  const CameraModel & rightCameraModel,
105  const Transform & extrinsics) :
106  leftSuffix_("left"),
107  rightSuffix_("right"),
108  left_(leftCameraModel),
109  right_(rightCameraModel),
110  name_(name)
111 {
112  left_.setName(name+"_"+getLeftSuffix());
113  right_.setName(name+"_"+getRightSuffix());
114 
115  if(!extrinsics.isNull())
116  {
117  UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
118 
119  extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
120  extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
121 
122  cv::Mat R1,R2,P1,P2,Q;
123  cv::stereoRectify(left_.K_raw(), left_.D_raw(),
124  right_.K_raw(), right_.D_raw(),
125  left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
126  cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
127 
128  left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
129  right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
130  }
131 }
132 
133 StereoCameraModel::StereoCameraModel(
134  double fx,
135  double fy,
136  double cx,
137  double cy,
138  double baseline,
139  const Transform & localTransform,
140  const cv::Size & imageSize) :
141  leftSuffix_("left"),
142  rightSuffix_("right"),
143  left_(fx, fy, cx, cy, localTransform, 0, imageSize),
144  right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
145 {
146 }
147 
148 //minimal to be saved
149 StereoCameraModel::StereoCameraModel(
150  const std::string & name,
151  double fx,
152  double fy,
153  double cx,
154  double cy,
155  double baseline,
156  const Transform & localTransform,
157  const cv::Size & imageSize) :
158  leftSuffix_("left"),
159  rightSuffix_("right"),
160  left_(name+"_"+getLeftSuffix(), fx, fy, cx, cy, localTransform, 0, imageSize),
161  right_(name+"_"+getRightSuffix(), fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
162  name_(name)
163 {
164 }
165 
166 void StereoCameraModel::setName(const std::string & name, const std::string & leftSuffix, const std::string & rightSuffix)
167 {
168  name_=name;
169  leftSuffix_ = leftSuffix;
170  rightSuffix_ = rightSuffix;
171  left_.setName(name_+"_"+getLeftSuffix());
172  right_.setName(name_+"_"+getRightSuffix());
173 }
174 
175 bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
176 {
177  name_ = cameraName;
178  bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix());
179  bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix());
180  if(leftLoaded && rightLoaded)
181  {
182  if(ignoreStereoTransform)
183  {
184  return true;
185  }
186  //load rotation, translation
187  R_ = cv::Mat();
188  T_ = cv::Mat();
189  E_ = cv::Mat();
190  F_ = cv::Mat();
191 
192  std::string filePath = directory+"/"+cameraName+"_pose.yaml";
193  if(UFile::exists(filePath))
194  {
195  UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
196  cv::FileStorage fs(filePath, cv::FileStorage::READ);
197 
198  cv::FileNode n;
199 
200  n = fs["camera_name"];
201  if(n.type() != cv::FileNode::NONE)
202  {
203  name_ = (int)n;
204  }
205  else
206  {
207  UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
208  }
209 
210  // import from ROS calibration format
211  n = fs["rotation_matrix"];
212  if(n.type() != cv::FileNode::NONE)
213  {
214  int rows = (int)n["rows"];
215  int cols = (int)n["cols"];
216  std::vector<double> data;
217  n["data"] >> data;
218  UASSERT(rows*cols == (int)data.size());
219  UASSERT(rows == 3 && cols == 3);
220  R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
221  }
222  else
223  {
224  UWARN("Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
225  }
226 
227  n = fs["translation_matrix"];
228  if(n.type() != cv::FileNode::NONE)
229  {
230  int rows = (int)n["rows"];
231  int cols = (int)n["cols"];
232  std::vector<double> data;
233  n["data"] >> data;
234  UASSERT(rows*cols == (int)data.size());
235  UASSERT(rows == 3 && cols == 1);
236  T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
237  }
238  else
239  {
240  UWARN("Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
241  }
242 
243  n = fs["essential_matrix"];
244  if(n.type() != cv::FileNode::NONE)
245  {
246  int rows = (int)n["rows"];
247  int cols = (int)n["cols"];
248  std::vector<double> data;
249  n["data"] >> data;
250  UASSERT(rows*cols == (int)data.size());
251  UASSERT(rows == 3 && cols == 3);
252  E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
253  }
254  else
255  {
256  UWARN("Missing \"essential_matrix\" field in \"%s\"", filePath.c_str());
257  }
258 
259  n = fs["fundamental_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  F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
269  }
270  else
271  {
272  UWARN("Missing \"fundamental_matrix\" field in \"%s\"", filePath.c_str());
273  }
274 
275  fs.release();
276 
277  return true;
278  }
279  else
280  {
281  UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
282  }
283  }
284  return false;
285 }
286 bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
287 {
288  if(left_.save(directory) && right_.save(directory))
289  {
290  if(ignoreStereoTransform)
291  {
292  return true;
293  }
294  return saveStereoTransform(directory);
295  }
296  return false;
297 }
298 
299 bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
300 {
301  std::string filePath = directory+"/"+name_+"_pose.yaml";
302  if(!filePath.empty() && (!R_.empty() && !T_.empty()))
303  {
304  UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
305  cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
306 
307  // export in ROS calibration format
308 
309  if(!name_.empty())
310  {
311  fs << "camera_name" << name_;
312  }
313 
314  if(!R_.empty())
315  {
316  fs << "rotation_matrix" << "{";
317  fs << "rows" << R_.rows;
318  fs << "cols" << R_.cols;
319  fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
320  fs << "}";
321  }
322 
323  if(!T_.empty())
324  {
325  fs << "translation_matrix" << "{";
326  fs << "rows" << T_.rows;
327  fs << "cols" << T_.cols;
328  fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
329  fs << "}";
330  }
331 
332  if(!E_.empty())
333  {
334  fs << "essential_matrix" << "{";
335  fs << "rows" << E_.rows;
336  fs << "cols" << E_.cols;
337  fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
338  fs << "}";
339  }
340 
341  if(!F_.empty())
342  {
343  fs << "fundamental_matrix" << "{";
344  fs << "rows" << F_.rows;
345  fs << "cols" << F_.cols;
346  fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
347  fs << "}";
348  }
349 
350  fs.release();
351 
352  return true;
353  }
354  else
355  {
356  UERROR("Failed saving stereo extrinsics (they are null).");
357  }
358  return false;
359 }
360 
361 void StereoCameraModel::scale(double scale)
362 {
363  left_ = left_.scaled(scale);
364  right_ = right_.scaled(scale);
365 }
366 
367 void StereoCameraModel::roi(const cv::Rect & roi)
368 {
369  left_ = left_.roi(roi);
370  right_ = right_.roi(roi);
371 }
372 
373 float StereoCameraModel::computeDepth(float disparity) const
374 {
375  //depth = baseline * f / (disparity + cx1-cx0);
376  UASSERT(this->isValidForProjection());
377  if(disparity == 0.0f)
378  {
379  return 0.0f;
380  }
381  return baseline() * left().fx() / (disparity + right().cx() - left().cx());
382 }
383 
384 float StereoCameraModel::computeDisparity(float depth) const
385 {
386  // disparity = (baseline * fx / depth) - (cx1-cx0);
387  UASSERT(this->isValidForProjection());
388  if(depth == 0.0f)
389  {
390  return 0.0f;
391  }
392  return baseline() * left().fx() / depth - right().cx() + left().cx();
393 }
394 
395 float StereoCameraModel::computeDisparity(unsigned short depth) const
396 {
397  // disparity = (baseline * fx / depth) - (cx1-cx0);
398  UASSERT(this->isValidForProjection());
399  if(depth == 0)
400  {
401  return 0.0f;
402  }
403  return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
404 }
405 
406 Transform StereoCameraModel::stereoTransform() const
407 {
408  if(!R_.empty() && !T_.empty())
409  {
410  return Transform(
411  R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
412  R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
413  R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
414  }
415  return Transform();
416 }
417 
418 } /* namespace rtabmap */
rtabmap::CameraModel::K_raw
cv::Mat K_raw() const
Definition: CameraModel.h:101
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:112
rtabmap::StereoCameraModel::R
const cv::Mat & R() const
Definition: StereoCameraModel.h:107
rtabmap::CameraModel::name
const std::string & name() const
Definition: CameraModel.h:93
f
f
rtabmap::StereoCameraModel::stereoTransform
Transform stereoTransform() const
Definition: StereoCameraModel.cpp:406
rtabmap::Transform::rotationMatrix
cv::Mat rotationMatrix() const
Definition: Transform.cpp:196
rtabmap::Transform::translationMatrix
cv::Mat translationMatrix() const
Definition: Transform.cpp:201
rtabmap::CameraModel::save
bool save(const std::string &directory) const
Definition: CameraModel.cpp:362
rtabmap::StereoCameraModel::F
const cv::Mat & F() const
Definition: StereoCameraModel.h:110
UConversion.h
Some conversion functions.
rtabmap::StereoCameraModel::getRightSuffix
const std::string & getRightSuffix() const
Definition: StereoCameraModel.h:123
rtabmap::StereoCameraModel::computeDepth
float computeDepth(float disparity) const
Definition: StereoCameraModel.cpp:373
rtabmap::StereoCameraModel::computeDisparity
float computeDisparity(float depth) const
Definition: StereoCameraModel.cpp:384
UASSERT
#define UASSERT(condition)
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
rtabmap::CameraModel::load
bool load(const std::string &directory, const std::string &cameraName)
Definition: CameraModel.cpp:212
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::StereoCameraModel::T
const cv::Mat & T() const
Definition: StereoCameraModel.h:108
rtabmap::StereoCameraModel::E
const cv::Mat & E() const
Definition: StereoCameraModel.h:109
rtabmap::StereoCameraModel::saveStereoTransform
bool saveStereoTransform(const std::string &directory) const
Definition: StereoCameraModel.cpp:299
rtabmap::CameraModel::isValidForRectification
bool isValidForRectification() const
Definition: CameraModel.h:82
rtabmap::StereoCameraModel::save
bool save(const std::string &directory, bool ignoreStereoTransform=true) const
Definition: StereoCameraModel.cpp:286
rtabmap::CameraModel::roi
CameraModel roi(const cv::Rect &roi) const
Definition: CameraModel.cpp:489
rtabmap::StereoCameraModel::roi
void roi(const cv::Rect &roi)
Definition: StereoCameraModel.cpp:367
rtabmap::StereoCameraModel::load
bool load(const std::string &directory, const std::string &cameraName, bool ignoreStereoTransform=true)
Definition: StereoCameraModel.cpp:175
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:120
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::StereoCameraModel::setName
void setName(const std::string &name, const std::string &leftSuffix="left", const std::string &rightSuffix="right")
Definition: StereoCameraModel.cpp:166
rtabmap::StereoCameraModel::getLeftSuffix
const std::string & getLeftSuffix() const
Definition: StereoCameraModel.h:122
rtabmap::CameraModel::D_raw
cv::Mat D_raw() const
Definition: CameraModel.h:102
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:116
rtabmap::CameraModel::setName
void setName(const std::string &name)
Definition: CameraModel.h:92
rtabmap::CameraModel::scaled
CameraModel scaled(double scale) const
Definition: CameraModel.cpp:452
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109
rtabmap::StereoCameraModel::name
const std::string & name() const
Definition: StereoCameraModel.h:92
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:40