i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

-Updated CameraModel and StereoCameraModel so that a stereo camera model can be initialization from know intrinsics and extrinsics

Browse Source 
-CreateSimpleCalibrationDialog updated to create mono/stereo calibration files with advanced option (setting intrinsics of each camera + extrinsics between them). Rectification R and new camera P matrices are then automatically computed.
-Added GeodeticCoords class for convience conversion between GPS values (latitude/longitude/altitude) to local coordinate (ENU).
-Added support of Malaga Urban and St Lucia ground truths.
-CameraImages: added option to debayer images. Timestamps file: added support of "sec millisec" format.
-Added UException class: exceptions are sent instead of exiting the application on UASSERT or UFATAL.
This commit is contained in:
matlabbe
2016-01-19 17:43:33 -05:00
parent 72e66605e4
commit 7bfd76e747
29 changed files with 1739 additions and 702 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
corelib/include/rtabmap/core/CameraModel.h
View File

@@ -74,21 +74,25 @@ public:
void initRectificationMap();
bool isValid() const {return !K_.empty() &&
!D_.empty() &&
!R_.empty() &&
!P_.empty() &&
fx()>0.0 &&
fy()>0.0;}
bool isValid() const {return (!K_.empty() || !P_.empty()) && fx()>0.0 && fy()>0.0;}
bool isValidForRectification() const
{
return imageSize_.width>0 &&
imageSize_.height>0 &&
!K_.empty() &&
!D_.empty() &&
!R_.empty() &&
!P_.empty();
}
void setName(const std::string & name) {name_=name;}
const std::string & name() const {return name_;}
double fx() const {return P_.at<double>(0,0);}
double fy() const {return P_.at<double>(1,1);}
double cx() const {return P_.at<double>(0,2);}
double cy() const {return P_.at<double>(1,2);}
double Tx() const {return P_.at<double>(0,3);}
double fx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,0):P_.at<double>(0,0);}
double fy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,1):P_.at<double>(1,1);}
double cx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,2):P_.at<double>(0,2);}
double cy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,2):P_.at<double>(1,2);}
double Tx() const {return P_.empty()?0.0:P_.at<double>(0,3);}
const cv::Mat & K() const {return K_;} //intrinsic camera matrix
const cv::Mat & D() const {return D_;} //intrinsic distorsion matrix

3
corelib/include/rtabmap/core/CameraRGB.h
View File

@@ -63,12 +63,14 @@ public:
unsigned int imagesCount() const;
std::vector<std::string> filenames() const;
bool isImagesRectified() const {return _rectifyImages;}
int getBayerMode() const {return _bayerMode;}
const CameraModel & cameraModel() const {return _model;}
void setPath(const std::string & dir) {_path=dir;}
void setStartIndex(int index) {_startAt = index;} // negative means last
void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
{
@@ -126,6 +128,7 @@ private:
// on each call of takeImage()
bool _refreshDir;
bool _rectifyImages;
int _bayerMode;
bool _isDepth;
float _depthScaleFactor;
int _count;

63
corelib/include/rtabmap/core/GeodeticCoords.h Normal file
View File

@@ -0,0 +1,63 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef GEODETICCOORDS_H_
#define GEODETICCOORDS_H_
#include <rtabmap/core/RtabmapExp.h>
#include <opencv2/core/types.hpp>
namespace RTABMAP_EXP rtabmap {
class GeodeticCoords
{
public:
GeodeticCoords();
GeodeticCoords(double latitude, double longitude, double altitude);
const double & latitude() const {return latitude_;}
const double & longitude() const {return longitude_;}
const double & altitude() const {return altitude_;}
void setLatitude(const double & value) {latitude_ = value;}
void setLongitude(const double & value) {longitude_ = value;}
void setAltitude(const double & value) {altitude_ = value;}
cv::Point3d toGeocentric_WGS84() const;
cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const;
private:
double latitude_; // deg
double longitude_; // deg
double altitude_; // m
};
}
#endif /* GEODETICCOORDS_H_ */

51
corelib/include/rtabmap/core/StereoCameraModel.h
View File

@@ -43,16 +43,9 @@ public:
const cv::Size & imageSize2,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
}
const Transform & localTransform = Transform::getIdentity());
// if R and T are not null, left and right camera models should be valid to be rectified.
StereoCameraModel(
const std::string & name,
const CameraModel & leftCameraModel,
@@ -60,18 +53,14 @@ public:
const cv::Mat & R = cv::Mat(),
const cv::Mat & T = cv::Mat(),
const cv::Mat & E = cv::Mat(),
const cv::Mat & F = cv::Mat()) :
left_(leftCameraModel),
right_(rightCameraModel),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
left_.setName(name+"_left");
right_.setName(name+"_right");
}
const cv::Mat & F = cv::Mat());
// if extrinsics transform is not null, left and right camera models should be valid to be rectified.
StereoCameraModel(
const std::string & name,
const CameraModel & leftCameraModel,
const CameraModel & rightCameraModel,
const Transform & extrinsics);
//minimal
StereoCameraModel(
double fx,
@@ -79,11 +68,7 @@ public:
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(fx, fy, cx, cy, localTransform),
right_(fx, fy, cx, cy, localTransform, baseline*-fx)
{
}
const Transform & localTransform = Transform::getIdentity());
//minimal to be saved
StereoCameraModel(
const std::string & name,
@@ -92,15 +77,13 @@ public:
double cx,
double cy,
double baseline,
const Transform & localTransform = Transform::getIdentity()) :
left_(name+"_left", fx, fy, cx, cy, localTransform),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
name_(name)
{
}
const Transform & localTransform = Transform::getIdentity());
virtual ~StereoCameraModel() {}
bool isValid() const {return left_.isValid() && right_.isValid() && baseline() > 0.0;}
bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}
void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
void setName(const std::string & name);
const std::string & name() const {return name_;}
@@ -108,7 +91,7 @@ public:
bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
double baseline() const {return -right_.Tx()/right_.fx();}
double baseline() const {return right_.fx()!=0.0?-right_.Tx()/right_.fx():0.0;}
float computeDepth(float disparity) const;
float computeDisparity(float depth) const; // m

5
corelib/include/rtabmap/core/Transform.h
View File

@@ -81,6 +81,7 @@ public:
void setNull();
void setIdentity();
const cv::Mat & dataMatrix() const {return data_;}
const float * data() const {return (const float *)data_.data;}
float * data() {return (float *)data_.data;}
int size() const {return 12;}
@@ -99,6 +100,9 @@ public:
Transform translation() const;
Transform to3DoF() const;
cv::Mat rotationMatrix() const;
cv::Mat translationMatrix() const;
void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
void getEulerAngles(float & roll, float & pitch, float & yaw) const;
void getTranslation(float & x, float & y, float & z) const;
@@ -139,6 +143,7 @@ public:
* Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
*/
static Transform fromString(const std::string & string);
static bool canParseString(const std::string & string);
private:
cv::Mat data_;

1
corelib/src/CMakeLists.txt
View File

@@ -26,6 +26,7 @@ SET(SRC_FILES
Signature.cpp
Features2d.cpp
Transform.cpp
GeodeticCoords.cpp
util2d.cpp

338
corelib/src/CameraModel.cpp
View File

@@ -34,8 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
CameraModel::CameraModel() :
P_(cv::Mat::zeros(3, 4, CV_64FC1))
CameraModel::CameraModel()
{
}
@@ -56,12 +55,10 @@ CameraModel::CameraModel(
P_(P),
localTransform_(localTransform)
{
UASSERT(!name_.empty());
UASSERT(imageSize_.width > 0 && imageSize_.height > 0);
UASSERT(K_.rows == 3 && K_.cols == 3);
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
UASSERT(R_.rows == 3 && R_.cols == 3);
UASSERT(P_.rows == 3 && P_.cols == 4);
UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
}
CameraModel::CameraModel(
@@ -72,20 +69,22 @@ CameraModel::CameraModel(
const Transform & localTransform,
double Tx) :
K_(cv::Mat::eye(3, 3, CV_64FC1)),
D_(cv::Mat::zeros(1, 5, CV_64FC1)),
R_(cv::Mat::eye(3, 3, CV_64FC1)),
P_(cv::Mat::eye(3, 4, CV_64FC1)),
localTransform_(localTransform)
{
UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
P_.at<double>(0,0) = fx;
P_.at<double>(1,1) = fy;
P_.at<double>(0,2) = cx;
P_.at<double>(1,2) = cy;
P_.at<double>(0,3) = Tx;
UASSERT(!localTransform.isNull());
if(Tx != 0.0)
{
P_ = cv::Mat::eye(3, 4, CV_64FC1),
P_.at<double>(0,0) = fx;
P_.at<double>(1,1) = fy;
P_.at<double>(0,2) = cx;
P_.at<double>(1,2) = cy;
P_.at<double>(0,3) = Tx;
}
K_.at<double>(0,0) = fx;
K_.at<double>(1,1) = fy;
@@ -103,20 +102,22 @@ CameraModel::CameraModel(
double Tx) :
name_(name),
K_(cv::Mat::eye(3, 3, CV_64FC1)),
D_(cv::Mat::zeros(1, 5, CV_64FC1)),
R_(cv::Mat::eye(3, 3, CV_64FC1)),
P_(cv::Mat::eye(3, 4, CV_64FC1)),
localTransform_(localTransform)
{
UASSERT_MSG(fx >= 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy >= 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
P_.at<double>(0,0) = fx;
P_.at<double>(1,1) = fy;
P_.at<double>(0,2) = cx;
P_.at<double>(1,2) = cy;
P_.at<double>(0,3) = Tx;
UASSERT(!localTransform.isNull());
if(Tx != 0.0)
{
P_ = cv::Mat::eye(3, 4, CV_64FC1),
P_.at<double>(0,0) = fx;
P_.at<double>(1,1) = fy;
P_.at<double>(0,2) = cx;
P_.at<double>(1,2) = cy;
P_.at<double>(0,3) = Tx;
}
K_.at<double>(0,0) = fx;
K_.at<double>(1,1) = fy;
@@ -127,6 +128,9 @@ CameraModel::CameraModel(
void CameraModel::initRectificationMap()
{
UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
UASSERT(R_.rows == 3 && R_.cols == 3);
UASSERT(P_.rows == 3 && P_.cols == 4);
// init rectification map
UINFO("Initialize rectify map");
cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
@@ -137,68 +141,122 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
K_ = cv::Mat();
D_ = cv::Mat();
R_ = cv::Mat();
P_ = cv::Mat::zeros(3, 4, CV_64FC1);
P_ = cv::Mat();
mapX_ = cv::Mat();
mapY_ = cv::Mat();
name_.clear();
imageSize_ = cv::Size();
std::string filePath = directory+"/"+cameraName+".yaml";
if(UFile::exists(filePath))
{
UINFO("Reading calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
name_ = (int)fs["camera_name"];
imageSize_.width = (int)fs["image_width"];
imageSize_.height = (int)fs["image_height"];
UASSERT(!name_.empty());
//UASSERT(imageSize_.width > 0);
//UASSERT(imageSize_.height > 0);
// import from ROS calibration format
cv::FileNode n = fs["camera_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["distortion_coefficients"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["rectification_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["projection_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 4);
P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
fs.release();
if(imageSize_.height > 0 && imageSize_.width > 0)
try
{
initRectificationMap();
}
UINFO("Reading calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
return true;
cv::FileNode n,n2;
n = fs["camera_name"];
if(n.type() != cv::FileNode::NONE)
{
name_ = (int)n;
}
else
{
UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
}
n = fs["image_width"];
n2 = fs["image_height"];
if(n.type() != cv::FileNode::NONE)
{
imageSize_.width = (int)fs["image_width"];
imageSize_.height = (int)fs["image_height"];
}
else
{
UWARN("Missing \"image_width\" and/or \"image_height\" fields in \"%s\"", filePath.c_str());
}
// import from ROS calibration format
n = fs["camera_matrix"];
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"camera_matrix\" field in \"%s\"", filePath.c_str());
}
n = fs["distortion_coefficients"];
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
}
n = fs["rectification_matrix"];
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"rectification_matrix\" field in \"%s\"", filePath.c_str());
}
n = fs["projection_matrix"];
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 4);
P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
}
fs.release();
if(isValidForRectification())
{
initRectificationMap();
}
return true;
}
catch(const cv::Exception & e)
{
UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
}
}
else
{
@@ -210,56 +268,77 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
bool CameraModel::save(const std::string & directory) const
{
std::string filePath = directory+"/"+name_+".yaml";
if(!filePath.empty() && !name_.empty() && !K_.empty() && !D_.empty() && !R_.empty() && !P_.empty())
if(!filePath.empty() && (!K_.empty() || !D_.empty() || !R_.empty() || !P_.empty()))
{
UINFO("Saving calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
fs << "camera_name" << name_;
fs << "image_width" << imageSize_.width;
fs << "image_height" << imageSize_.height;
fs << "camera_matrix" << "{";
fs << "rows" << K_.rows;
fs << "cols" << K_.cols;
fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
fs << "}";
fs << "distortion_coefficients" << "{";
fs << "rows" << D_.rows;
fs << "cols" << D_.cols;
fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
fs << "}";
// compaibility with ROS
if(D_.cols > 5)
if(!name_.empty())
{
fs << "distortion_model" << "rational_polynomial";
fs << "camera_name" << name_;
}
else
if(imageSize_.width>0 && imageSize_.height>0)
{
fs << "distortion_model" << "plumb_bob";
fs << "image_width" << imageSize_.width;
fs << "image_height" << imageSize_.height;
}
fs << "rectification_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
if(!K_.empty())
{
fs << "camera_matrix" << "{";
fs << "rows" << K_.rows;
fs << "cols" << K_.cols;
fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
fs << "}";
}
fs << "projection_matrix" << "{";
fs << "rows" << P_.rows;
fs << "cols" << P_.cols;
fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
fs << "}";
if(!D_.empty())
{
fs << "distortion_coefficients" << "{";
fs << "rows" << D_.rows;
fs << "cols" << D_.cols;
fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
fs << "}";
// compaibility with ROS
if(D_.cols > 5)
{
fs << "distortion_model" << "rational_polynomial";
}
else
{
fs << "distortion_model" << "plumb_bob";
}
}
if(!R_.empty())
{
fs << "rectification_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
}
if(!P_.empty())
{
fs << "projection_matrix" << "{";
fs << "rows" << P_.rows;
fs << "cols" << P_.cols;
fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
fs << "}";
}
fs.release();
return true;
}
else
{
UERROR("Cannot save calibration to \"%s\" because it is empty.", filePath.c_str());
}
return false;
}
@@ -270,17 +349,25 @@ CameraModel CameraModel::scaled(double scale) const
if(this->isValid())
{
// has only effect on K and P
cv::Mat K = K_.clone();
K.at<double>(0,0) *= scale;
K.at<double>(1,1) *= scale;
K.at<double>(0,2) *= scale;
K.at<double>(1,2) *= scale;
cv::Mat K;
if(!K_.empty())
{
K = K_.clone();
K.at<double>(0,0) *= scale;
K.at<double>(1,1) *= scale;
K.at<double>(0,2) *= scale;
K.at<double>(1,2) *= scale;
}
cv::Mat P = P_.clone();
P.at<double>(0,0) *= scale;
P.at<double>(1,1) *= scale;
P.at<double>(0,2) *= scale;
P.at<double>(1,2) *= scale;
cv::Mat P;
if(!P_.empty())
{
P = P_.clone();
P.at<double>(0,0) *= scale;
P.at<double>(1,1) *= scale;
P.at<double>(0,2) *= scale;
P.at<double>(1,2) *= scale;
}
scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
}
else
@@ -371,6 +458,7 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
}
else
{
UERROR("Cannot rectify image because the rectify map is not initialized.");
return raw.clone();
}
}

65
corelib/src/CameraRGB.cpp
View File

@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_surface.h>
#include <iostream>
#include <fstream>
#include <cmath>
namespace rtabmap
@@ -55,6 +56,7 @@ CameraImages::CameraImages() :
_startAt(0),
_refreshDir(false),
_rectifyImages(false),
_bayerMode(-1),
_isDepth(false),
_depthScaleFactor(1.0f),
_count(0),
@@ -81,6 +83,7 @@ CameraImages::CameraImages(const std::string & path,
_startAt(0),
_refreshDir(false),
_rectifyImages(false),
_bayerMode(-1),
_isDepth(false),
_depthScaleFactor(1.0f),
_count(0),
@@ -259,24 +262,39 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
else if(timestampsPath_.size())
{
FILE * file = 0;
#ifdef _MSC_VER
fopen_s(&file, timestampsPath_.c_str(), "r");
#else
file = fopen(timestampsPath_.c_str(), "r");
#endif
if(file)
std::ifstream file;
file.open(timestampsPath_.c_str(), std::ifstream::in);
while(file.good())
{
char line[16];
while ( fgets (line , 16 , file) != NULL )
std::string str;
std::getline(file, str);
if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
{
stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
continue;
}
fclose(file);
std::list<std::string> strList = uSplit(str, ' ');
std::string stampStr = strList.front();
if(strList.size() == 2)
{
// format "seconds millisec"
// the millisec str needs 0-padding if size < 6
std::string millisecStr = strList.back();
while(millisecStr.size() < 6)
{
millisecStr = "0" + millisecStr;
}
stampStr = stampStr+'.'+millisecStr;
}
stamps_.push_back(uStr2Double(stampStr));
}
file.close();
if(stamps_.size() != this->imagesCount())
{
UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
"the timestamps file path if you don't want to use them (current file path=%s).",
(int)stamps_.size(), this->imagesCount(), timestampsPath_.c_str());
stamps_.clear();
@@ -293,19 +311,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
success = false;
}
else if((_groundTruthFormat != 1 && _groundTruthFormat != 5) && poses.size() != this->imagesCount())
else if((_groundTruthFormat != 1 && _groundTruthFormat != 5 && _groundTruthFormat != 6 && _groundTruthFormat != 7) && poses.size() != this->imagesCount())
{
UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
"the ground truth file path if you don't want to use it (current file path=%s).",
(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
success = false;
}
else if((_groundTruthFormat == 1 || _groundTruthFormat == 5) && stamps_.size() == 0)
else if((_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7) && stamps_.size() == 0)
{
UERROR("When using RGBD-SLAM and GPS formats for ground truth, images must have timestamps!");
UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats for ground truth, images must have timestamps!");
success = false;
}
else if(_groundTruthFormat == 1 || _groundTruthFormat == 5)
else if(_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7)
{
UDEBUG("");
//Match ground truth values with images
@@ -568,7 +586,6 @@ SensorData CameraImages::captureImage()
cvReleaseImage(&i);
}
#endif
if(img.channels()>3)
{
UWARN("Conversion from 4 channels to 3 channels (file=%s)", imageFilePath.c_str());
@@ -576,6 +593,20 @@ SensorData CameraImages::captureImage()
cv::cvtColor(img, out, CV_BGRA2BGR);
img = out;
}
else if(_bayerMode >= 0 && _bayerMode <=3)
{
cv::Mat debayeredImg;
try
{
cv::cvtColor(img, debayeredImg, CV_BayerBG2BGR + _bayerMode);
img = debayeredImg;
}
catch(const cv::Exception & e)
{
UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
}
}
}
if(!img.empty() && _model.isValid() && _rectifyImages)

2
corelib/src/CameraRGBD.cpp
View File

@@ -1395,7 +1395,7 @@ SensorData CameraFreenect2::captureImage()
else
{
//rgb + ir or rgb + depth
if(stereoModel_.isValid())
if(stereoModel_.isValidForRectification())
{
cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
cv::Mat rgbMat; // rtabmap uses 3 channels RGB

20
corelib/src/CameraStereo.cpp
View File

@@ -422,8 +422,14 @@ SensorData CameraStereoDC1394::captureImage()
if(!left.empty() && !right.empty())
{
// Rectification
left = stereoModel_.left().rectifyImage(left);
right = stereoModel_.right().rectifyImage(right);
if(stereoModel_.left().isValidForRectification())
{
left = stereoModel_.left().rectifyImage(left);
}
if(stereoModel_.right().isValidForRectification())
{
right = stereoModel_.right().rectifyImage(right);
}
StereoCameraModel model;
if(stereoModel_.isValid())
{
@@ -740,7 +746,6 @@ CameraStereoImages::CameraStereoImages(
camera2_(new CameraImages(pathRightImages))
{
this->setImagesRectified(rectifyImages);
camera2_->setImagesRectified(rectifyImages);
}
CameraStereoImages::CameraStereoImages(
@@ -800,7 +805,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
stereoModel_.setLocalTransform(this->getLocalTransform());
stereoModel_.setName(cameraName);
if(this->isImagesRectified() && !stereoModel_.isValid())
if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
return false;
@@ -815,6 +820,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
{
if(camera2_)
{
camera2_->setBayerMode(this->getBayerMode());
if(camera2_->init())
{
if(this->imagesCount() == camera2_->imagesCount())
@@ -879,11 +885,12 @@ SensorData CameraStereoImages::captureImage()
cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
rightImage = tmp;
}
if(this->isImagesRectified() && stereoModel_.left().isValid() && stereoModel_.right().isValid())
if(this->isImagesRectified() && stereoModel_.isValidForRectification())
{
leftImage = stereoModel_.left().rectifyImage(leftImage);
rightImage = stereoModel_.right().rectifyImage(rightImage);
}
data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
data.setGroundTruth(left.groundTruth());
}
@@ -952,7 +959,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
}
stereoModel_.setLocalTransform(this->getLocalTransform());
if(rectifyImages_ && !stereoModel_.isValid())
if(rectifyImages_ && !stereoModel_.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
return false;
@@ -991,6 +998,7 @@ SensorData CameraStereoVideo::captureImage()
rightImage = tmp;
rightCvt = true;
}
if(rectifyImages_ && stereoModel_.left().isValid() && stereoModel_.right().isValid())
{
leftImage = stereoModel_.left().rectifyImage(leftImage);

137
corelib/src/GeodeticCoords.cpp Normal file
View File

@@ -0,0 +1,137 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*
* The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
* https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
*/
/* +---------------------------------------------------------------------------+
| Mobile Robot Programming Toolkit (MRPT) |
| http://www.mrpt.org/ |
| |
| Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
| See: http://www.mrpt.org/Authors - All rights reserved. |
| Released under BSD License. See details in http://www.mrpt.org/License |
+---------------------------------------------------------------------------+ */
#include "rtabmap/core/GeodeticCoords.h"
#include <math.h>
namespace rtabmap {
inline double DEG2RAD(const double x) { return x*M_PI/180.0;}
inline double square(const double & value) {return value*value;}
//*---------------------------------------------------------------
// geodeticToGeocentric_WGS84
// ---------------------------------------------------------------*/
cv::Point3d GeodeticCoords::toGeocentric_WGS84() const
{
// --------------------------------------------------------------------
// See: http://en.wikipedia.org/wiki/Reference_ellipsoid
// Constants are for WGS84
// --------------------------------------------------------------------
static const double a = 6378137; // Semi-major axis of the Earth (meters)
static const double b = 6356752.3142; // Semi-minor axis:
static const double ae = acos(b/a); // eccentricity:
static const double cos2_ae_earth = square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth: // 0.993305619995739L;
static const double sin2_ae_earth = square(sin(ae)); // The sin^2 of the angular eccentricity of the Earth: // 0.006694380004261L;
const double lon = DEG2RAD( double(this->longitude()) );
const double lat = DEG2RAD( double(this->latitude()) );
// The radius of curvature in the prime vertical:
const double N = a / std::sqrt( 1.0 - sin2_ae_earth*square( sin(lat) ) );
// Generate 3D point:
cv::Point3d out;
out.x = (N+this->altitude())*cos(lat)*cos(lon);
out.y = (N+this->altitude())*cos(lat)*sin(lon);
out.z = (cos2_ae_earth*N+this->altitude())*sin(lat);
return out;
}
/*---------------------------------------------------------------
geodeticToENU_WGS84
---------------------------------------------------------------*/
cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
{
// --------------------------------------------------------------------
// Explanation: We compute the earth-centric coordinates first,
// then make a system transformation to local XYZ coordinates
// using a system of three orthogonal vectors as local reference.
//
// See: http://en.wikipedia.org/wiki/Reference_ellipsoid
// (JLBC 21/DEC/2006) (Fixed: JLBC 9/JUL/2008)
// - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to ellipsoid surface.
// --------------------------------------------------------------------
// Generate 3D point:
cv::Point3d P_geocentric = this->toGeocentric_WGS84();
// Generate reference 3D point:
cv::Point3d P_geocentric_ref = origin.toGeocentric_WGS84();
const double clat = cos(DEG2RAD(origin.latitude())), slat = sin(DEG2RAD(origin.latitude()));
const double clon = cos(DEG2RAD(origin.longitude())), slon = sin(DEG2RAD(origin.longitude()));
// Compute the resulting relative coordinates:
// For using smaller numbers:
P_geocentric -= P_geocentric_ref;
// Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
// after rotation given by the transposed rotation matrix from ENU -> ECEF.
cv::Point3d out;
out.x = -slon*P_geocentric.x + clon*P_geocentric.y;
out.y = -clon*slat*P_geocentric.x -slon*slat*P_geocentric.y + clat*P_geocentric.z;
out.z = clon*clat*P_geocentric.x + slon*clat*P_geocentric.y +slat*P_geocentric.z;
return out;
}
GeodeticCoords::GeodeticCoords() :
latitude_(0.0),
longitude_(0.0),
altitude_(0.0)
{
}
GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) :
latitude_(latitude),
longitude_(longitude),
altitude_(altitude)
{
}
}

111
corelib/src/Graph.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <rtabmap/core/Memory.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/eigen.h>
@@ -153,7 +154,7 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=GPS (t,x,y)
int format, // 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1
@@ -187,20 +188,106 @@ bool importPoses(
return false;
}
int id=1;
GeodeticCoords origin;
Transform originPose;
while(file.good())
{
std::string str;
std::getline(file, str);
if(str.empty() || str.at(0) == '#')
if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
{
continue;
}
if(format == 5) // GPS format
if(format == 7) // St Lucia format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 3 || strList.size() == 4)
if(strList.size() == 12)
{
// Data Type
//0=Timestamp (seconds)
//1=Timestamp (millisec)
//2=Latitude (deg)
//3=Longitude (deg)
//4=Altitude (m)
//5=Height AMSL (m)
//6=vENU X
//7=vENU Y
//8=vENU Z
//9=Roll (rad)
//10=Pitch (rad)
//11=Yaw (rad)
// the millisec str needs 0-padding if size < 6
std::string millisecStr = strList[1];
while(millisecStr.size() < 6)
{
millisecStr = "0" + millisecStr;
}
double stamp = uStr2Double(strList[0] + "." + millisecStr);
// conversion GPS to local coordinate XYZ
double longitude = uStr2Double(strList[2]);
double latitude = uStr2Double(strList[3]);
double altitude = uStr2Double(strList[4]);
if(poses.empty())
{
origin = GeodeticCoords(longitude, latitude, altitude);
}
cv::Point3d coordENU = GeodeticCoords(longitude, latitude, altitude).toENU_WGS84(origin);
double roll = uStr2Double(strList[10]);
double pitch = uStr2Double(strList[9]);
double yaw = -(uStr2Double(strList[11])-M_PI_2);
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
poses.insert(std::make_pair(id, Transform(coordENU.x,coordENU.y,coordENU.z, roll,pitch,yaw)));
}
else
{
UERROR("Error parsing \"%s\" with St Lucia format (should have 12 values, e.g., 101215_153851_Ins0.log)", str.c_str());
}
}
else if(format == 6) // MALAGA URBAN format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 25)
{
// 0=Time
// Lat Lon Alt fix #sats speed dir
// 8=Local_X
// 9=Local_Y
// 10=Local_Z
// rawlog_ID Geocen_X Geocen_Y Geocen_Z GPS_X GPS_Y GPS_Z GPS_VX GPS_VY GPS_VZ Local_VX Local_VY Local_VZ SAT_Time
double stamp = uStr2Double(strList[0]);
double x = uStr2Double(strList[8]);
double y = uStr2Double(strList[9]);
double z = uStr2Double(strList[10]);
if(stamps)
{
stamps->insert(std::make_pair(id, stamp));
}
float yaw = 0.0f;
if(uContains(poses, id-1))
{
// set yaw depending on successive poses
Transform & previousPose = poses.at(id-1);
yaw = atan2(y-previousPose.y(),x-previousPose.x());
previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
}
poses.insert(std::make_pair(id, Transform(x,y,z,0,0,yaw)));
}
else
{
UERROR("Error parsing \"%s\" with Malaga Urban format (should have 25 values, *_GPS.txt)", str.c_str());
}
}
else if(format == 5) // NewCollege format
{
std::vector<std::string> strList = uListToVector(uSplit(str));
if(strList.size() == 3)
{
if( uIsNumber(uReplaceChar(strList[0], ' ', "")) &&
uIsNumber(uReplaceChar(strList[1], ' ', "")) &&
@@ -216,18 +303,20 @@ bool importPoses(
stamps->insert(std::make_pair(id, stamp));
}
float yaw = 0.0f;
if(strList.size()==4)
{
yaw = uStr2Double(uReplaceChar(strList[3], ' ', ""));
}
else if(uContains(poses, id-1))
if(uContains(poses, id-1))
{
// set yaw depending on successive poses
Transform & previousPose = poses.at(id-1);
yaw = atan2(y-previousPose.y(),x-previousPose.x());
previousPose = Transform(previousPose.x(), previousPose.y(), yaw);
}
poses.insert(std::make_pair(id, Transform(x,y,0,0,0,yaw)));
Transform pose = Transform(x,y,0,0,0,yaw);
if(poses.size() == 0)
{
originPose = pose.inverse();
}
pose = originPose * pose; // transform in local coordinate where first value is the origin
poses.insert(std::make_pair(id, pose));
}
else
{
@@ -236,7 +325,7 @@ bool importPoses(
}
else
{
UERROR("Error parsing \"%s\" with GPS format (should have 3 values: stamp x y)", str.c_str());
UERROR("Error parsing \"%s\" with NewCollege format (should have 3 values: stamp x y)", str.c_str());
}
}
else if(format == 1) // rgbd-slam format

7
corelib/src/RegistrationVis.cpp
View File

@@ -698,8 +698,11 @@ Transform RegistrationVis::computeTransformationImpl(
transforms[1] = transforms[1].inverse();
}
UDEBUG("t1=%s", transforms[0].prettyPrint().c_str());
UDEBUG("t2=%s", transforms[1].prettyPrint().c_str());
if(!_forwardEstimateOnly)
{
UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
}
if(!transforms[1].isNull())
{
if(transforms[0].isNull())

284
corelib/src/StereoCameraModel.cpp
View File

@@ -34,6 +34,123 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
StereoCameraModel::StereoCameraModel(
const std::string & name,
const cv::Size & imageSize1,
const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
const cv::Size & imageSize2,
const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
const Transform & localTransform) :
left_(name+"_left", imageSize1, K1, D1, R1, P1, localTransform),
right_(name+"_right", imageSize2, K2, D2, R2, P2, localTransform),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
}
StereoCameraModel::StereoCameraModel(
const std::string & name,
const CameraModel & leftCameraModel,
const CameraModel & rightCameraModel,
const cv::Mat & R,
const cv::Mat & T,
const cv::Mat & E,
const cv::Mat & F) :
left_(leftCameraModel),
right_(rightCameraModel),
name_(name),
R_(R),
T_(T),
E_(E),
F_(F)
{
left_.setName(name+"_left");
right_.setName(name+"_right");
UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
if(!R_.empty() && !T_.empty())
{
UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
cv::Mat R1,R2,P1,P2,Q;
cv::stereoRectify(left_.K(), left_.D(),
right_.K(), right_.D(),
left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
left_ = CameraModel(left_.name(), left_.imageSize(), left_.K(), left_.D(), R1, P1, left_.localTransform());
right_ = CameraModel(right_.name(), right_.imageSize(), right_.K(), right_.D(), R2, P2, right_.localTransform());
}
}
StereoCameraModel::StereoCameraModel(
const std::string & name,
const CameraModel & leftCameraModel,
const CameraModel & rightCameraModel,
const Transform & extrinsics) :
left_(leftCameraModel),
right_(rightCameraModel),
name_(name)
{
left_.setName(name+"_left");
right_.setName(name+"_right");
if(!extrinsics.isNull())
{
UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
cv::Mat R1,R2,P1,P2,Q;
cv::stereoRectify(left_.K(), left_.D(),
right_.K(), right_.D(),
left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
left_ = CameraModel(left_.name(), left_.imageSize(), left_.K(), left_.D(), R1, P1, left_.localTransform());
right_ = CameraModel(right_.name(), right_.imageSize(), right_.K(), right_.D(), R2, P2, right_.localTransform());
}
}
StereoCameraModel::StereoCameraModel(
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform) :
left_(fx, fy, cx, cy, localTransform),
right_(fx, fy, cx, cy, localTransform, baseline*-fx)
{
}
//minimal to be saved
StereoCameraModel::StereoCameraModel(
const std::string & name,
double fx,
double fy,
double cx,
double cy,
double baseline,
const Transform & localTransform) :
left_(name+"_left", fx, fy, cx, cy, localTransform),
right_(name+"_right", fx, fy, cx, cy, localTransform, baseline*-fx),
name_(name)
{
}
void StereoCameraModel::setName(const std::string & name)
{
name_=name;
@@ -53,6 +170,8 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
//load rotation, translation
R_ = cv::Mat();
T_ = cv::Mat();
E_ = cv::Mat();
F_ = cv::Mat();
std::string filePath = directory+"/"+cameraName+"_pose.yaml";
if(UFile::exists(filePath))
@@ -60,44 +179,82 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::READ);
name_ = (int)fs["camera_name"];
cv::FileNode n;
n = fs["camera_name"];
if(n.type() != cv::FileNode::NONE)
{
name_ = (int)n;
}
else
{
UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
}
// import from ROS calibration format
cv::FileNode n = fs["rotation_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
n = fs["rotation_matrix"];
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
}
n = fs["translation_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 1);
T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
if(n.type() != cv::FileNode::NONE)
{
n = fs["translation_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 1);
T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
}
n = fs["essential_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
if(n.type() != cv::FileNode::NONE)
{
n = fs["essential_matrix"];
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"essential_matrix\" field in \"%s\"", filePath.c_str());
}
n = fs["fundamental_matrix"];
rows = (int)n["rows"];
cols = (int)n["cols"];
data.clear();
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
if(n.type() != cv::FileNode::NONE)
{
int rows = (int)n["rows"];
int cols = (int)n["cols"];
std::vector<double> data;
n["data"] >> data;
UASSERT(rows*cols == (int)data.size());
UASSERT(rows == 3 && cols == 3);
F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
}
else
{
UWARN("Missing \"fundamental_matrix\" field in \"%s\"", filePath.c_str());
}
fs.release();
@@ -119,43 +276,62 @@ bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTra
return true;
}
std::string filePath = directory+"/"+name_+"_pose.yaml";
if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
if(!filePath.empty() && (!R_.empty() && !T_.empty()))
{
UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
// export in ROS calibration format
fs << "camera_name" << name_;
if(!name_.empty())
{
fs << "camera_name" << name_;
}
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
if(!R_.empty())
{
fs << "rotation_matrix" << "{";
fs << "rows" << R_.rows;
fs << "cols" << R_.cols;
fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
fs << "}";
}
fs << "translation_matrix" << "{";
fs << "rows" << T_.rows;
fs << "cols" << T_.cols;
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}";
if(!T_.empty())
{
fs << "translation_matrix" << "{";
fs << "rows" << T_.rows;
fs << "cols" << T_.cols;
fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
fs << "}";
}
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
if(!E_.empty())
{
fs << "essential_matrix" << "{";
fs << "rows" << E_.rows;
fs << "cols" << E_.cols;
fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
fs << "}";
}
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
fs << "}";
if(!F_.empty())
{
fs << "fundamental_matrix" << "{";
fs << "rows" << F_.rows;
fs << "cols" << F_.cols;
fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
fs << "}";
}
fs.release();
return true;
}
else
{
UERROR("Failed saving stereo extrinsics (they are null).");
}
}
return false;
}

23
corelib/src/Transform.cpp
View File

@@ -180,6 +180,16 @@ Transform Transform::to3DoF() const
return Transform(x,y,0, 0,0,yaw);
}
cv::Mat Transform::rotationMatrix() const
{
return data_.colRange(0, 3).clone();
}
cv::Mat Transform::translationMatrix() const
{
return data_.col(3).clone();
}
void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
{
pcl::getTranslationAndEulerAngles(toEigen3f(), x, y, z, roll, pitch, yaw);
@@ -404,4 +414,17 @@ Transform Transform::fromString(const std::string & string)
return t;
}
/**
* Format (3 values): x y z
* Format (6 values): x y z roll pitch yaw
* Format (7 values): x y z qx qy qz qw
* Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
* Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
*/
bool Transform::canParseString(const std::string & string)
{
std::list<std::string> list = uSplit(string, ' ');
return list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
}
}