diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp index 05e1221d36bc86bff88154567fd7afbbf34db91b..bfb2473497238422203e38132da7307758ba6235 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp @@ -50,7 +50,6 @@ StereoVideoSource::~StereoVideoSource() { void StereoVideoSource::init(const string &file) { capabilities_ = kCapVideo | kCapStereo; - calibrated_ = false; if (ftl::is_video(file)) { // Load video file @@ -94,7 +93,6 @@ void StereoVideoSource::init(const string &file) { if (calib_->loadCalibration(fname)) { calib_->calculateRectificationParameters(); calib_->setRectify(true); - calibrated_ = true; } } else { @@ -103,6 +101,16 @@ void StereoVideoSource::init(const string &file) { + std::string("calibration.yml"); LOG(ERROR) << "No calibration, default path set to " + fname; + + // set use config file/set (some) default values + + cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_64FC1); + K.at<double>(0,0) = host_->value("focal", 500.0); + K.at<double>(1,1) = host_->value("focal", 500.0); + K.at<double>(0,2) = host_->value("centre_x", -color_size_.width/2.0f); + K.at<double>(1,2) = host_->value("centre_y", -color_size_.height/2.0f); + + calib_->setIntrinsics(color_size_, {K, K}); } //////////////////////////////////////////////////////////////////////////// @@ -135,8 +143,6 @@ void StereoVideoSource::init(const string &file) { } } - calibrated_ = true; // Means we have intrinsics - return true; }); @@ -200,43 +206,27 @@ void StereoVideoSource::updateParameters() { float max_depth = this->host_->getConfig().value<double>("max_depth", 15.0); // left - - // FIXME: Check this change doesn't break anything (adding of calibrated_) - if (calibrated_) { - K = calib_->getCameraMatrixLeft(color_size_); - state_.getLeft() = { - static_cast<float>(K.at<double>(0,0)), // Fx - static_cast<float>(K.at<double>(1,1)), // Fy - static_cast<float>(-K.at<double>(0,2)), // Cx - static_cast<float>(-K.at<double>(1,2)), // Cy - (unsigned int) color_size_.width, - (unsigned int) color_size_.height, - min_depth, - max_depth, - baseline, - doff - }; - - host_->getConfig()["focal"] = params_.fx; - host_->getConfig()["centre_x"] = params_.cx; - host_->getConfig()["centre_y"] = params_.cy; - host_->getConfig()["baseline"] = params_.baseline; - host_->getConfig()["doffs"] = params_.doffs; - } else { - state_.getLeft() = { - host_->value("focal", 500.0f), // Fx - host_->value("focal", 500.0f), // Fy - host_->value("centre_x", -color_size_.width/2.0f), // Cx - host_->value("centre_y", -color_size_.height/2.0f), // Cy - (unsigned int) color_size_.width, - (unsigned int) color_size_.height, - min_depth, - max_depth, - baseline, - doff - }; - } - + + K = calib_->getCameraMatrixLeft(color_size_); + state_.getLeft() = { + static_cast<float>(K.at<double>(0,0)), // Fx + static_cast<float>(K.at<double>(1,1)), // Fy + static_cast<float>(-K.at<double>(0,2)), // Cx + static_cast<float>(-K.at<double>(1,2)), // Cy + (unsigned int) color_size_.width, + (unsigned int) color_size_.height, + min_depth, + max_depth, + baseline, + doff + }; + + host_->getConfig()["focal"] = params_.fx; + host_->getConfig()["centre_x"] = params_.cx; + host_->getConfig()["centre_y"] = params_.cy; + host_->getConfig()["baseline"] = params_.baseline; + host_->getConfig()["doffs"] = params_.doffs; + // right K = calib_->getCameraMatrixRight(color_size_); diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp index 2dfe96c3b789a47b5eb11f71ae554772889130b1..4b6b60e63a522c8031f5cc386fe6211eba064b9f 100644 --- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp +++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp @@ -38,7 +38,6 @@ class StereoVideoSource : public detail::Source { LocalSource *lsrc_; Calibrate *calib_; - bool calibrated_; cv::Size color_size_; cv::Size depth_size_;