mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-08 12:30:20 +08:00
-Updated CameraModel and StereoCameraModel so that a stereo camera model can be initialization from know intrinsics and extrinsics
-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
+213
-125
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user