rtabmap: CameraModel.cpp Source File

Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 <opencv2/imgproc/imgproc.hpp>
00034 
00035 namespace rtabmap {
00036 
00037 CameraModel::CameraModel()
00038 {
00039 
00040 }
00041 
00042 CameraModel::CameraModel(
00043                 const std::string & cameraName,
00044                 const cv::Size & imageSize,
00045                 const cv::Mat & K,
00046                 const cv::Mat & D,
00047                 const cv::Mat & R,
00048                 const cv::Mat & P,
00049                 const Transform & localTransform) :
00050                 name_(cameraName),
00051                 imageSize_(imageSize),
00052                 K_(K),
00053                 D_(D),
00054                 R_(R),
00055                 P_(P),
00056                 localTransform_(localTransform)
00057 {
00058         UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
00059         UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
00060         UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
00061         UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
00062 }
00063 
00064 CameraModel::CameraModel(
00065                 double fx,
00066                 double fy,
00067                 double cx,
00068                 double cy,
00069                 const Transform & localTransform,
00070                 double Tx,
00071                 const cv::Size & imageSize) :
00072                 imageSize_(imageSize),
00073                 K_(cv::Mat::eye(3, 3, CV_64FC1)),
00074                 localTransform_(localTransform)
00075 {
00076         UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
00077         UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
00078         UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
00079         UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
00080         UASSERT(!localTransform.isNull());
00081         if(Tx != 0.0)
00082         {
00083                 P_ = cv::Mat::eye(3, 4, CV_64FC1),
00084                 P_.at<double>(0,0) = fx;
00085                 P_.at<double>(1,1) = fy;
00086                 P_.at<double>(0,2) = cx;
00087                 P_.at<double>(1,2) = cy;
00088                 P_.at<double>(0,3) = Tx;
00089         }
00090 
00091         K_.at<double>(0,0) = fx;
00092         K_.at<double>(1,1) = fy;
00093         K_.at<double>(0,2) = cx;
00094         K_.at<double>(1,2) = cy;
00095 }
00096 
00097 CameraModel::CameraModel(
00098                 const std::string & name,
00099                 double fx,
00100                 double fy,
00101                 double cx,
00102                 double cy,
00103                 const Transform & localTransform,
00104                 double Tx,
00105                 const cv::Size & imageSize) :
00106                 name_(name),
00107                 imageSize_(imageSize),
00108                 K_(cv::Mat::eye(3, 3, CV_64FC1)),
00109                 localTransform_(localTransform)
00110 {
00111         UASSERT_MSG(fx > 0.0, uFormat("fx=%f", fx).c_str());
00112         UASSERT_MSG(fy > 0.0, uFormat("fy=%f", fy).c_str());
00113         UASSERT_MSG(cx >= 0.0, uFormat("cx=%f", cx).c_str());
00114         UASSERT_MSG(cy >= 0.0, uFormat("cy=%f", cy).c_str());
00115         UASSERT(!localTransform.isNull());
00116         if(Tx != 0.0)
00117         {
00118                 P_ = cv::Mat::eye(3, 4, CV_64FC1),
00119                 P_.at<double>(0,0) = fx;
00120                 P_.at<double>(1,1) = fy;
00121                 P_.at<double>(0,2) = cx;
00122                 P_.at<double>(1,2) = cy;
00123                 P_.at<double>(0,3) = Tx;
00124         }
00125 
00126         K_.at<double>(0,0) = fx;
00127         K_.at<double>(1,1) = fy;
00128         K_.at<double>(0,2) = cx;
00129         K_.at<double>(1,2) = cy;
00130 }
00131 
00132 void CameraModel::initRectificationMap()
00133 {
00134         UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
00135         UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
00136         UASSERT(R_.rows == 3 && R_.cols == 3);
00137         UASSERT(P_.rows == 3 && P_.cols == 4);
00138         // init rectification map
00139         UINFO("Initialize rectify map");
00140         cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
00141 }
00142 
00143 bool CameraModel::load(const std::string & directory, const std::string & cameraName)
00144 {
00145         K_ = cv::Mat();
00146         D_ = cv::Mat();
00147         R_ = cv::Mat();
00148         P_ = cv::Mat();
00149         mapX_ = cv::Mat();
00150         mapY_ = cv::Mat();
00151         name_.clear();
00152         imageSize_ = cv::Size();
00153 
00154         std::string filePath = directory+"/"+cameraName+".yaml";
00155         if(UFile::exists(filePath))
00156         {
00157                 try
00158                 {
00159                         UINFO("Reading calibration file \"%s\"", filePath.c_str());
00160                         cv::FileStorage fs(filePath, cv::FileStorage::READ);
00161 
00162                         cv::FileNode n,n2;
00163 
00164                         n = fs["camera_name"];
00165                         if(n.type() != cv::FileNode::NONE)
00166                         {
00167                                 name_ = (int)n;
00168                         }
00169                         else
00170                         {
00171                                 UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
00172                         }
00173 
00174                         n = fs["image_width"];
00175                         n2 = fs["image_height"];
00176                         if(n.type() != cv::FileNode::NONE)
00177                         {
00178                                 imageSize_.width = (int)fs["image_width"];
00179                                 imageSize_.height = (int)fs["image_height"];
00180                         }
00181                         else
00182                         {
00183                                 UWARN("Missing \"image_width\" and/or \"image_height\" fields in \"%s\"", filePath.c_str());
00184                         }
00185 
00186                         // import from ROS calibration format
00187                         n = fs["camera_matrix"];
00188                         if(n.type() != cv::FileNode::NONE)
00189                         {
00190                                 int rows = (int)n["rows"];
00191                                 int cols = (int)n["cols"];
00192                                 std::vector<double> data;
00193                                 n["data"] >> data;
00194                                 UASSERT(rows*cols == (int)data.size());
00195                                 UASSERT(rows == 3 && cols == 3);
00196                                 K_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00197                         }
00198                         else
00199                         {
00200                                 UWARN("Missing \"camera_matrix\" field in \"%s\"", filePath.c_str());
00201                         }
00202 
00203                         n = fs["distortion_coefficients"];
00204                         if(n.type() != cv::FileNode::NONE)
00205                         {
00206                                 int rows = (int)n["rows"];
00207                                 int cols = (int)n["cols"];
00208                                 std::vector<double> data;
00209                                 n["data"] >> data;
00210                                 UASSERT(rows*cols == (int)data.size());
00211                                 UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
00212                                 D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00213                         }
00214                         else
00215                         {
00216                                 UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
00217                         }
00218 
00219                         n = fs["rectification_matrix"];
00220                         if(n.type() != cv::FileNode::NONE)
00221                         {
00222                                 int rows = (int)n["rows"];
00223                                 int cols = (int)n["cols"];
00224                                 std::vector<double> data;
00225                                 n["data"] >> data;
00226                                 UASSERT(rows*cols == (int)data.size());
00227                                 UASSERT(rows == 3 && cols == 3);
00228                                 R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00229                         }
00230                         else
00231                         {
00232                                 UWARN("Missing \"rectification_matrix\" field in \"%s\"", filePath.c_str());
00233                         }
00234 
00235                         n = fs["projection_matrix"];
00236                         if(n.type() != cv::FileNode::NONE)
00237                         {
00238                                 int rows = (int)n["rows"];
00239                                 int cols = (int)n["cols"];
00240                                 std::vector<double> data;
00241                                 n["data"] >> data;
00242                                 UASSERT(rows*cols == (int)data.size());
00243                                 UASSERT(rows == 3 && cols == 4);
00244                                 P_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00245                         }
00246                         else
00247                         {
00248                                 UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
00249                         }
00250 
00251                         fs.release();
00252 
00253                         if(isValidForRectification())
00254                         {
00255                                 initRectificationMap();
00256                         }
00257 
00258                         return true;
00259                 }
00260                 catch(const cv::Exception & e)
00261                 {
00262                         UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
00263                 }
00264         }
00265         else
00266         {
00267                 UWARN("Could not load calibration file \"%s\".", filePath.c_str());
00268         }
00269         return false;
00270 }
00271 
00272 bool CameraModel::save(const std::string & directory) const
00273 {
00274         std::string filePath = directory+"/"+name_+".yaml";
00275         if(!filePath.empty() && (!K_.empty() || !D_.empty() || !R_.empty() || !P_.empty()))
00276         {
00277                 UINFO("Saving calibration to file \"%s\"", filePath.c_str());
00278                 cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
00279 
00280                 // export in ROS calibration format
00281 
00282                 if(!name_.empty())
00283                 {
00284                         fs << "camera_name" << name_;
00285                 }
00286                 if(imageSize_.width>0 && imageSize_.height>0)
00287                 {
00288                         fs << "image_width" << imageSize_.width;
00289                         fs << "image_height" << imageSize_.height;
00290                 }
00291 
00292                 if(!K_.empty())
00293                 {
00294                         fs << "camera_matrix" << "{";
00295                         fs << "rows" << K_.rows;
00296                         fs << "cols" << K_.cols;
00297                         fs << "data" << std::vector<double>((double*)K_.data, ((double*)K_.data)+(K_.rows*K_.cols));
00298                         fs << "}";
00299                 }
00300 
00301                 if(!D_.empty())
00302                 {
00303                         fs << "distortion_coefficients" << "{";
00304                         fs << "rows" << D_.rows;
00305                         fs << "cols" << D_.cols;
00306                         fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
00307                         fs << "}";
00308 
00309                         // compaibility with ROS
00310                         if(D_.cols > 5)
00311                         {
00312                                 fs << "distortion_model" << "rational_polynomial";
00313                         }
00314                         else
00315                         {
00316                                 fs << "distortion_model" << "plumb_bob";
00317                         }
00318                 }
00319 
00320                 if(!R_.empty())
00321                 {
00322                         fs << "rectification_matrix" << "{";
00323                         fs << "rows" << R_.rows;
00324                         fs << "cols" << R_.cols;
00325                         fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
00326                         fs << "}";
00327                 }
00328 
00329                 if(!P_.empty())
00330                 {
00331                         fs << "projection_matrix" << "{";
00332                         fs << "rows" << P_.rows;
00333                         fs << "cols" << P_.cols;
00334                         fs << "data" << std::vector<double>((double*)P_.data, ((double*)P_.data)+(P_.rows*P_.cols));
00335                         fs << "}";
00336                 }
00337 
00338                 fs.release();
00339 
00340                 return true;
00341         }
00342         else
00343         {
00344                 UERROR("Cannot save calibration to \"%s\" because it is empty.", filePath.c_str());
00345         }
00346         return false;
00347 }
00348 
00349 CameraModel CameraModel::scaled(double scale) const
00350 {
00351         CameraModel scaledModel = *this;
00352         UASSERT(scale > 0.0);
00353         if(this->isValidForProjection())
00354         {
00355                 // has only effect on K and P
00356                 cv::Mat K;
00357                 if(!K_.empty())
00358                 {
00359                         K = K_.clone();
00360                         K.at<double>(0,0) *= scale;
00361                         K.at<double>(1,1) *= scale;
00362                         K.at<double>(0,2) *= scale;
00363                         K.at<double>(1,2) *= scale;
00364                 }
00365 
00366                 cv::Mat P;
00367                 if(!P_.empty())
00368                 {
00369                         P = P_.clone();
00370                         P.at<double>(0,0) *= scale;
00371                         P.at<double>(1,1) *= scale;
00372                         P.at<double>(0,2) *= scale;
00373                         P.at<double>(1,2) *= scale;
00374                         P.at<double>(0,3) *= scale;
00375                         P.at<double>(1,3) *= scale;
00376                 }
00377                 scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
00378         }
00379         else
00380         {
00381                 UWARN("Trying to scale a camera model not valid! Ignoring scaling...");
00382         }
00383         return scaledModel;
00384 }
00385 
00386 double CameraModel::horizontalFOV() const
00387 {
00388         if(imageWidth() > 0 && fx() > 0.0)
00389         {
00390                 return atan((double(imageWidth())/2.0)/fx())*2.0*180.0/CV_PI;
00391         }
00392         return 0.0;
00393 }
00394 
00395 double CameraModel::verticalFOV() const
00396 {
00397         if(imageHeight() > 0 && fy() > 0.0)
00398         {
00399                 return atan((double(imageHeight())/2.0)/fy())*2.0*180.0/CV_PI;
00400         }
00401         return 0.0;
00402 }
00403 
00404 cv::Mat CameraModel::rectifyImage(const cv::Mat & raw, int interpolation) const
00405 {
00406         UDEBUG("");
00407         if(!mapX_.empty() && !mapY_.empty())
00408         {
00409                 cv::Mat rectified;
00410                 cv::remap(raw, rectified, mapX_, mapY_, interpolation);
00411                 return rectified;
00412         }
00413         else
00414         {
00415                 UERROR("Cannot rectify image because the rectify map is not initialized.");
00416                 return raw.clone();
00417         }
00418 }
00419 
00420 //inspired from https://github.com/code-iai/iai_kinect2/blob/master/depth_registration/src/depth_registration_cpu.cpp
00421 cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
00422 {
00423         UDEBUG("");
00424         UASSERT(raw.type() == CV_16UC1);
00425         if(!mapX_.empty() && !mapY_.empty())
00426         {
00427                 cv::Mat rectified = cv::Mat::zeros(mapX_.rows, mapX_.cols, raw.type());
00428                 for(int y=0; y<mapX_.rows; ++y)
00429                 {
00430                         for(int x=0; x<mapX_.cols; ++x)
00431                         {
00432                                 cv::Point2f pt(mapX_.at<float>(y,x), mapY_.at<float>(y,x));
00433                                 int xL = (int)floor(pt.x);
00434                                 int xH = (int)ceil(pt.x);
00435                                 int yL = (int)floor(pt.y);
00436                                 int yH = (int)ceil(pt.y);
00437                                 if(xL >= 0 && yL >= 0 && xH < raw.cols && yH < raw.rows)
00438                                 {
00439                                         const unsigned short & pLT = raw.at<unsigned short>(yL, xL);
00440                                         const unsigned short & pRT = raw.at<unsigned short>(yL, xH);
00441                                         const unsigned short & pLB = raw.at<unsigned short>(yH, xL);
00442                                         const unsigned short & pRB = raw.at<unsigned short>(yH, xH);
00443                                         if(pLT > 0 && pRT > 0 && pLB > 0 && pRB > 0)
00444                                         {
00445                                                 unsigned short avg = (pLT + pRT + pLB + pRB) / 4;
00446                                                 unsigned short thres = 0.01 * avg;
00447                                                 if( abs(pLT - avg) < thres &&
00448                                                         abs(pRT - avg) < thres &&
00449                                                         abs(pLB - avg) < thres &&
00450                                                         abs(pRB - avg) < thres)
00451                                                 {
00452                                                         //bilinear interpolation
00453                                                         float a = pt.x - (float)xL;
00454                                                         float c = pt.y - (float)yL;
00455 
00456                                                         //http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
00457                                                         rectified.at<unsigned short>(y,x) =
00458                                                                         (raw.at<unsigned short>(yL, xL) * (1.f - a) + raw.at<unsigned short>(yL, xH) * a) * (1.f - c) +
00459                                                                         (raw.at<unsigned short>(yH, xL) * (1.f - a) + raw.at<unsigned short>(yH, xH) * a) * c;
00460                                                 }
00461                                         }
00462                                 }
00463                         }
00464                 }
00465                 return rectified;
00466         }
00467         else
00468         {
00469                 UERROR("Cannot rectify image because the rectify map is not initialized.");
00470                 return raw.clone();
00471         }
00472 }
00473 
00474 } /* namespace rtabmap */