29 #include <rtabmap/core/Version.h> 34 #include <opencv2/imgproc/imgproc.hpp> 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))) 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,
51 rightSuffix_(
"right"),
52 left_(name+
"_"+leftSuffix_, imageSize1, K1, D1, R1, P1, localTransform),
53 right_(name+
"_"+rightSuffix_, imageSize2, K2, D2, R2, P2, localTransform),
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));
67 const std::string &
name,
76 left_(leftCameraModel),
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));
91 if(!
R_.empty() && !
T_.empty())
103 const std::string &
name,
109 left_(leftCameraModel),
137 const cv::Size & imageSize) :
140 left_(fx, fy, cx, cy, localTransform, 0, imageSize),
141 right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
147 const std::string &
name,
154 const cv::Size & imageSize) :
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))) 190 if(P1.at<
double>(0,0) < 0)
192 P1.at<
double>(0,0) *= -1;
193 P1.at<
double>(1,1) *= -1;
195 if(P2.at<
double>(0,0) < 0)
197 P2.at<
double>(0,0) *= -1;
198 P2.at<
double>(1,1) *= -1;
200 if(P2.at<
double>(0,3) > 0)
202 P2.at<
double>(0,3) *= -1;
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);
219 left_ =
CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
228 if(leftLoaded && rightLoaded)
230 if(ignoreStereoTransform)
240 std::string filePath = directory+
"/"+cameraName+
"_pose.yaml";
243 UINFO(
"Reading stereo calibration file \"%s\"", filePath.c_str());
244 cv::FileStorage fs(filePath, cv::FileStorage::READ);
248 n = fs[
"camera_name"];
249 if(n.type() != cv::FileNode::NONE)
255 UWARN(
"Missing \"camera_name\" field in \"%s\"", filePath.c_str());
259 n = fs[
"rotation_matrix"];
260 if(n.type() != cv::FileNode::NONE)
262 int rows = (int)n[
"rows"];
263 int cols = (int)n[
"cols"];
264 std::vector<double> 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();
272 UWARN(
"Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
275 n = fs[
"translation_matrix"];
276 if(n.type() != cv::FileNode::NONE)
278 int rows = (int)n[
"rows"];
279 int cols = (int)n[
"cols"];
280 std::vector<double> 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();
288 UWARN(
"Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
291 n = fs[
"essential_matrix"];
292 if(n.type() != cv::FileNode::NONE)
294 int rows = (int)n[
"rows"];
295 int cols = (int)n[
"cols"];
296 std::vector<double> 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();
304 UWARN(
"Missing \"essential_matrix\" field in \"%s\"", filePath.c_str());
307 n = fs[
"fundamental_matrix"];
308 if(n.type() != cv::FileNode::NONE)
310 int rows = (int)n[
"rows"];
311 int cols = (int)n[
"cols"];
312 std::vector<double> 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();
320 UWARN(
"Missing \"fundamental_matrix\" field in \"%s\"", filePath.c_str());
329 UWARN(
"Could not load stereo calibration file \"%s\".", filePath.c_str());
338 if(ignoreStereoTransform)
349 std::string filePath = directory+
"/"+
name_+
"_pose.yaml";
350 if(!filePath.empty() && (!
R_.empty() && !
T_.empty()))
352 UINFO(
"Saving stereo calibration to file \"%s\"", filePath.c_str());
353 cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
359 fs <<
"camera_name" <<
name_;
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));
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));
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));
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));
404 UERROR(
"Failed saving stereo extrinsics (they are null).");
414 const int headerSize = 10;
415 int header[headerSize] = {
416 RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR, RTABMAP_VERSION_PATCH,
418 (int)
R_.total(), (int)
T_.total(), (int)
E_.total(), (int)
F_.total(),
419 (int)leftData.size(), (int)rightData.size()};
420 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]);
421 std::vector<unsigned char> data(
422 sizeof(
int)*headerSize +
423 sizeof(
double)*(
R_.total()+
T_.total()+
E_.total()+
F_.total()) +
424 leftData.size() + rightData.size());
425 memcpy(data.data(), header,
sizeof(int)*headerSize);
426 int index =
sizeof(int)*headerSize;
429 memcpy(data.data()+index,
R_.data,
sizeof(double)*(
R_.total()));
430 index+=
sizeof(double)*(
R_.total());
434 memcpy(data.data()+index,
T_.data,
sizeof(double)*(
T_.total()));
435 index+=
sizeof(double)*(
T_.total());
439 memcpy(data.data()+index,
E_.data,
sizeof(double)*(
E_.total()));
440 index+=
sizeof(double)*(
E_.total());
444 memcpy(data.data()+index,
F_.data,
sizeof(double)*(
F_.total()));
445 index+=
sizeof(double)*(
F_.total());
449 memcpy(data.data()+index, leftData.data(), leftData.size());
450 index+=leftData.size();
454 memcpy(data.data()+index, rightData.data(), rightData.size());
455 index+=rightData.size();
468 if(dataSize >=
sizeof(
int)*headerSize)
476 const int *
header = (
const int *)data;
477 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]);
478 int type = header[3];
481 unsigned int requiredDataSize =
sizeof(int)*headerSize +
482 sizeof(
double)*(header[iR]+header[iT]+header[iE]+header[iF]) +
483 header[iLeft] + header[iRight];
485 uFormat(
"dataSize=%d != required=%d (header: version %d.%d.%d type=%d R=%d T=%d E=%d F=%d Left=%d Right=%d)",
488 header[0], header[1], header[2], header[3],
489 header[iR], header[iT], header[iE],header[iF], header[iLeft], header[iRight]).c_str());
491 unsigned int index =
sizeof(int)*headerSize;
496 R_ = cv::Mat(3, 3, CV_64FC1, (
void*)(data+index)).clone();
497 index+=
sizeof(double)*(
R_.total());
503 T_ = cv::Mat(3, 1, CV_64FC1, (
void*)(data+index)).clone();
504 index+=
sizeof(double)*(
T_.total());
510 E_ = cv::Mat(3, 3, CV_64FC1, (
void*)(data+index)).clone();
511 index+=
sizeof(double)*(
E_.total());
517 F_ = cv::Mat(3, 3, CV_64FC1, (
void*)(data+index)).clone();
518 index+=
sizeof(double)*(
F_.total());
521 if(header[iLeft] != 0)
526 if(header[iRight] != 0)
537 UERROR(
"Serialized calibration is not stereo (type=%d), use the appropriate class matching the type to deserialize.", type);
540 UERROR(
"Wrong serialized calibration data format detected (size in bytes=%d)! Cannot deserialize the data.", (
int)dataSize);
560 if(disparity == 0.0
f)
591 if(!
R_.empty() && !
T_.empty())
594 R_.at<
double>(0,0),
R_.at<
double>(0,1),
R_.at<
double>(0,2),
T_.at<
double>(0),
595 R_.at<
double>(1,0),
R_.at<
double>(1,1),
R_.at<
double>(1,2),
T_.at<
double>(1),
596 R_.at<
double>(2,0),
R_.at<
double>(2,1),
R_.at<
double>(2,2),
T_.at<
double>(2));
const cv::Size & imageSize() const
const cv::Mat & R() const
const std::string & name() const
Transform stereoTransform() const
std::vector< unsigned char > serialize() const
void updateStereoRectification()
bool save(const std::string &directory) const
unsigned int deserialize(const std::vector< unsigned char > &data)
const cv::Mat & F() const
bool load(const std::string &filePath)
std_msgs::Header * header(M &m)
Some conversion functions.
const std::string & getRightSuffix() const
float computeDepth(float disparity) const
float computeDisparity(float depth) const
#define UASSERT(condition)
bool isValidForProjection() const
const CameraModel & left() const
std::vector< unsigned char > serialize() const
#define UASSERT_MSG(condition, msg_str)
const cv::Mat & T() const
const cv::Mat & E() const
bool saveStereoTransform(const std::string &directory) const
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)
bool isValidForRectification() const
bool save(const std::string &directory, bool ignoreStereoTransform=true) const
CameraModel roi(const cv::Rect &roi) const
void roi(const cv::Rect &roi)
bool load(const std::string &directory, const std::string &cameraName, bool ignoreStereoTransform=true)
const CameraModel & right() const
ULogger class and convenient macros.
unsigned int deserialize(const std::vector< unsigned char > &data)
void setName(const std::string &name, const std::string &leftSuffix="left", const std::string &rightSuffix="right")
const std::string & getLeftSuffix() const
const Transform & localTransform() const
std::string UTILITE_EXP uFormat(const char *fmt,...)
void setName(const std::string &name)
CameraModel scaled(double scale) const
const Transform & localTransform() const
const std::string & name() const