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