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/StereoCameraModel.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 StereoCameraModel::StereoCameraModel(
00038 const std::string & name,
00039 const cv::Size & imageSize1,
00040 const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
00041 const cv::Size & imageSize2,
00042 const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
00043 const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
00044 const Transform & localTransform) :
00045 leftSuffix_("left"),
00046 rightSuffix_("right"),
00047 left_(name+"_"+leftSuffix_, imageSize1, K1, D1, R1, P1, localTransform),
00048 right_(name+"_"+rightSuffix_, imageSize2, K2, D2, R2, P2, localTransform),
00049 name_(name),
00050 R_(R),
00051 T_(T),
00052 E_(E),
00053 F_(F)
00054 {
00055 UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
00056 UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
00057 UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
00058 UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
00059 }
00060
00061 StereoCameraModel::StereoCameraModel(
00062 const std::string & name,
00063 const CameraModel & leftCameraModel,
00064 const CameraModel & rightCameraModel,
00065 const cv::Mat & R,
00066 const cv::Mat & T,
00067 const cv::Mat & E,
00068 const cv::Mat & F) :
00069 leftSuffix_("left"),
00070 rightSuffix_("right"),
00071 left_(leftCameraModel),
00072 right_(rightCameraModel),
00073 name_(name),
00074 R_(R),
00075 T_(T),
00076 E_(E),
00077 F_(F)
00078 {
00079 left_.setName(name+"_"+getLeftSuffix());
00080 right_.setName(name+"_"+getRightSuffix());
00081 UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
00082 UASSERT(T_.empty() || (T_.rows == 3 && T_.cols == 1 && T_.type() == CV_64FC1));
00083 UASSERT(E_.empty() || (E_.rows == 3 && E_.cols == 3 && E_.type() == CV_64FC1));
00084 UASSERT(F_.empty() || (F_.rows == 3 && F_.cols == 3 && F_.type() == CV_64FC1));
00085
00086 if(!R_.empty() && !T_.empty())
00087 {
00088 UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
00089
00090 cv::Mat R1,R2,P1,P2,Q;
00091 cv::stereoRectify(left_.K_raw(), left_.D_raw(),
00092 right_.K_raw(), right_.D_raw(),
00093 left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
00094 cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
00095
00096 left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
00097 right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
00098 }
00099 }
00100
00101 StereoCameraModel::StereoCameraModel(
00102 const std::string & name,
00103 const CameraModel & leftCameraModel,
00104 const CameraModel & rightCameraModel,
00105 const Transform & extrinsics) :
00106 leftSuffix_("left"),
00107 rightSuffix_("right"),
00108 left_(leftCameraModel),
00109 right_(rightCameraModel),
00110 name_(name)
00111 {
00112 left_.setName(name+"_"+getLeftSuffix());
00113 right_.setName(name+"_"+getRightSuffix());
00114
00115 if(!extrinsics.isNull())
00116 {
00117 UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
00118
00119 extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
00120 extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
00121
00122 cv::Mat R1,R2,P1,P2,Q;
00123 cv::stereoRectify(left_.K_raw(), left_.D_raw(),
00124 right_.K_raw(), right_.D_raw(),
00125 left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
00126 cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
00127
00128 left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
00129 right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
00130 }
00131 }
00132
00133 StereoCameraModel::StereoCameraModel(
00134 double fx,
00135 double fy,
00136 double cx,
00137 double cy,
00138 double baseline,
00139 const Transform & localTransform,
00140 const cv::Size & imageSize) :
00141 leftSuffix_("left"),
00142 rightSuffix_("right"),
00143 left_(fx, fy, cx, cy, localTransform, 0, imageSize),
00144 right_(fx, fy, cx, cy, localTransform, baseline*-fx, imageSize)
00145 {
00146 }
00147
00148
00149 StereoCameraModel::StereoCameraModel(
00150 const std::string & name,
00151 double fx,
00152 double fy,
00153 double cx,
00154 double cy,
00155 double baseline,
00156 const Transform & localTransform,
00157 const cv::Size & imageSize) :
00158 leftSuffix_("left"),
00159 rightSuffix_("right"),
00160 left_(name+"_"+getLeftSuffix(), fx, fy, cx, cy, localTransform, 0, imageSize),
00161 right_(name+"_"+getRightSuffix(), fx, fy, cx, cy, localTransform, baseline*-fx, imageSize),
00162 name_(name)
00163 {
00164 }
00165
00166 void StereoCameraModel::setName(const std::string & name, const std::string & leftSuffix, const std::string & rightSuffix)
00167 {
00168 name_=name;
00169 leftSuffix_ = leftSuffix;
00170 rightSuffix_ = rightSuffix;
00171 left_.setName(name_+"_"+getLeftSuffix());
00172 right_.setName(name_+"_"+getRightSuffix());
00173 }
00174
00175 bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
00176 {
00177 name_ = cameraName;
00178 bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix());
00179 bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix());
00180 if(leftLoaded && rightLoaded)
00181 {
00182 if(ignoreStereoTransform)
00183 {
00184 return true;
00185 }
00186
00187 R_ = cv::Mat();
00188 T_ = cv::Mat();
00189 E_ = cv::Mat();
00190 F_ = cv::Mat();
00191
00192 std::string filePath = directory+"/"+cameraName+"_pose.yaml";
00193 if(UFile::exists(filePath))
00194 {
00195 UINFO("Reading stereo calibration file \"%s\"", filePath.c_str());
00196 cv::FileStorage fs(filePath, cv::FileStorage::READ);
00197
00198 cv::FileNode n;
00199
00200 n = fs["camera_name"];
00201 if(n.type() != cv::FileNode::NONE)
00202 {
00203 name_ = (int)n;
00204 }
00205 else
00206 {
00207 UWARN("Missing \"camera_name\" field in \"%s\"", filePath.c_str());
00208 }
00209
00210
00211 n = fs["rotation_matrix"];
00212 if(n.type() != cv::FileNode::NONE)
00213 {
00214 int rows = (int)n["rows"];
00215 int cols = (int)n["cols"];
00216 std::vector<double> data;
00217 n["data"] >> data;
00218 UASSERT(rows*cols == (int)data.size());
00219 UASSERT(rows == 3 && cols == 3);
00220 R_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00221 }
00222 else
00223 {
00224 UWARN("Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
00225 }
00226
00227 n = fs["translation_matrix"];
00228 if(n.type() != cv::FileNode::NONE)
00229 {
00230 int rows = (int)n["rows"];
00231 int cols = (int)n["cols"];
00232 std::vector<double> data;
00233 n["data"] >> data;
00234 UASSERT(rows*cols == (int)data.size());
00235 UASSERT(rows == 3 && cols == 1);
00236 T_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00237 }
00238 else
00239 {
00240 UWARN("Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
00241 }
00242
00243 n = fs["essential_matrix"];
00244 if(n.type() != cv::FileNode::NONE)
00245 {
00246 int rows = (int)n["rows"];
00247 int cols = (int)n["cols"];
00248 std::vector<double> data;
00249 n["data"] >> data;
00250 UASSERT(rows*cols == (int)data.size());
00251 UASSERT(rows == 3 && cols == 3);
00252 E_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00253 }
00254 else
00255 {
00256 UWARN("Missing \"essential_matrix\" field in \"%s\"", filePath.c_str());
00257 }
00258
00259 n = fs["fundamental_matrix"];
00260 if(n.type() != cv::FileNode::NONE)
00261 {
00262 int rows = (int)n["rows"];
00263 int cols = (int)n["cols"];
00264 std::vector<double> data;
00265 n["data"] >> data;
00266 UASSERT(rows*cols == (int)data.size());
00267 UASSERT(rows == 3 && cols == 3);
00268 F_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
00269 }
00270 else
00271 {
00272 UWARN("Missing \"fundamental_matrix\" field in \"%s\"", filePath.c_str());
00273 }
00274
00275 fs.release();
00276
00277 return true;
00278 }
00279 else
00280 {
00281 UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
00282 }
00283 }
00284 return false;
00285 }
00286 bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTransform) const
00287 {
00288 if(left_.save(directory) && right_.save(directory))
00289 {
00290 if(ignoreStereoTransform)
00291 {
00292 return true;
00293 }
00294 return saveStereoTransform(directory);
00295 }
00296 return false;
00297 }
00298
00299 bool StereoCameraModel::saveStereoTransform(const std::string & directory) const
00300 {
00301 std::string filePath = directory+"/"+name_+"_pose.yaml";
00302 if(!filePath.empty() && (!R_.empty() && !T_.empty()))
00303 {
00304 UINFO("Saving stereo calibration to file \"%s\"", filePath.c_str());
00305 cv::FileStorage fs(filePath, cv::FileStorage::WRITE);
00306
00307
00308
00309 if(!name_.empty())
00310 {
00311 fs << "camera_name" << name_;
00312 }
00313
00314 if(!R_.empty())
00315 {
00316 fs << "rotation_matrix" << "{";
00317 fs << "rows" << R_.rows;
00318 fs << "cols" << R_.cols;
00319 fs << "data" << std::vector<double>((double*)R_.data, ((double*)R_.data)+(R_.rows*R_.cols));
00320 fs << "}";
00321 }
00322
00323 if(!T_.empty())
00324 {
00325 fs << "translation_matrix" << "{";
00326 fs << "rows" << T_.rows;
00327 fs << "cols" << T_.cols;
00328 fs << "data" << std::vector<double>((double*)T_.data, ((double*)T_.data)+(T_.rows*T_.cols));
00329 fs << "}";
00330 }
00331
00332 if(!E_.empty())
00333 {
00334 fs << "essential_matrix" << "{";
00335 fs << "rows" << E_.rows;
00336 fs << "cols" << E_.cols;
00337 fs << "data" << std::vector<double>((double*)E_.data, ((double*)E_.data)+(E_.rows*E_.cols));
00338 fs << "}";
00339 }
00340
00341 if(!F_.empty())
00342 {
00343 fs << "fundamental_matrix" << "{";
00344 fs << "rows" << F_.rows;
00345 fs << "cols" << F_.cols;
00346 fs << "data" << std::vector<double>((double*)F_.data, ((double*)F_.data)+(F_.rows*F_.cols));
00347 fs << "}";
00348 }
00349
00350 fs.release();
00351
00352 return true;
00353 }
00354 else
00355 {
00356 UERROR("Failed saving stereo extrinsics (they are null).");
00357 }
00358 return false;
00359 }
00360
00361 void StereoCameraModel::scale(double scale)
00362 {
00363 left_ = left_.scaled(scale);
00364 right_ = right_.scaled(scale);
00365 }
00366
00367 void StereoCameraModel::roi(const cv::Rect & roi)
00368 {
00369 left_ = left_.roi(roi);
00370 right_ = right_.roi(roi);
00371 }
00372
00373 float StereoCameraModel::computeDepth(float disparity) const
00374 {
00375
00376 UASSERT(this->isValidForProjection());
00377 if(disparity == 0.0f)
00378 {
00379 return 0.0f;
00380 }
00381 return baseline() * left().fx() / (disparity + right().cx() - left().cx());
00382 }
00383
00384 float StereoCameraModel::computeDisparity(float depth) const
00385 {
00386
00387 UASSERT(this->isValidForProjection());
00388 if(depth == 0.0f)
00389 {
00390 return 0.0f;
00391 }
00392 return baseline() * left().fx() / depth - right().cx() + left().cx();
00393 }
00394
00395 float StereoCameraModel::computeDisparity(unsigned short depth) const
00396 {
00397
00398 UASSERT(this->isValidForProjection());
00399 if(depth == 0)
00400 {
00401 return 0.0f;
00402 }
00403 return baseline() * left().fx() / (float(depth)/1000.0f) - right().cx() + left().cx();
00404 }
00405
00406 Transform StereoCameraModel::stereoTransform() const
00407 {
00408 if(!R_.empty() && !T_.empty())
00409 {
00410 return Transform(
00411 R_.at<double>(0,0), R_.at<double>(0,1), R_.at<double>(0,2), T_.at<double>(0),
00412 R_.at<double>(1,0), R_.at<double>(1,1), R_.at<double>(1,2), T_.at<double>(1),
00413 R_.at<double>(2,0), R_.at<double>(2,1), R_.at<double>(2,2), T_.at<double>(2));
00414 }
00415 return Transform();
00416 }
00417
00418 }