diff --git a/cv-node/include/ftl/calibrate.hpp b/cv-node/include/ftl/calibrate.hpp
index e8d450828f4836afd821de910f21a5da1e98e192..6ddf9a4793c1f8e83dc72f95e7862c23cb069044 100644
--- a/cv-node/include/ftl/calibrate.hpp
+++ b/cv-node/include/ftl/calibrate.hpp
@@ -76,7 +76,8 @@ class Calibrate {
private:
bool runCalibration(cv::Mat &img, cv::Mat &cam);
- bool _recalibrate(size_t cam);
+ bool _recalibrate(size_t cam, std::vector<std::vector<cv::Point2f>> &imagePoints,
+ cv::Mat &cameraMatrix, cv::Mat &distCoeffs, cv::Size &imageSize);
cv::Mat _nextImage(size_t cam);
private:
diff --git a/cv-node/include/ftl/local.hpp b/cv-node/include/ftl/local.hpp
index 3ab86a2fb132c4dfd02aa3537b4d142cbcec147e..13196dd9e3231661b9f42525d24ab5051c411a68 100644
--- a/cv-node/include/ftl/local.hpp
+++ b/cv-node/include/ftl/local.hpp
@@ -11,8 +11,8 @@ namespace cv {
namespace ftl {
class LocalSource {
public:
- LocalSource();
- LocalSource(const std::string &vid);
+ LocalSource(bool flip=false);
+ LocalSource(const std::string &vid, bool flip=false);
bool left(cv::Mat &m);
bool right(cv::Mat &m);
@@ -29,6 +29,7 @@ class LocalSource {
double timestamp_;
bool stereo_;
//float fps_;
+ bool flip_;
cv::VideoCapture *camera_a_;
cv::VideoCapture *camera_b_;
};
diff --git a/cv-node/src/calibrate.cpp b/cv-node/src/calibrate.cpp
index 16f71b4031e746dd4ce8d1026504994e6b637166..c54eecf944cb60756197ade38ca40260c8b7792b 100644
--- a/cv-node/src/calibrate.cpp
+++ b/cv-node/src/calibrate.cpp
@@ -186,28 +186,100 @@ Calibrate::Calibrate(ftl::LocalSource *s, const std::string &cal) : local_(s) {
map1_.resize(2);
map2_.resize(2);
+ // TODO Load existing calibration if available...
+
calibrated_ = false;
}
bool Calibrate::recalibrate() {
- bool r = _recalibrate(0);
- if (local_->isStereo()) r &= _recalibrate(1);
+ vector<vector<Point2f> > imagePoints[2];
+ Mat cameraMatrix[2], distCoeffs[2];
+ Size imageSize[2];
+
+ bool r = _recalibrate(0, imagePoints[0], cameraMatrix[0], distCoeffs[0], imageSize[0]);
+ if (local_->isStereo()) r &= _recalibrate(1, imagePoints[1], cameraMatrix[1], distCoeffs[1], imageSize[1]);
if (r) calibrated_ = true;
+ if (r && local_->isStereo()) {
+ int nimages = static_cast<int>(imagePoints[0].size());
+ auto squareSize = settings_.squareSize;
+ vector<vector<Point3f>> objectPoints;
+ objectPoints.resize(nimages);
+
+ for(auto i = 0; i < nimages; i++ )
+ {
+ for(auto j = 0; j < settings_.boardSize.height; j++ )
+ for(auto k = 0; k < settings_.boardSize.width; k++ )
+ objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
+ }
+
+ Mat R, T, E, F;
+
+ LOG(INFO) << "Running stereo calibration...";
+
+ double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
+ cameraMatrix[0], distCoeffs[0],
+ cameraMatrix[1], distCoeffs[1],
+ imageSize[0], R, T, E, F,
+ CALIB_FIX_ASPECT_RATIO +
+ CALIB_ZERO_TANGENT_DIST +
+ CALIB_USE_INTRINSIC_GUESS +
+ CALIB_SAME_FOCAL_LENGTH +
+ CALIB_RATIONAL_MODEL +
+ CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
+ TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );
+ LOG(INFO) << "... done with RMS error=" << rms;
+
+ // save intrinsic parameters
+ FileStorage fs(FTL_CONFIG_ROOT "/intrinsics.yml", FileStorage::WRITE);
+ if( fs.isOpened() )
+ {
+ fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
+ "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
+ fs.release();
+ }
+ else
+ cout << "Error: can not save the intrinsic parameters\n";
+
+ Mat R1, R2, P1, P2, Q;
+ Rect validRoi[2];
+
+ stereoRectify(cameraMatrix[0], distCoeffs[0],
+ cameraMatrix[1], distCoeffs[1],
+ imageSize[0], R, T, R1, R2, P1, P2, Q,
+ CALIB_ZERO_DISPARITY, 1, imageSize[0], &validRoi[0], &validRoi[1]);
+
+ fs.open(FTL_CONFIG_ROOT "/extrinsics.yml", FileStorage::WRITE);
+ if( fs.isOpened() )
+ {
+ fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q;
+ fs.release();
+ }
+ else
+ cout << "Error: can not save the extrinsic parameters\n";
+
+
+ //Precompute maps for cv::remap()
+ initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize[0], CV_16SC2, map1_[0], map2_[0]);
+ initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize[0], CV_16SC2, map1_[1], map2_[1]);
+
+ }
+
return r;
}
-bool Calibrate::_recalibrate(size_t cam) {
+bool Calibrate::_recalibrate(size_t cam, vector<vector<Point2f>> &imagePoints,
+ Mat &cameraMatrix, Mat &distCoeffs, Size &imageSize) {
// TODO WHAT IS WINSIZE!!
int winSize = 11; //parser.get<int>("winSize");
float grid_width = settings_.squareSize * (settings_.boardSize.width - 1);
bool release_object = false;
- vector<vector<Point2f> > imagePoints;
- Mat cameraMatrix, distCoeffs;
- Size imageSize;
+ //vector<vector<Point2f> > imagePoints;
+ //Mat cameraMatrix, distCoeffs;
+ //Size imageSize;
int mode = CAPTURING;
clock_t prevTimestamp = 0;
const Scalar RED(0,0,255), GREEN(0,255,0);
@@ -369,6 +441,7 @@ bool Calibrate::_recalibrate(size_t cam) {
getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, imageSize, 1, imageSize, 0), imageSize,
CV_16SC2, map1_[cam], map2_[cam]);
}
+
return true;
}
diff --git a/cv-node/src/local.cpp b/cv-node/src/local.cpp
index 677c077295f373e2759734396e6bfc3e6aa91c90..075e19f394634cbe5c36de6dfc28cf0f18b7d752 100644
--- a/cv-node/src/local.cpp
+++ b/cv-node/src/local.cpp
@@ -13,10 +13,10 @@ using cv::Rect;
using std::string;
using namespace std::chrono;
-LocalSource::LocalSource() : timestamp_(0.0) {
+LocalSource::LocalSource(bool flip) : timestamp_(0.0), flip_(flip) {
// Use cameras
- camera_a_ = new VideoCapture(0);
- camera_b_ = new VideoCapture(1);
+ camera_a_ = new VideoCapture((flip) ? 1 : 0);
+ camera_b_ = new VideoCapture((flip) ? 0 : 1);
if (!camera_a_->isOpened()) {
delete camera_a_;
@@ -37,7 +37,7 @@ LocalSource::LocalSource() : timestamp_(0.0) {
}
}
-LocalSource::LocalSource(const string &vid): timestamp_(0.0) {
+LocalSource::LocalSource(const string &vid, bool flip): timestamp_(0.0), flip_(flip) {
if (vid == "") {
LOG(FATAL) << "No video file specified";
camera_a_ = nullptr;
@@ -93,7 +93,11 @@ bool LocalSource::left(cv::Mat &l) {
}
int resx = frame.cols / 2;
- l = Mat(frame, Rect(0,0,resx,frame.rows));
+ if (flip_) {
+ l = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ } else {
+ l = Mat(frame, Rect(0,0,resx,frame.rows));
+ }
}
return true;
@@ -127,7 +131,11 @@ bool LocalSource::right(cv::Mat &r) {
}
int resx = frame.cols / 2;
- r = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ if (flip_) {
+ r = Mat(frame, Rect(0,0,resx,frame.rows));
+ } else {
+ r = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ }
}
return true;
@@ -166,8 +174,13 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
}
int resx = frame.cols / 2;
- l = Mat(frame, Rect(0,0,resx,frame.rows));
- r = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ if (flip_) {
+ r = Mat(frame, Rect(0,0,resx,frame.rows));
+ l = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ } else {
+ l = Mat(frame, Rect(0,0,resx,frame.rows));
+ r = Mat(frame, Rect(resx,0,frame.cols-resx,frame.rows));
+ }
}
return true;
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index 82961d6c223cf970e4e895be719991df33b0562c..cc3e21921964767d3e4f3f02334f12f9cb4769e9 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -17,6 +17,7 @@ static string OPTION_calibration_config = FTL_CONFIG_ROOT "/calibration.xml";
static string OPTION_config;
static bool OPTION_display = false;
static bool OPTION_calibrate = false;
+static bool OPTION_flip = false;
void handle_options(char ***argv, int *argc) {
while (*argc > 0) {
@@ -39,6 +40,8 @@ void handle_options(char ***argv, int *argc) {
OPTION_config = cmd;
} else if (cmd.find("--display") == 0) {
OPTION_display = true;
+ } else if (cmd.find("--flip") == 0) {
+ OPTION_flip = true;
}
(*argc)--;
@@ -59,10 +62,10 @@ int main(int argc, char **argv) {
if (argc) {
// Load video file
- lsrc = new LocalSource(argv[0]);
+ lsrc = new LocalSource(argv[0], OPTION_flip);
} else {
// Use cameras
- lsrc = new LocalSource();
+ lsrc = new LocalSource(OPTION_flip);
}
auto sync = new SyncSource(); // TODO Pass protocol object