i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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
corelib/include/rtabmap/core/CameraModel.h
+15 -11
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
corelib/include/rtabmap/core/CameraRGB.h
+3
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;
corelib/include/rtabmap/core/GeodeticCoords.h
+63
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_ */
corelib/include/rtabmap/core/StereoCameraModel.h
+17 -34
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
corelib/include/rtabmap/core/Transform.h
+5
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_;
corelib/src/CMakeLists.txt
+1
View File
@@ -26,6 +26,7 @@ SET(SRC_FILES
Signature.cpp
Features2d.cpp
Transform.cpp
GeodeticCoords.cpp
util2d.cpp
corelib/src/CameraModel.cpp
+213 -125
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();
}
}
corelib/src/CameraRGB.cpp
+48 -17
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)
corelib/src/CameraRGBD.cpp
+1 -1
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
corelib/src/CameraStereo.cpp
+14 -6
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);
corelib/src/GeodeticCoords.cpp
+137
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)
{
}
}
corelib/src/Graph.cpp
+100 -11
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
corelib/src/RegistrationVis.cpp
+5 -2
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())
corelib/src/StereoCameraModel.cpp
+230 -54
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;
}
corelib/src/Transform.cpp
+23
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;
}
}
guilib/include/rtabmap/gui/CalibrationDialog.h
+1
View File
@@ -73,6 +73,7 @@ private slots:
void restart();
void calibrate();
bool save();
void unlock();
protected:
virtual void closeEvent(QCloseEvent* event);
guilib/include/rtabmap/gui/PreferencesDialog.h
+3 -1
View File
@@ -61,6 +61,7 @@ class Signature;
class LoopClosureViewer;
class Camera;
class CalibrationDialog;
class CreateSimpleCalibrationDialog;
class RTABMAPGUI_EXP PreferencesDialog : public QDialog
{
@@ -125,7 +126,7 @@ public:
void saveCustomConfig(const QString & section, const QString & key, const QString & value);
QString loadCustomConfig(const QString & section, const QString & key);
const rtabmap::ParametersMap & getAllParameters() const;
rtabmap::ParametersMap getAllParameters() const;
//General panel
int getGeneralLoggerLevel() const;
@@ -330,6 +331,7 @@ private:
//calibration
CalibrationDialog * _calibrationDialog;
CreateSimpleCalibrationDialog * _createCalibrationDialog;
QVector<QCheckBox*> _3dRenderingShowClouds;
QVector<QDoubleSpinBox*> _3dRenderingVoxelSize;
guilib/src/CalibrationDialog.cpp
+17 -11
View File
@@ -75,6 +75,7 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector
connect(ui_->pushButton_restart, SIGNAL(clicked()), this, SLOT(restart()));
connect(ui_->pushButton_save, SIGNAL(clicked()), this, SLOT(save()));
connect(ui_->checkBox_switchImages, SIGNAL(stateChanged(int)), this, SLOT(restart()));
connect(ui_->checkBox_unlock, SIGNAL(stateChanged(int)), SLOT(unlock()));
connect(ui_->spinBox_boardWidth, SIGNAL(valueChanged(int)), this, SLOT(setBoardWidth(int)));
connect(ui_->spinBox_boardHeight, SIGNAL(valueChanged(int)), this, SLOT(setBoardHeight(int)));
@@ -506,27 +507,27 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
if(!stereo_ && readyToCalibrate[0])
{
ui_->pushButton_calibrate->setEnabled(true);
unlock();
}
else if(stereo_ && readyToCalibrate[0] && readyToCalibrate[1] && stereoImagePoints_[0].size())
{
ui_->pushButton_calibrate->setEnabled(true);
unlock();
}
if(ui_->radioButton_rectified->isChecked())
{
if(models_[0].isValid())
if(models_[0].isValidForRectification())
{
images[0] = models_[0].rectifyImage(images[0]);
}
if(models_[1].isValid())
if(models_[1].isValidForRectification())
{
images[1] = models_[1].rectifyImage(images[1]);
}
}
else if(ui_->radioButton_stereoRectified->isChecked() &&
(stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(stereoModel_.left().isValidForRectification() &&
stereoModel_.right().isValidForRectification()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0)))
{
images[0] = stereoModel_.left().rectifyImage(images[0]);
@@ -576,7 +577,7 @@ void CalibrationDialog::restart()
minIrs_[1] = 0x0000;
maxIrs_[1] = 0x7fff;
ui_->pushButton_calibrate->setEnabled(false);
ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
ui_->pushButton_save->setEnabled(false);
ui_->radioButton_raw->setChecked(true);
ui_->radioButton_rectified->setEnabled(false);
@@ -617,6 +618,11 @@ void CalibrationDialog::restart()
ui_->lineEdit_P_2->clear();
}
void CalibrationDialog::unlock()
{
ui_->pushButton_calibrate->setEnabled(true);
}
void CalibrationDialog::calibrate()
{
processingData_ = true;
@@ -845,17 +851,17 @@ void CalibrationDialog::calibrate()
}
if(stereo_ &&
stereoModel_.left().isValid() &&
stereoModel_.right().isValid()&&
(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0))
stereoModel_.isValidForRectification())
{
stereoModel_.initRectificationMap();
ui_->radioButton_rectified->setEnabled(true);
ui_->radioButton_stereoRectified->setEnabled(true);
ui_->radioButton_stereoRectified->setChecked(true);
ui_->pushButton_save->setEnabled(true);
}
else if(models_[0].isValid())
else if(models_[0].isValidForRectification())
{
models_[0].initRectificationMap();
ui_->radioButton_rectified->setEnabled(true);
ui_->radioButton_rectified->setChecked(true);
ui_->pushButton_save->setEnabled(!stereo_);
guilib/src/CameraViewer.cpp
+3 -2
View File
@@ -165,8 +165,9 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
}
}
cloudView_->setVisible(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled());
if(showCloudCheckbox_->isEnabled() || showScanCheckbox_->isEnabled())
cloudView_->setVisible((showCloudCheckbox_->isEnabled() && showCloudCheckbox_->isChecked()) ||
(showScanCheckbox_->isEnabled() && showScanCheckbox_->isChecked()));
if(cloudView_->isVisible())
{
cloudView_->update();
}
guilib/src/CreateSimpleCalibrationDialog.cpp
+203 -46
View File
@@ -46,16 +46,49 @@ CreateSimpleCalibrationDialog::CreateSimpleCalibrationDialog(
savingFolder_(savingFolder),
cameraName_(cameraName)
{
if(cameraName_.isEmpty())
{
cameraName_ = "calib";
}
ui_ = new Ui_createSimpleCalibrationDialog();
ui_->setupUi(this);
connect(ui_->buttonBox->button(QDialogButtonBox::Save), SIGNAL(clicked()), this, SLOT(saveCalibration()));
connect(ui_->buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), ui_->stackedWidget, SLOT(setCurrentIndex(int)));
connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateStereoView()));
connect(ui_->checkBox_stereo, SIGNAL(stateChanged(int)), this, SLOT(updateSaveStatus()));
connect(ui_->comboBox_advanced, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_fx, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_fy, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_fx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_fy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_cx_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_cy_l, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->lineEdit_D_l, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_fx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_fy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_cx_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_cy_r, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->lineEdit_D_r, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
connect(ui_->spinBox_width, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
connect(ui_->spinBox_height, SIGNAL(valueChanged(int)), this, SLOT(updateSaveStatus()));
connect(ui_->doubleSpinBox_baseline, SIGNAL(valueChanged(double)), this, SLOT(updateSaveStatus()));
connect(ui_->lineEdit_RT, SIGNAL(textEdited(const QString &)), this, SLOT(updateSaveStatus()));
ui_->stackedWidget->setCurrentIndex(ui_->comboBox_advanced->currentIndex());
ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(false);
updateStereoView();
}
CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
@@ -63,29 +96,189 @@ CreateSimpleCalibrationDialog::~CreateSimpleCalibrationDialog()
delete ui_;
}
void CreateSimpleCalibrationDialog::updateStereoView()
{
bool checked = ui_->checkBox_stereo->isChecked();
ui_->doubleSpinBox_baseline->setVisible(checked);
ui_->label_baseline->setVisible(checked);
ui_->label_right->setVisible(checked);
ui_->doubleSpinBox_fx_r->setVisible(checked);
ui_->doubleSpinBox_fy_r->setVisible(checked);
ui_->doubleSpinBox_cx_r->setVisible(checked);
ui_->doubleSpinBox_cy_r->setVisible(checked);
ui_->lineEdit_D_r->setVisible(checked);
ui_->groupBox_stereo_extrinsics->setVisible(checked);
ui_->label_left->setVisible(checked);
}
void CreateSimpleCalibrationDialog::updateSaveStatus()
{
ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(ui_->doubleSpinBox_fx->value() > 0.0 && ui_->doubleSpinBox_fy->value() > 0.0);
bool valid = false;
if(ui_->comboBox_advanced->currentIndex() == 0 &&
ui_->doubleSpinBox_fx->value() > 0.0 &&
ui_->doubleSpinBox_fy->value() > 0.0 &&
(!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_baseline->value() != 0.0))
{
// basic
valid = true;
}
else if(ui_->comboBox_advanced->currentIndex() == 1 &&
ui_->doubleSpinBox_fx_l->value() > 0.0 &&
ui_->doubleSpinBox_fy_l->value() > 0.0 &&
ui_->doubleSpinBox_cx_l->value() > 0.0 &&
ui_->doubleSpinBox_cy_l->value() > 0.0 &&
(!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fx_r->value() > 0.0) &&
(!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_fy_r->value() > 0.0) &&
(!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cx_r->value() > 0.0) &&
(!ui_->checkBox_stereo->isChecked() || ui_->doubleSpinBox_cy_r->value() > 0.0) &&
ui_->spinBox_width->value() > 0 &&
ui_->spinBox_height->value() > 0 &&
!ui_->lineEdit_D_l->text().isEmpty() &&
(!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_D_r->text().isEmpty()) &&
(!ui_->checkBox_stereo->isChecked() || !ui_->lineEdit_RT->text().isEmpty()))
{
//advanced
QStringList distorsionsStrListL = ui_->lineEdit_D_l->text().trimmed().split(' ');
QStringList distorsionsStrListR = ui_->lineEdit_D_r->text().trimmed().split(' ');
if((distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
(!ui_->checkBox_stereo->isChecked() || distorsionsStrListL.size() == 4 || distorsionsStrListL.size() == 5 || distorsionsStrListL.size() == 8) &&
(!ui_->checkBox_stereo->isChecked() || Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString())))
{
valid = true;
}
}
ui_->buttonBox->button(QDialogButtonBox::Save)->setEnabled(valid);
}
void CreateSimpleCalibrationDialog::saveCalibration()
{
if(ui_->doubleSpinBox_baseline->value()==0)
QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
QString name = QFileInfo(filePath).baseName();
QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
if(!filePath.isEmpty())
{
QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_+"/"+cameraName_+".yaml", "*.yaml");
QString name = QFileInfo(filePath).baseName();
QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
if(!filePath.isEmpty())
cameraName_ = name;
CameraModel modelLeft;
float width = ui_->spinBox_width->value();
float height = ui_->spinBox_height->value();
if(ui_->comboBox_advanced->currentIndex() == 0)
{
cameraName_ = name;
CameraModel model(
//basic
modelLeft = CameraModel(
name.toStdString(),
ui_->doubleSpinBox_fx->value(),
ui_->doubleSpinBox_fy->value(),
ui_->doubleSpinBox_cx->value(),
ui_->doubleSpinBox_cy->value());
UASSERT(model.isValid());
if(model.save(dir.toStdString()))
UASSERT(modelLeft.isValid());
}
else
{
//advanced
cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
K.at<double>(0,0) = ui_->doubleSpinBox_fx_l->value();
K.at<double>(1,1) = ui_->doubleSpinBox_fy_l->value();
K.at<double>(0,2) = ui_->doubleSpinBox_cx_l->value();
K.at<double>(1,2) = ui_->doubleSpinBox_cy_l->value();
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
QStringList distorsionCoeffs = ui_->lineEdit_D_l->text().trimmed().split(' ');
UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
bool ok;
for(int i=0; i<distorsionCoeffs.size(); ++i)
{
D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
if(!ok)
{
QMessageBox::warning(this, tr("Save"), tr("Error parsing left distorsion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
return;
}
}
modelLeft = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
UASSERT(modelLeft.isValidForRectification());
}
if(ui_->checkBox_stereo->isChecked())
{
StereoCameraModel stereoModel;
if(ui_->comboBox_advanced->currentIndex() == 0)
{
CameraModel modelRight(
name.toStdString(),
ui_->doubleSpinBox_fx->value(),
ui_->doubleSpinBox_fy->value(),
ui_->doubleSpinBox_cx->value(),
ui_->doubleSpinBox_cy->value(),
Transform::getIdentity(),
ui_->doubleSpinBox_baseline->value()*-ui_->doubleSpinBox_fx->value());
UASSERT(modelRight.isValid());
stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform());
}
else if(ui_->comboBox_advanced->currentIndex() == 1)
{
CameraModel modelRight;
cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
K.at<double>(0,0) = ui_->doubleSpinBox_fx_r->value();
K.at<double>(1,1) = ui_->doubleSpinBox_fy_r->value();
K.at<double>(0,2) = ui_->doubleSpinBox_cx_r->value();
K.at<double>(1,2) = ui_->doubleSpinBox_cy_r->value();
cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
cv::Mat P = cv::Mat::zeros(3, 4, CV_64FC1);
K.copyTo(cv::Mat(P, cv::Range(0,3), cv::Range(0,3)));
QStringList distorsionCoeffs = ui_->lineEdit_D_r->text().trimmed().split(' ');
UASSERT(distorsionCoeffs.size() == 4 || distorsionCoeffs.size() == 5 || distorsionCoeffs.size() == 8);
cv::Mat D = cv::Mat::zeros(1, distorsionCoeffs.size(), CV_64FC1);
bool ok;
for(int i=0; i<distorsionCoeffs.size(); ++i)
{
D.at<double>(i) = distorsionCoeffs.at(i).toDouble(&ok);
if(!ok)
{
QMessageBox::warning(this, tr("Save"), tr("Error parsing right distorsion coefficients \"%1\".").arg(ui_->lineEdit_D_l->text()));
return;
}
}
modelRight = CameraModel(name.toStdString(), cv::Size(width,height), K, D, R, P);
UASSERT(modelRight.isValidForRectification());
UASSERT(Transform::canParseString(ui_->lineEdit_RT->text().trimmed().toStdString()));
stereoModel = StereoCameraModel(name.toStdString(), modelLeft, modelRight, Transform::fromString(ui_->lineEdit_RT->text().toStdString()));
}
std::string base = (dir+QDir::separator()+name).toStdString();
std::string leftPath = base+"_left.yaml";
std::string rightPath = base+"_right.yaml";
if(stereoModel.save(dir.toStdString(), ui_->comboBox_advanced->currentIndex() != 1))
{
if(ui_->comboBox_advanced->currentIndex() == 0)
{
QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\".").
arg(leftPath.c_str()).arg(rightPath.c_str()));
}
else
{
std::string posePath = base+"_pose.yaml";
QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
}
this->accept();
}
else
{
QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
}
}
else
{
if(modelLeft.save(dir.toStdString()))
{
QMessageBox::information(this, tr("Save"), tr("Calibration file saved to \"%1\".").arg(filePath));
this->accept();
@@ -96,42 +289,6 @@ void CreateSimpleCalibrationDialog::saveCalibration()
}
}
}
else
{
QString filePath = QFileDialog::getSaveFileName(this, tr("Save"), savingFolder_ + "/" + cameraName_, "*.yaml");
QString name = QFileInfo(filePath).baseName();
QString dir = QFileInfo(filePath).absoluteDir().absolutePath();
if(!name.isEmpty())
{
cameraName_ = name;
std::string base = (dir+QDir::separator()+name).toStdString();
std::string leftPath = base+"_left.yaml";
std::string rightPath = base+"_right.yaml";
std::string posePath = base+"_pose.yaml";
StereoCameraModel model(
name.toStdString(),
ui_->doubleSpinBox_fx->value(),
ui_->doubleSpinBox_fy->value(),
ui_->doubleSpinBox_cx->value(),
ui_->doubleSpinBox_cy->value(),
ui_->doubleSpinBox_baseline->value());
UASSERT(model.left().isValid() &&
model.right().isValid()&&
model.baseline() > 0.0);
if(model.save(dir.toStdString(), true))
{
QMessageBox::information(this, tr("Save"), tr("Calibration files saved:\n \"%1\"\n \"%2\"\n \"%3\".").
arg(leftPath.c_str()).arg(rightPath.c_str()).arg(posePath.c_str()));
this->accept();
}
else
{
QMessageBox::warning(this, tr("Save"), tr("Error saving \"%1\" and \"%2\"").arg(leftPath.c_str()).arg(rightPath.c_str()));
}
}
}
}
}
guilib/src/CreateSimpleCalibrationDialog.h
+4
View File
@@ -47,7 +47,11 @@ public:
virtual ~CreateSimpleCalibrationDialog();
void setCameraInfoDir(const QString & folder) {savingFolder_ = folder;}
const QString & cameraName() const {return cameraName_;}
private slots:
void updateStereoView();
void updateSaveStatus();
void saveCalibration();
guilib/src/PreferencesDialog.cpp
+33 -106
View File
@@ -92,7 +92,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui(0),
_indexModel(0),
_initialized(false),
_calibrationDialog(new CalibrationDialog(false, ".", this))
_calibrationDialog(new CalibrationDialog(false, ".", this)),
_createCalibrationDialog(new CreateSimpleCalibrationDialog(".", "", this))
{
ULOGGER_DEBUG("");
_calibrationDialog->setWindowFlags(Qt::Window);
@@ -354,6 +355,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->source_images_spinBox_startPos, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->source_images_refreshDir, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_rgbImages_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->comboBox_cameraImages_bayerMode, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
//video group
connect(_ui->source_video_toolButton_selectSource, SIGNAL(clicked()), this, SLOT(selectSourceVideoPath()));
connect(_ui->source_video_lineEdit_path, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1142,6 +1144,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_images_spinBox_startPos->setValue(0);
_ui->source_images_refreshDir->setChecked(false);
_ui->checkBox_rgbImages_rectify->setChecked(false);
_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(0);
_ui->checkBox_rgbVideo_rectify->setChecked(false);
_ui->source_checkBox_ignoreOdometry->setChecked(false);
@@ -1523,6 +1526,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
_ui->source_images_refreshDir->setChecked(settings.value("refreshDir",_ui->source_images_refreshDir->isChecked()).toBool());
_ui->checkBox_rgbImages_rectify->setChecked(settings.value("rectify",_ui->checkBox_rgbImages_rectify->isChecked()).toBool());
_ui->comboBox_cameraImages_bayerMode->setCurrentIndex(settings.value("bayerMode",_ui->comboBox_cameraImages_bayerMode->currentIndex()).toInt());
_ui->checkBox_cameraImages_timestamps->setChecked(settings.value("filenames_as_stamps",_ui->checkBox_cameraImages_timestamps->isChecked()).toBool());
_ui->checkBox_cameraImages_syncTimeStamps->setChecked(settings.value("sync_stamps",_ui->checkBox_cameraImages_syncTimeStamps->isChecked()).toBool());
@@ -1895,6 +1899,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("startPos", _ui->source_images_spinBox_startPos->value());
settings.setValue("refreshDir", _ui->source_images_refreshDir->isChecked());
settings.setValue("rectify", _ui->checkBox_rgbImages_rectify->isChecked());
settings.setValue("bayerMode", _ui->comboBox_cameraImages_bayerMode->currentIndex());
settings.setValue("filenames_as_stamps", _ui->checkBox_cameraImages_timestamps->isChecked());
settings.setValue("sync_stamps", _ui->checkBox_cameraImages_syncTimeStamps->isChecked());
settings.setValue("stamps", _ui->lineEdit_cameraImages_timestamps->text());
@@ -2404,11 +2409,15 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri
return value;
}
const rtabmap::ParametersMap & PreferencesDialog::getAllParameters() const
rtabmap::ParametersMap PreferencesDialog::getAllParameters() const
{
UASSERT_MSG(_parameters.size() == Parameters::getDefaultParameters().size(),
uFormat("%d vs %d (Is PreferencesDialog::init() called?)", (int)_parameters.size(), (int)Parameters::getDefaultParameters().size()).c_str());
return _parameters;
ParametersMap parameters = _parameters;
uInsert(parameters, _modifiedParameters);
return parameters;
}
void PreferencesDialog::selectSourceDriver(Src src)
@@ -2497,6 +2506,10 @@ void PreferencesDialog::selectCalibrationPath()
{
dir = getWorkingDirectory()+"/camera_info";
}
else if(!dir.contains('.'))
{
dir = getWorkingDirectory()+"/camera_info/"+dir;
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml)"));
if(path.size())
{
@@ -2568,7 +2581,7 @@ void PreferencesDialog::selectSourceImagesPathGt()
{
dir = getWorkingDirectory();
}
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt)"));
QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Ground Truth (*.txt *.log *.toro *.g2o)"));
if(path.size())
{
QStringList list;
@@ -2691,7 +2704,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
QWidget * obj = _ui->stackedWidget->findChild<QWidget*>(key.c_str());
if(obj)
{
bool oldState = obj->blockSignals(true);
QSpinBox * spin = qobject_cast<QSpinBox *>(obj);
QDoubleSpinBox * doubleSpin = qobject_cast<QDoubleSpinBox *>(obj);
QComboBox * combo = qobject_cast<QComboBox *>(obj);
@@ -2799,7 +2811,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
{
ULOGGER_WARN("QObject called %s can't be cast to a supported widget", key.c_str());
}
obj->blockSignals(oldState);
}
else
{
@@ -2866,79 +2877,6 @@ void PreferencesDialog::addParameter(const QObject * object, int value)
// Add parameter
UDEBUG("modify param %s=%s", object->objectName().toStdString().c_str(), uNumber2Str(value).c_str());
uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString()));
if(comboBox)
{
// Add related panels to parameters
if(comboBox == _ui->comboBox_vh_strategy)
{
if(value == 0) // 0 none
{
// No panel related...
}
else if(value == 1) // 2 epipolar
{
this->addParameters(_ui->groupBox_vh_epipolar2);
}
}
else if(comboBox == _ui->comboBox_detector_strategy ||
comboBox == _ui->reextract_type)
{
if(value == 0) // surf
{
this->addParameters(_ui->groupBox_detector_surf2);
}
else if(value == 1) // sift
{
this->addParameters(_ui->groupBox_detector_sift2);
}
else if(value == 2) // orb
{
this->addParameters(_ui->groupBox_detector_orb2);
}
else if(value == 3) // fast+freak
{
this->addParameters(_ui->groupBox_detector_fast2);
this->addParameters(_ui->groupBox_detector_freak2);
}
else if(value == 4) // fast+brief
{
this->addParameters(_ui->groupBox_detector_fast2);
this->addParameters(_ui->groupBox_detector_brief2);
}
else if(value == 5) // gftt+freak
{
this->addParameters(_ui->groupBox_detector_gftt2);
this->addParameters(_ui->groupBox_detector_freak2);
}
else if(value == 6) // gftt+brief
{
this->addParameters(_ui->groupBox_detector_gftt2);
this->addParameters(_ui->groupBox_detector_brief2);
}
else if(value == 7) // brisk
{
this->addParameters(_ui->groupBox_detector_brisk2);
}
}
else if(comboBox == _ui->loopClosure_estimationType)
{
this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex());
}
else if(comboBox == _ui->graphOptimization_type)
{
this->addParameter(_ui->graphOptimization_iterations, _ui->graphOptimization_iterations->value());
this->addParameter(_ui->graphOptimization_covarianceIgnored, _ui->graphOptimization_covarianceIgnored->isChecked());
this->addParameter(_ui->graphOptimization_slam2d, _ui->graphOptimization_slam2d->isChecked());
this->addParameter(_ui->graphOptimization_stopEpsilon, _ui->graphOptimization_stopEpsilon->value());
this->addParameter(_ui->graphOptimization_robust, _ui->graphOptimization_robust->isChecked());
}
else if(comboBox == _ui->comboBox_registrationStrategy)
{
this->addParameters(_ui->groupBox_visualTransform2);
this->addParameters(_ui->groupBox_icp2);
}
}
}
else
{
@@ -2964,23 +2902,6 @@ void PreferencesDialog::addParameter(const QObject * object, bool value)
// Add parameter
UDEBUG("modify param %s=%s", object->objectName().toStdString().c_str(), uBool2Str(value).c_str());
uInsert(_modifiedParameters, rtabmap::ParametersPair(object->objectName().toStdString(), uBool2Str(value)));
// RGBD panel
if(value && checkbox == _ui->general_checkBox_activateRGBD)
{
// add all RGBD parameters!
this->addParameters(_ui->groupBox_slam_update);
this->addParameters(_ui->groupBox_graphOptimization2);
this->addParameters(_ui->groupBox_proximityDetection2);
this->addParameters(_ui->groupBox_visualTransform2);
this->addParameters(_ui->groupBox_icp2);
this->addParameters(_ui->groupBox_pathPlanning2);
}
if(groupBox)
{
this->addParameters(groupBox);
}
}
else
{
@@ -3825,6 +3746,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
_ui->doubleSpinBox_cameraRGBDImages_scale->value(),
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraRGBDImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraRGBDImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
((CameraRGBDImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -3865,9 +3787,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
camera = new CameraStereoImages(
_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
_ui->checkBox_stereoImages_rectify->isChecked(),
_ui->checkBox_stereoImages_rectify->isChecked() && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraStereoImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraStereoImages*)camera)->setGroundTruthPath(_ui->lineEdit_cameraImages_gt->text().toStdString(), _ui->comboBox_cameraImages_gtFormat->currentIndex());
((CameraStereoImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
@@ -3885,7 +3808,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
{
camera = new CameraStereoVideo(
_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
_ui->checkBox_stereoVideo_rectify->isChecked(),
_ui->checkBox_stereoVideo_rectify->isChecked() && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -3900,7 +3823,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
{
camera = new CameraVideo(
_ui->source_video_lineEdit_path->text().toStdString(),
_ui->checkBox_rgbVideo_rectify->isChecked(),
_ui->checkBox_rgbVideo_rectify->isChecked() && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -3913,23 +3836,24 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
((CameraImages*)camera)->setDirRefreshed(_ui->source_images_refreshDir->isChecked());
((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked());
((CameraImages*)camera)->setImagesRectified(_ui->checkBox_rgbImages_rectify->isChecked() && !useRawImages);
((CameraRGBDImages*)camera)->setGroundTruthPath(
((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraImages*)camera)->setGroundTruthPath(
_ui->lineEdit_cameraImages_gt->text().toStdString(),
_ui->comboBox_cameraImages_gtFormat->currentIndex());
((CameraRGBDImages*)camera)->setScanPath(
((CameraImages*)camera)->setScanPath(
_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
_ui->spinBox_cameraImages_max_scan_pts->value(),
_ui->spinBox_cameraImages_scanDownsampleStep->value(),
_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
_ui->spinBox_cameraImages_scanNormalsK->value(),
this->getLaserLocalTransform());
((CameraRGBDImages*)camera)->setDepthFromScan(
((CameraImages*)camera)->setDepthFromScan(
_ui->groupBox_depthFromScan->isChecked(),
_ui->checkBox_depthFromScan_vertical->isChecked(),
_ui->checkBox_depthFromScan_fillBorders->isChecked());
((CameraRGBDImages*)camera)->setTimestamps(
((CameraImages*)camera)->setTimestamps(
_ui->checkBox_cameraImages_timestamps->isChecked(),
_ui->lineEdit_cameraImages_timestamps->text().toStdString(),
_ui->checkBox_cameraImages_syncTimeStamps->isChecked());
@@ -4303,8 +4227,11 @@ void PreferencesDialog::calibrate()
void PreferencesDialog::calibrateSimple()
{
CreateSimpleCalibrationDialog dialog(this->getCameraInfoDir(), "", this);
dialog.exec();
_createCalibrationDialog->setCameraInfoDir(this->getCameraInfoDir());
if(_createCalibrationDialog->exec() == QMessageBox::Accepted)
{
_ui->lineEdit_calibrationFile->setText(_createCalibrationDialog->cameraName());
}
}
}
guilib/src/ui/calibrationDialog.ui
+20 -6
View File
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1314</width>
<height>795</height>
<height>860</height>
</rect>
</property>
<property name="windowTitle">
@@ -404,11 +404,22 @@
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_calibrate">
<property name="text">
<string>Calibrate</string>
</property>
</widget>
<layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0">
<item>
<widget class="QPushButton" name="pushButton_calibrate">
<property name="text">
<string>Calibrate</string>
</property>
</widget>
</item>
<item>
<widget class="QCheckBox" name="checkBox_unlock">
<property name="text">
<string>Unlock</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QGroupBox" name="groupBox_3">
@@ -707,6 +718,9 @@
</widget>
</item>
</layout>
<zorder>buttonBox</zorder>
<zorder>groupBox</zorder>
<zorder></zorder>
</widget>
<customwidgets>
<customwidget>
guilib/src/ui/createSimpleCalibrationDialog.ui
+380 -120
View File
@@ -6,106 +6,399 @@
<rect>
<x>0</x>
<y>0</y>
<width>306</width>
<height>212</height>
<width>546</width>
<height>568</height>
</rect>
</property>
<property name="windowTitle">
<string>Create simple calibration</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout">
<layout class="QVBoxLayout" name="verticalLayout_3">
<item>
<layout class="QFormLayout" name="formLayout">
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fx">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QComboBox" name="comboBox_advanced">
<item>
<property name="text">
<string>Basic (images are already rectified)</string>
</property>
</item>
<item>
<property name="text">
<string>Advanced (images should be rectified)</string>
</property>
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label">
<item>
<widget class="QCheckBox" name="checkBox_stereo">
<property name="text">
<string>fx</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fy">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string>fy</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cx">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_3">
<property name="text">
<string>cx</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cy">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_4">
<property name="text">
<string>cy</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_baseline">
<property name="decimals">
<number>6</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>baseline (only for stereo)</string>
<string>Stereo</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="QStackedWidget" name="stackedWidget">
<property name="currentIndex">
<number>1</number>
</property>
<widget class="QWidget" name="page_2">
<layout class="QVBoxLayout" name="verticalLayout">
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Camera Intrinsics</string>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="1" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string>fy</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fx">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_baseline">
<property name="text">
<string>baseline (m)</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_3">
<property name="text">
<string>cx</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fy">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_4">
<property name="text">
<string>cy</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label">
<property name="text">
<string>fx</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_baseline">
<property name="decimals">
<number>6</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cy">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cx">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QWidget" name="page">
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Camera Intrinsics</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<item>
<layout class="QGridLayout" name="gridLayout_3" columnstretch="0,0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_left">
<property name="text">
<string>Left</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_right">
<property name="text">
<string>Right</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fx_l">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fx_r">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_10">
<property name="text">
<string>fx</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fy_l">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_fy_r">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_9">
<property name="text">
<string>fy</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cx_l">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cx_r">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_11">
<property name="text">
<string>cx</string>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cy_l">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_cy_r">
<property name="decimals">
<number>6</number>
</property>
<property name="maximum">
<double>99999.000000000000000</double>
</property>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="label_8">
<property name="text">
<string>cy</string>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLineEdit" name="lineEdit_D_l"/>
</item>
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_D_r"/>
</item>
<item row="5" column="2">
<widget class="QLabel" name="label_12">
<property name="toolTip">
<string>k1, k2, p1, p2[, k3[, k4, k5, k6 ]]</string>
</property>
<property name="text">
<string>Distorsion coefficients (4, 5 or 8 values)</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_4" columnstretch="0,1">
<item row="0" column="0">
<widget class="QSpinBox" name="spinBox_width">
<property name="maximum">
<number>99999</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Image width</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_height">
<property name="maximum">
<number>99999</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_13">
<property name="text">
<string>Image height</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_stereo_extrinsics">
<property name="title">
<string>Stereo Camera Extrinsics</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
<item row="0" column="0" rowspan="2" colspan="2">
<widget class="QLineEdit" name="lineEdit_RT">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
</widget>
</item>
<item row="0" column="2">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Transform between the cameras</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer_2">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
</item>
<item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="orientation">
@@ -119,38 +412,5 @@
</layout>
</widget>
<resources/>
<connections>
<connection>
<sender>buttonBox</sender>
<signal>accepted()</signal>
<receiver>createSimpleCalibrationDialog</receiver>
<slot>accept()</slot>
<hints>
<hint type="sourcelabel">
<x>248</x>
<y>254</y>
</hint>
<hint type="destinationlabel">
<x>157</x>
<y>274</y>
</hint>
</hints>
</connection>
<connection>
<sender>buttonBox</sender>
<signal>rejected()</signal>
<receiver>createSimpleCalibrationDialog</receiver>
<slot>reject()</slot>
<hints>
<hint type="sourcelabel">
<x>316</x>
<y>260</y>
</hint>
<hint type="destinationlabel">
<x>286</x>
<y>274</y>
</hint>
</hints>
</connection>
</connections>
<connections/>
</ui>
guilib/src/ui/preferencesDialog.ui
+147 -86
View File
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>979</width>
<height>655</height>
<width>984</width>
<height>662</height>
</rect>
</property>
<property name="sizePolicy">
@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-499</y>
<width>680</width>
<height>1171</height>
<y>-855</y>
<width>681</width>
<height>1919</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -1703,7 +1703,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="9" column="1">
<widget class="QLabel" name="label_244">
<property name="text">
<string>Create a simple calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of a source of rectified or registered images.</string>
<string>Create a calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of the source of images.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -1868,7 +1868,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</sizepolicy>
</property>
<property name="text">
<string>Simple calibration</string>
<string>Create calibration</string>
</property>
</widget>
</item>
@@ -3331,21 +3331,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
<item>
<layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
<item row="5" column="1">
<widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="2">
<item row="1" column="2">
<widget class="QLabel" name="label_255">
<property name="text">
<string>Use file names as timestamps. Format is epoch time. Example: &quot;1305031102.175304.png&quot;</string>
@@ -3358,48 +3344,28 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QToolButton" name="toolButton_cameraImages_timestamps">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLineEdit" name="lineEdit_cameraImages_timestamps">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_251">
<property name="text">
<string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QToolButton" name="toolButton_cameraImages_gt">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLineEdit" name="lineEdit_cameraImages_gt">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="2">
<item row="4" column="2">
<widget class="QLabel" name="label_288">
<property name="text">
<string>Ground truth file. Select the correct format below.</string>
@@ -3412,8 +3378,94 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="2">
<widget class="QLabel" name="label_289">
<property name="text">
<string>Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="2">
<widget class="QLabel" name="label_292">
<property name="text">
<string>Maximum laser scan points.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="2">
<widget class="QLabel" name="label_256">
<property name="text">
<string>Synchronize capture rate with timestamps.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLineEdit" name="lineEdit_cameraImages_path_scans">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="2">
<widget class="QLabel" name="label_251">
<property name="text">
<string>Timestamps file (*.txt). The file should contain one column. The number of rows should be the same than the number of images in the folder. Not used if &quot;Use file names as timestamps&quot; above is checked. </string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QToolButton" name="toolButton_cameraImages_gt">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QComboBox" name="comboBox_cameraImages_gtFormat">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Raw Format (3 values): x y z&lt;br/&gt;Raw Format (6 values): x y z roll pitch yaw&lt;br/&gt;Raw Format (7 values): x y z qx qy qz qw&lt;br/&gt;Raw Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Raw Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;RGBD-SLAM (stamp tx ty tz qx qy qz qw)&lt;br/&gt;KITTI (stamp + 12 values transform)&lt;br/&gt;TORO&lt;br/&gt;g2o&lt;br/&gt;NewCollege (stamp x y)&lt;br/&gt;Malaga Urban (GPS)&lt;br/&gt;St Lucia Stereo (INS)&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Raw</string>
@@ -3441,32 +3493,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
<item>
<property name="text">
<string>GPS</string>
<string>NewCollege</string>
</property>
</item>
<item>
<property name="text">
<string>Malaga Urban</string>
</property>
</item>
<item>
<property name="text">
<string>St Lucia Stereo</string>
</property>
</item>
</widget>
</item>
<item row="4" column="2">
<widget class="QLabel" name="label_289">
<property name="text">
<string>Ground truth format.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QToolButton" name="toolButton_cameraImages_path_scans">
<property name="text">
<string>...</string>
</property>
</widget>
</item>
<item row="5" column="2">
<item row="6" column="2">
<widget class="QLabel" name="label_293">
<property name="text">
<string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
@@ -3479,7 +3528,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -3489,7 +3538,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="6" column="2">
<item row="7" column="2">
<widget class="QLabel" name="label_294">
<property name="text">
<string>Local transform from /base_link to /scan_link. Mouse over the box to show formats.</string>
@@ -3502,7 +3551,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
<property name="toolTip">
<string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
@@ -3512,10 +3561,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="7" column="2">
<widget class="QLabel" name="label_292">
<item row="0" column="2">
<widget class="QLabel" name="label_265">
<property name="text">
<string>Maximum laser scan points.</string>
<string>Bayer mode. For convenience, if the images are bayered.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -3525,24 +3574,36 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_256">
<property name="text">
<string>Synchronize capture rate with timestamps.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkBox_cameraImages_syncTimeStamps">
<property name="text">
<string/>
<item row="0" column="1">
<widget class="QComboBox" name="comboBox_cameraImages_bayerMode">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContents</enum>
</property>
<item>
<property name="text">
<string>Disabled</string>
</property>
</item>
<item>
<property name="text">
<string>BG</string>
</property>
</item>
<item>
<property name="text">
<string>GB</string>
</property>
</item>
<item>
<property name="text">
<string>RG</string>
</property>
</item>
<item>
<property name="text">
<string>GR</string>
</property>
</item>
</widget>
</item>
</layout>
tools/ConsoleApp/main.cpp
-14
View File
@@ -63,8 +63,6 @@ void showUsage()
" -debug Set Log level to Debug (Default Error)\n"
" -info Set Log level to Info (Default Error)\n"
" -warn Set Log level to Warning (Default Error)\n"
" -exit_warn Set exit level to Warning (Default Fatal)\n"
" -exit_error Set exit level to Error (Default Fatal)\n"
" -log_console Log to console\n"
" -v Get version of RTAB-Map\n"
" -input \"path\" Load previous database if it exists.\n");
@@ -118,7 +116,6 @@ int main(int argc, char * argv[])
int startAt = 1;
ParametersMap pm;
ULogger::Level logLevel = ULogger::kError;
ULogger::Level exitLevel = ULogger::kFatal;
bool logConsole = false;
for(int i=1; i<argc; ++i)
@@ -240,16 +237,6 @@ int main(int argc, char * argv[])
logLevel = ULogger::kWarning;
continue;
}
if(strcmp(argv[i], "-exit_warn") == 0)
{
exitLevel = ULogger::kWarning;
continue;
}
if(strcmp(argv[i], "-exit_error") == 0)
{
exitLevel = ULogger::kError;
continue;
}
if(strcmp(argv[i], "-log_console") == 0)
{
logConsole = true;
@@ -351,7 +338,6 @@ int main(int argc, char * argv[])
//ULogger::setType(ULogger::kTypeFile, rtabmap.getWorkingDir()+"/LogConsole.txt", false);
//ULogger::setBuffered(true);
ULogger::setLevel(logLevel);
ULogger::setExitLevel(exitLevel);
UTimer timer;
timer.start();
utilite/include/rtabmap/utilite/UException.h
+33
View File
@@ -0,0 +1,33 @@
/*
* utilite is a cross-platform library with
* useful utilities for fast and small developing.
* Copyright (C) 2010 Mathieu Labbe
*
* utilite is free library: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* utilite is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef UEXCEPTION_H_
#define UEXCEPTION_H_
#include <stdexcept>
class UException: public std::runtime_error {
public:
UException(const std::string & what) :
std::runtime_error(what)
{}
};
#endif /* UEXCEPTION_H_ */
utilite/include/rtabmap/utilite/ULogger.h
+5 -26
View File
@@ -25,6 +25,7 @@
#include "rtabmap/utilite/UMutex.h"
#include "rtabmap/utilite/UDestroyer.h"
#include "rtabmap/utilite/UEvent.h"
#include "rtabmap/utilite/UException.h"
#include <stdio.h>
#include <time.h>
@@ -51,14 +52,15 @@
#define ULOGGER_INFO(...) ULOGGER_LOG(ULogger::kInfo, __VA_ARGS__)
#define ULOGGER_WARN(...) ULOGGER_LOG(ULogger::kWarning, __VA_ARGS__)
#define ULOGGER_ERROR(...) ULOGGER_LOG(ULogger::kError, __VA_ARGS__)
#define ULOGGER_FATAL(...) ULOGGER_LOG(ULogger::kFatal, __VA_ARGS__)
#define ULOGGER_FATAL(...) ULOGGER_LOG(ULogger::kFatal, __VA_ARGS__) // Throw UException
#define UDEBUG(...) ULOGGER_DEBUG(__VA_ARGS__)
#define UINFO(...) ULOGGER_INFO(__VA_ARGS__)
#define UWARN(...) ULOGGER_WARN(__VA_ARGS__)
#define UERROR(...) ULOGGER_ERROR(__VA_ARGS__)
#define UFATAL(...) ULOGGER_FATAL(__VA_ARGS__)
#define UFATAL(...) ULOGGER_FATAL(__VA_ARGS__) // Throw UException
// Throw UException
#define UASSERT(condition) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met!", #condition)
#define UASSERT_MSG(condition, msg_str) if(!(condition)) ULogger::write(ULogger::kFatal, __FILE__, __LINE__, __FUNCTION__, "Condition (%s) not met! [%s]", #condition, msg_str)
@@ -335,15 +337,6 @@ public:
static void setLevel(ULogger::Level level) {level_ = level;}
static ULogger::Level level() {return level_;}
/**
* Make application to exit when a log with level is written (useful for debugging). The message is printed to
* console (whatever the logger type) and an ULogEvent is sent (synchronously... see UEventsManager::post()) before exiting.
*
* Note : A kFatal level will always exit whatever the level specified here.
*/
static void setExitLevel(ULogger::Level exitLevel) {exitLevel_ = exitLevel;}
static ULogger::Level exitLevel() {return exitLevel_;}
/**
* An ULogEvent is sent on each message logged at the specified level.
* Note : On message with level >= exitLevel, the event is sent synchronously (see UEventsManager::post()).
@@ -386,7 +379,7 @@ public:
int line,
const char *function,
const char* msg,
...);
...) throw(UException);
/**
* Get the time in the format "2008-7-13 12:23:44".
@@ -538,12 +531,6 @@ private:
*/
static Level level_;
/*
* The severity at which the application exits.
* Note : A FATAL level will always exit whatever the level specified here.
*/
static Level exitLevel_;
/*
* The severity at which the message is also sent in a ULogEvent.
*/
@@ -563,14 +550,6 @@ private:
static bool buffered_;
static std::string bufferedMsgs_;
/*
* State attribute. This state happens when an exit level
* message is received.
* Messages received during this state are not logged.
* @see exitLevel_
*/
static bool exitingState_;
};
#endif // ULOGGER_H
utilite/src/ULogger.cpp
+15 -23
View File
@@ -53,9 +53,7 @@ bool ULogger::printWhereFullPath_ = false;
bool ULogger::printThreadID_ = false;
bool ULogger::limitWhereLength_ = false;
bool ULogger::buffered_ = false;
bool ULogger::exitingState_ = false;
ULogger::Level ULogger::level_ = kInfo; // By default, we show all info msgs + upper level (Warning, Error)
ULogger::Level ULogger::exitLevel_ = kFatal;
ULogger::Level ULogger::eventLevel_ = kFatal;
const char * ULogger::levelName_[5] = {"DEBUG", " INFO", " WARN", "ERROR", "FATAL"};
ULogger* ULogger::instance_ = 0;
@@ -313,18 +311,13 @@ void ULogger::write(ULogger::Level level,
const char* msg,
...)
{
if(exitingState_)
{
// Ignore messages after a fatal exit...
return;
}
loggerMutex_.lock();
if(type_ == kTypeNoLog && level < kFatal)
{
loggerMutex_.unlock();
return;
}
if(strlen(msg) == 0 && !printWhere_ && level < exitLevel_)
if(strlen(msg) == 0 && !printWhere_ && level < kFatal)
{
loggerMutex_.unlock();
// No need to show an empty message if we don't print where.
@@ -502,11 +495,10 @@ void ULogger::write(ULogger::Level level,
va_start(args, msg);
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(level >= exitLevel_)
if(level >= kFatal)
{
// Send it synchronously, then receivers
// can do something before the code (exiting) below is executed.
exitingState_ = true;
UEventsManager::post(new ULogEvent(fullMsg, kFatal), false);
}
else
@@ -515,23 +507,23 @@ void ULogger::write(ULogger::Level level,
}
}
if(level >= exitLevel_)
if(level >= kFatal)
{
printf("\n*******\n%s message occurred! Application will now exit.\n", levelName_[level]);
if(type_ != kTypeConsole)
std::string fullMsg = uFormat("%s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
fullMsg.append(uFormatv(msg, args));
va_end(args);
if(instance_)
{
printf(" %s%s%s%s", levelStr.c_str(), pidStr.c_str(), time.c_str(), whereStr.c_str());
va_start(args, msg);
vprintf(msg, args);
va_end(args);
destroyer_.setDoomed(0);
delete instance_; // If a FileLogger is used, this will close the file.
instance_ = 0;
}
printf("\n*******\n");
destroyer_.setDoomed(0);
delete instance_; // If a FileLogger is used, this will close the file.
instance_ = 0;
//========================================================================
// EXIT APPLICATION
exit(1);
// Throw exception
loggerMutex_.unlock();
throw UException(fullMsg);
//========================================================================
}
}