00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/CameraModel.h>
00029 #include <rtabmap/utilite/ULogger.h>
00030 #include <rtabmap/utilite/UDirectory.h>
00031 #include <rtabmap/utilite/UFile.h>
00032 #include <rtabmap/utilite/UConversion.h>
00033 #include <rtabmap/utilite/UMath.h>
00034 #include <rtabmap/utilite/UStl.h>
00035 #include <opencv2/imgproc/imgproc.hpp>
00036
00037 namespace rtabmap {
00038
00039 CameraModel::CameraModel()
00040 {
00041
00042 }
00043
00044 CameraModel::CameraModel(
00045 const std::string & cameraName,
00046 const cv::Size & imageSize,
00047 const cv::Mat & K,
00048 const cv::Mat & D,
00049 const cv::Mat & R,
00050 const cv::Mat & P,
00051 const Transform & localTransform) :
00052 name_(cameraName),
00053 imageSize_(imageSize),
00054 K_(K),
00055 D_(D),
00056 R_(R),
00057 P_(P),
00058 localTransform_(localTransform)
00059 {
00060 UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
00061 UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
00062 UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
00063 UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
00064 }
00065
00066 CameraModel::CameraModel(
00067 double fx,
00068 double fy,
00069 double cx,
00070 double cy,
00071 const Transform & localTransform,
00072 double Tx,
00073 const cv::Size & imageSize) :
00074 imageSize_(imageSize),
00075 K_(cv::Mat::eye(3, 3, CV_64FC1)),
00076 localTransform_(localTransform)
00077 {
00078 UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
00079 UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
00080 UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
00081 UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
00082 UASSERT(!localTransform.isNull());
00083
00084 if(cx==0.0 && imageSize.width > 0)
00085 {
00086 cx = double(imageSize.width)/2.0-0.5;
00087 }
00088 if(cy==0.0 && imageSize.height > 0)
00089 {
00090 cy = double(imageSize.height)/2.0-0.5;
00091 }
00092
00093 if(Tx != 0.0)
00094 {
00095 P_ = cv::Mat::eye(3, 4, CV_64FC1),
00096 P_.at<double>(0,0) = fx;
00097 P_.at<double>(1,1) = fy;
00098 P_.at<double>(0,2) = cx;
00099 P_.at<double>(1,2) = cy;
00100 P_.at<double>(0,3) = Tx;
00101 }
00102
00103 K_.at<double>(0,0) = fx;
00104 K_.at<double>(1,1) = fy;
00105 K_.at<double>(0,2) = cx;
00106 K_.at<double>(1,2) = cy;
00107 }
00108
00109 CameraModel::CameraModel(
00110 const std::string & name,
00111 double fx,
00112 double fy,
00113 double cx,
00114 double cy,
00115 const Transform & localTransform,
00116 double Tx,
00117 const cv::Size & imageSize) :
00118 name_(name),
00119 imageSize_(imageSize),
00120 K_(cv::Mat::eye(3, 3, CV_64FC1)),
00121 localTransform_(localTransform)
00122 {
00123 UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
00124 UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
00125 UASSERT_MSG(cx >= 0.0 && imageSize.width>=0, uFormat("cx=%f imageSize.width=%d", cx, imageSize.width).c_str());
00126 UASSERT_MSG(cy >= 0.0 && imageSize.height>=0, uFormat("cy=%f imageSize.height=%d", cy, imageSize.height).c_str());
00127 UASSERT(!localTransform.isNull());
00128
00129 if(cx==0.0 && imageSize.width > 0)
00130 {
00131 cx = double(imageSize.width)/2.0-0.5;
00132 }
00133 if(cy==0.0 && imageSize.height > 0)
00134 {
00135 cy = double(imageSize.height)/2.0-0.5;
00136 }
00137
00138 if(Tx != 0.0)
00139 {
00140 P_ = cv::Mat::eye(3, 4, CV_64FC1),
00141 P_.at<double>(0,0) = fx;
00142 P_.at<double>(1,1) = fy;
00143 P_.at<double>(0,2) = cx;
00144 P_.at<double>(1,2) = cy;
00145 P_.at<double>(0,3) = Tx;
00146 }
00147
00148 K_.at<double>(0,0) = fx;
00149 K_.at<double>(1,1) = fy;
00150 K_.at<double>(0,2) = cx;
00151 K_.at<double>(1,2) = cy;
00152 }
00153
00154 void CameraModel::initRectificationMap()
00155 {
00156 UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
00157 UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
00158 UASSERT(R_.rows == 3 && R_.cols == 3);
00159 UASSERT(P_.rows == 3 && P_.cols == 4);
00160
00161 UINFO("Initialize rectify map");
00162 if(D_.cols == 6)
00163 {
00164 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
00165
00166
00167 cv::Mat D(1, 4, CV_64FC1);
00168 D.at<double>(0,0) = D_.at<double>(0,1);
00169 D.at<double>(0,1) = D_.at<double>(0,2);
00170 D.at<double>(0,2) = D_.at<double>(0,4);
00171 D.at<double>(0,3) = D_.at<double>(0,5);
00172 cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
00173 }
00174 else
00175 #else
00176 UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
00177 CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
00178 }
00179 #endif
00180 {
00181
00182 cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
00183 }
00184 }
00185
00186 void CameraModel::setImageSize(const cv::Size & size)
00187 {
00188 UASSERT((size.height > 0 && size.width > 0) || (size.height == 0 && size.width == 0));
00189 imageSize_ = size;
00190 double ncx = cx();
00191 double ncy = cy();
00192 if(ncx==0.0 && imageSize_.width > 0)
00193 {
00194 ncx = double(imageSize_.width)/2.0-0.5;
00195 }
00196 if(ncy==0.0 && imageSize_.height > 0)
00197 {
00198 ncy = double(imageSize_.height)/2.0-0.5;
00199 }
00200 if(!P_.empty())
00201 {
00202 P_.at<double>(0,2) = ncx;
00203 P_.at<double>(1,2) = ncy;
00204 }
00205 if(!K_.empty())
00206 {
00207 K_.at<double>(0,2) = ncx;
00208 K_.at<double>(1,2) = ncy;
00209 }
00210 }
00211
00212 bool CameraModel::load(const std::string & directory, const std::string & cameraName)
00213 {
00214 K_ = cv::Mat();
00215 D_ = cv::Mat();
00216 R_ = cv::Mat();
00217 P_ = cv::Mat();
00218 mapX_ = cv::Mat();
00219 mapY_ = cv::Mat();
00220 name_.clear();
00221 imageSize_ = cv::Size();
00222
00223 std::string filePath = directory+"/"+cameraName+".yaml";
00224 if(UFile::exists(filePath))
00225 {
00226 try
00227 {
00228 UINFO("Reading calibration file \"%s\"", filePath.c_str());
00229 cv::FileStorage fs(filePath, cv::FileStorage::READ);
00230
00231 cv::FileNode n,n2;
00232
00233 n = fs["camera_name"];
00234 if(n.type() != cv::FileNode::NONE)
00235 {
00236 name_ = (int)n;
00237 }
00238 else
00239 {
00240 UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
00241 }
00242
00243 n = fs["image_width"];
00244 n2 = fs["image_height"];
00245 if(n.type() != cv::FileNode::NONE)
00246 {
00247 imageSize_.width = (int)fs["image_width"];
00248 imageSize_.height = (int)fs["image_height"];
00249 }
00250 else
00251 {
00252 UWARN("Missing \"image_width\" and/or \"image_height\" fields in \"%s\"", filePath.c_str());
00253 }
00254
00255
00256 n = fs["camera_matrix"];
00257 if(n.type() != cv::FileNode::NONE)
00258 {
00259 int rows = (int)n["rows"];
00260 int cols = (int)n["cols"];
00261 std::vector<double> data;
00262 n["data"] >> data;
00263 UASSERT(rows*cols == (int)data.size());
00264 UASSERT(rows == 3 && cols == 3);
00265 K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00266 }
00267 else
00268 {
00269 UWARN("Missing \"camera_matrix\" field in \"%s\"", filePath.c_str());
00270 }
00271
00272 n = fs["distortion_coefficients"];
00273 if(n.type() != cv::FileNode::NONE)
00274 {
00275 int rows = (int)n["rows"];
00276 int cols = (int)n["cols"];
00277 std::vector<double> data;
00278 n["data"] >> data;
00279 UASSERT(rows*cols == (int)data.size());
00280 UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
00281 D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00282 }
00283 else
00284 {
00285 UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
00286 }
00287
00288 n = fs["distortion_model"];
00289 if(n.type() != cv::FileNode::NONE)
00290 {
00291 std::string distortionModel = (std::string)n;
00292 if(D_.cols>=4 &&
00293 (uStrContains(distortionModel, "fisheye") ||
00294 uStrContains(distortionModel, "equidistant")))
00295 {
00296 cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
00297 D.at<double>(0,0) = D_.at<double>(0,0);
00298 D.at<double>(0,1) = D_.at<double>(0,1);
00299 D.at<double>(0,4) = D_.at<double>(0,2);
00300 D.at<double>(0,5) = D_.at<double>(0,3);
00301 D_ = D;
00302 }
00303 }
00304 else
00305 {
00306 UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
00307 }
00308
00309 n = fs["rectification_matrix"];
00310 if(n.type() != cv::FileNode::NONE)
00311 {
00312 int rows = (int)n["rows"];
00313 int cols = (int)n["cols"];
00314 std::vector<double> data;
00315 n["data"] >> data;
00316 UASSERT(rows*cols == (int)data.size());
00317 UASSERT(rows == 3 && cols == 3);
00318 R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00319 }
00320 else
00321 {
00322 UWARN("Missing \"rectification_matrix\" field in \"%s\"", filePath.c_str());
00323 }
00324
00325 n = fs["projection_matrix"];
00326 if(n.type() != cv::FileNode::NONE)
00327 {
00328 int rows = (int)n["rows"];
00329 int cols = (int)n["cols"];
00330 std::vector<double> data;
00331 n["data"] >> data;
00332 UASSERT(rows*cols == (int)data.size());
00333 UASSERT(rows == 3 && cols == 4);
00334 P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00335 }
00336 else
00337 {
00338 UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
00339 }
00340
00341 fs.release();
00342
00343 if(isValidForRectification())
00344 {
00345 initRectificationMap();
00346 }
00347
00348 return true;
00349 }
00350 catch(const cv::Exception & e)
00351 {
00352 UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
00353 }
00354 }
00355 else
00356 {
00357 UWARN("Could not load calibration file \"%s\".", filePath.c_str());
00358 }
00359 return false;
00360 }
00361
00362 bool CameraModel::save(const std::string & directory) const
00363 {
00364 std::string filePath = directory+"/"+name_+".yaml";
00365 if(!filePath.empty() && (!K_.empty() || !D_.empty() || !R_.empty() || !P_.empty()))
00366 {
00367 UINFO("Saving calibration to file \"%s\"", filePath.c_str());
00368 cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
00369
00370
00371
00372 if(!name_.empty())
00373 {
00374 fs << "camera_name" << name_;
00375 }
00376 if(imageSize_.width>0 && imageSize_.height>0)
00377 {
00378 fs << "image_width" << imageSize_.width;
00379 fs << "image_height" << imageSize_.height;
00380 }
00381
00382 if(!K_.empty())
00383 {
00384 fs << "camera_matrix" << "{";
00385 fs << "rows" << K_.rows;
00386 fs << "cols" << K_.cols;
00387 fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
00388 fs << "}";
00389 }
00390
00391 if(!D_.empty())
00392 {
00393 cv::Mat D = D_;
00394 if(D_.cols == 6)
00395 {
00396 D = cv::Mat(1,4,CV_64FC1);
00397 D.at<double>(0,0) = D_.at<double>(0,0);
00398 D.at<double>(0,1) = D_.at<double>(0,1);
00399 D.at<double>(0,2) = D_.at<double>(0,4);
00400 D.at<double>(0,3) = D_.at<double>(0,5);
00401 }
00402 fs << "distortion_coefficients" << "{";
00403 fs << "rows" << D.rows;
00404 fs << "cols" << D.cols;
00405 fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
00406 fs << "}";
00407
00408
00409 if(D_.cols == 6)
00410 {
00411 fs << "distortion_model" << "equidistant";
00412 }
00413 else if(D.cols > 5)
00414 {
00415 fs << "distortion_model" << "rational_polynomial";
00416 }
00417 else
00418 {
00419 fs << "distortion_model" << "plumb_bob";
00420 }
00421 }
00422
00423 if(!R_.empty())
00424 {
00425 fs << "rectification_matrix" << "{";
00426 fs << "rows" << R_.rows;
00427 fs << "cols" << R_.cols;
00428 fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
00429 fs << "}";
00430 }
00431
00432 if(!P_.empty())
00433 {
00434 fs << "projection_matrix" << "{";
00435 fs << "rows" << P_.rows;
00436 fs << "cols" << P_.cols;
00437 fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
00438 fs << "}";
00439 }
00440
00441 fs.release();
00442
00443 return true;
00444 }
00445 else
00446 {
00447 UERROR("Cannot save calibration to \"%s\" because it is empty.", filePath.c_str());
00448 }
00449 return false;
00450 }
00451
00452 CameraModel CameraModel::scaled(double scale) const
00453 {
00454 CameraModel scaledModel = *this;
00455 UASSERT(scale > 0.0);
00456 if(this->isValidForProjection())
00457 {
00458
00459 cv::Mat K;
00460 if(!K_.empty())
00461 {
00462 K = K_.clone();
00463 K.at<double>(0,0) *= scale;
00464 K.at<double>(1,1) *= scale;
00465 K.at<double>(0,2) *= scale;
00466 K.at<double>(1,2) *= scale;
00467 }
00468
00469 cv::Mat P;
00470 if(!P_.empty())
00471 {
00472 P = P_.clone();
00473 P.at<double>(0,0) *= scale;
00474 P.at<double>(1,1) *= scale;
00475 P.at<double>(0,2) *= scale;
00476 P.at<double>(1,2) *= scale;
00477 P.at<double>(0,3) *= scale;
00478 P.at<double>(1,3) *= scale;
00479 }
00480 scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
00481 }
00482 else
00483 {
00484 UWARN("Trying to scale a camera model not valid! Ignoring scaling...");
00485 }
00486 return scaledModel;
00487 }
00488
00489 CameraModel CameraModel::roi(const cv::Rect & roi) const
00490 {
00491 CameraModel roiModel = *this;
00492 if(this->isValidForProjection())
00493 {
00494
00495 cv::Mat K;
00496 if(!K_.empty())
00497 {
00498 K = K_.clone();
00499 K.at<double>(0,2) -= roi.x;
00500 K.at<double>(1,2) -= roi.y;
00501 }
00502
00503 cv::Mat P;
00504 if(!P_.empty())
00505 {
00506 P = P_.clone();
00507 P.at<double>(0,2) -= roi.x;
00508 P.at<double>(1,2) -= roi.y;
00509 }
00510 roiModel = CameraModel(name_, roi.size(), K, D_, R_, P, localTransform_);
00511 }
00512 else
00513 {
00514 UWARN("Trying to extract roi from a camera model not valid! Ignoring roi...");
00515 }
00516 return roiModel;
00517 }
00518
00519 double CameraModel::horizontalFOV() const
00520 {
00521 if(imageWidth() > 0 && fx() > 0.0)
00522 {
00523 return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
00524 }
00525 return 0.0;
00526 }
00527
00528 double CameraModel::verticalFOV() const
00529 {
00530 if(imageHeight() > 0 && fy() > 0.0)
00531 {
00532 return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
00533 }
00534 return 0.0;
00535 }
00536
00537 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
00538 {
00539 UDEBUG("");
00540 if(!mapX_.empty() && !mapY_.empty())
00541 {
00542 cv::Mat rectified;
00543 cv::remap(raw, rectified, mapX_, mapY_, interpolation);
00544 return rectified;
00545 }
00546 else
00547 {
00548 UERROR("Cannot rectify image because the rectify map is not initialized.");
00549 return raw.clone();
00550 }
00551 }
00552
00553
00554 cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
00555 {
00556 UDEBUG("");
00557 UASSERT(raw.type() == CV_16UC1);
00558 if(!mapX_.empty() && !mapY_.empty())
00559 {
00560 cv::Mat rectified = cv::Mat::zeros(mapX_.rows, mapX_.cols, raw.type());
00561 for(int y=0; y<mapX_.rows; ++y)
00562 {
00563 for(int x=0; x<mapX_.cols; ++x)
00564 {
00565 cv::Point2f pt(mapX_.at<float>(y,x), mapY_.at<float>(y,x));
00566 int xL = (int)floor(pt.x);
00567 int xH = (int)ceil(pt.x);
00568 int yL = (int)floor(pt.y);
00569 int yH = (int)ceil(pt.y);
00570 if(xL >= 0 && yL >= 0 && xH < raw.cols && yH < raw.rows)
00571 {
00572 const unsigned short & pLT = raw.at<unsigned short>(yL, xL);
00573 const unsigned short & pRT = raw.at<unsigned short>(yL, xH);
00574 const unsigned short & pLB = raw.at<unsigned short>(yH, xL);
00575 const unsigned short & pRB = raw.at<unsigned short>(yH, xH);
00576 if(pLT > 0 && pRT > 0 && pLB > 0 && pRB > 0)
00577 {
00578 unsigned short avg = (pLT + pRT + pLB + pRB) / 4;
00579 unsigned short thres = 0.01 * avg;
00580 if( abs(pLT - avg) < thres &&
00581 abs(pRT - avg) < thres &&
00582 abs(pLB - avg) < thres &&
00583 abs(pRB - avg) < thres)
00584 {
00585
00586 float a = pt.x - (float)xL;
00587 float c = pt.y - (float)yL;
00588
00589
00590 rectified.at<unsigned short>(y,x) =
00591 (pLT * (1.f - a) + pRT * a) * (1.f - c) +
00592 (pLB * (1.f - a) + pRB * a) * c;
00593 }
00594 }
00595 }
00596 }
00597 }
00598 return rectified;
00599 }
00600 else
00601 {
00602 UERROR("Cannot rectify image because the rectify map is not initialized.");
00603 return raw.clone();
00604 }
00605 }
00606
00607
00608 void CameraModel::project(float u, float v, float depth, float & x, float & y, float & z) const
00609 {
00610 if(depth > 0.0f)
00611 {
00612
00613 x = (u - cx()) * depth / fx();
00614 y = (v - cy()) * depth / fy();
00615 z = depth;
00616 }
00617 else
00618 {
00619 x = y = z = std::numeric_limits<float>::quiet_NaN();
00620 }
00621 }
00622
00623 void CameraModel::reproject(float x, float y, float z, float & u, float & v) const
00624 {
00625 UASSERT(z!=0.0f);
00626 float invZ = 1.0f/z;
00627 u = (fx()*x)*invZ + cx();
00628 v = (fy()*y)*invZ + cy();
00629 }
00630 void CameraModel::reproject(float x, float y, float z, int & u, int & v) const
00631 {
00632 UASSERT(z!=0.0f);
00633 float invZ = 1.0f/z;
00634 u = (fx()*x)*invZ + cx();
00635 v = (fy()*y)*invZ + cy();
00636 }
00637
00638 bool CameraModel::inFrame(int u, int v) const
00639 {
00640 return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
00641 }
00642
00643 }