diff --git a/cv-node/config/config.json b/cv-node/config/config.json
index 9a768db7d2359bb35a592c0704a3d7cf54dcae6c..257a38777bbc8e3e5a5e528cb37497712316fa04 100644
--- a/cv-node/config/config.json
+++ b/cv-node/config/config.json
@@ -1,7 +1,8 @@
{
"source": {
"flip": false,
- "nostereo": false
+ "nostereo": false,
+ "scale": 1.0
},
"calibrate": false,
"calibration": {
@@ -24,7 +25,7 @@
"flip_vertical": false
},
"camera": {
- "name": "Pansonic Lumix DMC-FZ300",
+ "name": "Panasonic Lumix DMC-FZ300",
"focal_length": 25,
"sensor_width": 6.17,
"base_line": 0.1
diff --git a/cv-node/include/ftl/calibrate.hpp b/cv-node/include/ftl/calibrate.hpp
index 1982b4dc192816d6f2e4ac8d8f5df2eabd371886..2539864f0b2a6a87c6a9601b3b1d6585be3d14fc 100644
--- a/cv-node/include/ftl/calibrate.hpp
+++ b/cv-node/include/ftl/calibrate.hpp
@@ -75,6 +75,8 @@ class Calibrate {
bool isCalibrated();
+ const cv::Mat &getQ() const { return Q_; }
+
private:
bool _recalibrate(std::vector<std::vector<cv::Point2f>> *imagePoints,
cv::Mat *cameraMatrix, cv::Mat *distCoeffs, cv::Size *imageSize);
@@ -87,6 +89,7 @@ class Calibrate {
bool calibrated_;
std::vector<cv::Mat> map1_;
std::vector<cv::Mat> map2_;
+ cv::Mat Q_;
};
};
diff --git a/cv-node/include/ftl/local.hpp b/cv-node/include/ftl/local.hpp
index 6209eb24b07bb3d61490e2e061757d87822e5603..f4c98f357f8da25c2cfac8d9058bf99362db2ab3 100644
--- a/cv-node/include/ftl/local.hpp
+++ b/cv-node/include/ftl/local.hpp
@@ -32,6 +32,7 @@ class LocalSource {
//float fps_;
bool flip_;
bool nostereo_;
+ float downsize_;
cv::VideoCapture *camera_a_;
cv::VideoCapture *camera_b_;
};
diff --git a/cv-node/src/algorithms/rtcensus.cpp b/cv-node/src/algorithms/rtcensus.cpp
index 86b7768588caef401372a0203a722361562188ab..23939ed0021d5f4009da4db3be805c39e7be11dd 100644
--- a/cv-node/src/algorithms/rtcensus.cpp
+++ b/cv-node/src/algorithms/rtcensus.cpp
@@ -134,7 +134,7 @@ static cv::Mat d_sub(vector<uint16_t> &dsi, size_t w, size_t h, size_t ds) {
static cv::Mat consistency(cv::Mat &d_sub_r, cv::Mat &d_sub_l) {
size_t w = d_sub_r.cols;
size_t h = d_sub_r.rows;
- Mat result = Mat::zeros(Size(w,h), CV_64FC1);
+ Mat result = Mat::zeros(Size(w,h), CV_32FC1);
for (size_t v=0; v<h; v++) {
for (size_t u=0; u<w; u++) {
@@ -143,8 +143,8 @@ static cv::Mat consistency(cv::Mat &d_sub_r, cv::Mat &d_sub_l) {
auto b = d_sub_r.at<double>(v,u-a);
- if (std::abs(a-b) <= 1.0) result.at<double>(v,u) = std::abs((a+b)/2);
- else result.at<double>(v,u) = 0.0;
+ if (std::abs(a-b) <= 1.0) result.at<float>(v,u) = std::abs((a+b)/2);
+ else result.at<float>(v,u) = 0.0f;
}
}
diff --git a/cv-node/src/calibrate.cpp b/cv-node/src/calibrate.cpp
index ef2e6fcb7dcf78220a54c80c1aac257de51b42fc..ca5bea249353b1028b94b8732128c70bfe876534 100644
--- a/cv-node/src/calibrate.cpp
+++ b/cv-node/src/calibrate.cpp
@@ -203,7 +203,7 @@ bool Calibrate::_loadCalibration() {
float scale = 1.0f;
Rect roi1, roi2;
- Mat Q;
+
// reading intrinsic parameters
FileStorage fs(FTL_CONFIG_ROOT "/intrinsics.yml", FileStorage::READ);
if(!fs.isOpened())
@@ -231,8 +231,13 @@ bool Calibrate::_loadCalibration() {
Mat R, T, R1, P1, R2, P2;
fs["R"] >> R;
fs["T"] >> T;
+ fs["R1"] >> R1;
+ fs["R2"] >> R2;
+ fs["P1"] >> P1;
+ fs["P2"] >> P2;
+ fs["Q"] >> Q_;
- stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );
+ stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q_, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );
Mat map11, map12, map21, map22;
initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map1_[0], map2_[0]);
@@ -293,18 +298,18 @@ bool Calibrate::recalibrate() {
else
LOG(ERROR) << "Error: can not save the intrinsic parameters";
- Mat R1, R2, P1, P2, Q;
+ Mat R1, R2, P1, P2;
Rect validRoi[2];
stereoRectify(cameraMatrix[0], distCoeffs[0],
cameraMatrix[1], distCoeffs[1],
- imageSize[0], R, T, R1, R2, P1, P2, Q,
+ 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 << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q_;
fs.release();
}
else
diff --git a/cv-node/src/local.cpp b/cv-node/src/local.cpp
index 13eddcf32d901677bbfe4a128309886ce481d156..4342d9581fde66909fb9c1105d645d0d037c03d4 100644
--- a/cv-node/src/local.cpp
+++ b/cv-node/src/local.cpp
@@ -14,7 +14,10 @@ using std::string;
using namespace std::chrono;
LocalSource::LocalSource(nlohmann::json &config)
- : timestamp_(0.0), flip_(config["flip"]), nostereo_(config["nostereo"]) {
+ : timestamp_(0.0),
+ flip_(config["flip"]),
+ nostereo_(config["nostereo"]),
+ downsize_(config.value("scale",1.0f)) {
// Use cameras
camera_a_ = new VideoCapture((flip_) ? 1 : 0);
if (!nostereo_) camera_b_ = new VideoCapture((flip_) ? 0 : 1);
@@ -40,7 +43,10 @@ LocalSource::LocalSource(nlohmann::json &config)
}
LocalSource::LocalSource(const string &vid, nlohmann::json &config)
- : timestamp_(0.0), flip_(config["flip"]), nostereo_(config["nostereo"]) {
+ : timestamp_(0.0),
+ flip_(config["flip"]),
+ nostereo_(config["nostereo"]),
+ downsize_(config.value("scale",1.0f)) {
if (vid == "") {
LOG(FATAL) << "No video file specified";
camera_a_ = nullptr;
@@ -186,6 +192,11 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
}
}
+ if (downsize_ != 1.0f) {
+ cv::resize(l, l, cv::Size(l.cols * downsize_,l.rows * downsize_), 0, 0, cv::INTER_LINEAR);
+ cv::resize(r, r, cv::Size(r.cols * downsize_,r.rows * downsize_), 0, 0, cv::INTER_LINEAR);
+ }
+
return true;
}
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index c4c804dc391bfff7cb8daf1e388343a3128051ca..afa9be6571abb2da57fbf75cd750b5973ef4ad60 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -121,9 +121,14 @@ int main(int argc, char **argv) {
// Choose and configure disparity algorithm
auto disparity = Disparity::create(config["disparity"]);
- Mat l, r, filtered_disp;
+ Mat l, r, disparity32F, lbw, rbw;
- while (true) {
+ cv::viz::Viz3d myWindow("FTL");
+
+ float fact = (float)(config["camera"]["focal_length"]) / (float)(config["camera"]["sensor_width"]);
+ Mat rot_vec = Mat::zeros(1,3,CV_32F);
+
+ while (!myWindow.wasStopped()) {
// Read calibrated images.
calibrate.undistort(l,r);
@@ -135,29 +140,57 @@ int main(int argc, char **argv) {
sync->get(LEFT,l);
sync->get(RIGHT,r);
- // Downscale
- //cv::resize(l, l, cv::Size(l.cols * 0.75,l.rows * 0.75), 0, 0, INTER_LINEAR);
- //cv::resize(r, r, cv::Size(r.cols * 0.75,r.rows * 0.75), 0, 0, INTER_LINEAR);
-
// Black and white
- cvtColor(l, l, COLOR_BGR2GRAY);
- cvtColor(r, r, COLOR_BGR2GRAY);
+ cvtColor(l, lbw, COLOR_BGR2GRAY);
+ cvtColor(r, rbw, COLOR_BGR2GRAY);
- disparity->compute(l,r,filtered_disp);
+ disparity->compute(lbw,rbw,disparity32F);
LOG(INFO) << "Disparity complete ";
+ disparity32F.convertTo(disparity32F, CV_32F);
+
// TODO Send RGB+D data somewhere
- //left_disp = (fact * (double)l.cols * 0.1) / filtered_disp;
+ // Convert disparity to depth
+ disparity32F = (fact * (float)l.cols * 0.1f) / disparity32F;
- normalize(filtered_disp, filtered_disp, 0, 255, NORM_MINMAX, CV_8U);
+ //normalize(filtered_disp, filtered_disp, 0, 255, NORM_MINMAX, CV_8U);
+
+ //cv::imshow("Disparity",filtered_disp);
+ //Mat i3d;
+ //const Mat &Q = calibrate.getQ();
+ cv::Mat Q_32F;
+ calibrate.getQ().convertTo(Q_32F,CV_32F);
+ cv::Mat_<cv::Vec3f> XYZ(disparity32F.rows,disparity32F.cols); // Output point cloud
+ reprojectImageTo3D(disparity32F, XYZ, Q_32F, true);
- cv::imshow("Disparity",filtered_disp);
+ //cv::imshow("Points",XYZ);
+
+ cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, l );
+ cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
+
+ /* Rotation using rodrigues */
+ /// Rotate around (1,1,1)
+ rot_vec.at<float>(0,0) = 0.0f;
+ rot_vec.at<float>(0,1) = 0.0f;
+ rot_vec.at<float>(0,2) = CV_PI * 1.0f;
+
+ Mat rot_mat;
+ Rodrigues(rot_vec, rot_mat);
+
+ /// Construct pose
+ Affine3f pose(rot_mat, Vec3f(0, 0, 0));
+
+ //myWindow.showWidget( "coosys", viz::WCoordinateSystem() );
+ myWindow.showWidget( "Depth", cloud_widget );
+ myWindow.setWidgetPose("Depth", pose);
+
+ myWindow.spinOnce( 30, true );
- if(cv::waitKey(10) == 27){
+ //if(cv::waitKey(10) == 27){
//exit if ESC is pressed
- break;
- }
+ // break;
+ //}
}
}