diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 4c3bf601a8d2f8548adee0544438e0bf250a0f5d..6b053a2e686f36693295c847420318ebaf241299 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,15 +1,26 @@
-image: ubuntu:18.04
-
-before_script:
- - export DEBIAN_FRONTEND=noninteractive
- - apt-get update -qq && apt-get install -y -qq g++ cmake git
- - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev
+windows job:
+ tags:
+ - win
+ script:
+ - 'call "C:/Program Files (x86)/Microsoft Visual Studio/2017/Community/VC/Auxiliary/Build/vcvars64.bat"'
+ - mkdir build
+ - cd build
+ - 'cmake -DWITH_PCL=FALSE -DCMAKE_GENERATOR_PLATFORM=x64 -DOpenCV_DIR="D:/opencv-4.0.1/build/install" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1" ..'
+ - devenv ftl.utu.fi.sln /build Release
+
+linux job:
+ tags:
+ - docker
-ftl:
+ image: ubuntu:18.04
+
+ before_script:
+ - export DEBIAN_FRONTEND=noninteractive
+ - apt-get update -qq && apt-get install -y -qq g++ cmake git
+ - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev libpcl-dev
script:
- mkdir build
- cd build
- cmake ..
- make
- - make test
-
+ - ctest --output-on-failure
diff --git a/CMakeLists.txt b/CMakeLists.txt
index dc83c57d6b51db9789afabb25f755820dcaac9d4..6eaf7b6d5304fca33e7009e98e14653de458d535 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,17 +11,40 @@ include(GNUInstallDirs)
include(CTest)
enable_testing()
+option(WITH_PCL "Use PCL if available" ON)
+option(WITH_FIXSTARS "Use Fixstars libSGM if available" ON)
+option(BUILD_VISION "Enable the vision component" ON)
+option(BUILD_RECONSTRUCT "Enable the reconstruction component" ON)
+option(BUILD_RENDERER "Enable the renderer component" ON)
+
set(THREADS_PREFER_PTHREAD_FLAG ON)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
include(Findglog)
-#find_package( glog REQUIRED )
+
find_package( OpenCV REQUIRED )
find_package( Threads REQUIRED )
find_package( URIParser REQUIRED )
find_package( MsgPack REQUIRED )
-find_package( LibSGM )
-#find_package( ZLIB REQUIRED )
+
+if (WITH_FIXSTARS)
+ find_package( LibSGM )
+endif()
+
+if(${CMAKE_VERSION} VERSION_GREATER "3.12.0")
+ cmake_policy(SET CMP0074 NEW)
+endif()
+
+if (WITH_PCL)
+ find_package( PCL QUIET COMPONENTS io common visualization registration segmentation)
+endif()
+
+set(CMAKE_CXX_STANDARD 17) # For PCL/VTK https://github.com/PointCloudLibrary/pcl/issues/2686
+set(HAVE_OPENCV TRUE)
+
+if (PCL_FOUND)
+ set(HAVE_PCL TRUE)
+endif()
# Readline library is not required on Windows
# May also entirely remove dependence on this... it should be optional at least.
@@ -93,7 +116,7 @@ check_include_file_cxx("opencv2/cudastereo.hpp" HAVE_OPENCVCUDA)
find_program(CPPCHECK_FOUND cppcheck)
if (CPPCHECK_FOUND)
message(STATUS "Found cppcheck: will perform source checks")
- set(CMAKE_CXX_CPPCHECK "cppcheck" "--enable=style" "--suppress=*:*catch.hpp" "--suppress=*:*json.hpp")
+ set(CMAKE_CXX_CPPCHECK "cppcheck" "--enable=style" "--suppress=*:*catch.hpp" "--suppress=*:*elas*" "--suppress=*:*json.hpp")
endif()
include_directories(${PROJECT_SOURCE_DIR}/common/cpp/include)
@@ -116,10 +139,20 @@ endif()
SET(CMAKE_USE_RELATIVE_PATHS ON)
add_subdirectory(common/cpp)
-add_subdirectory(renderer)
add_subdirectory(net)
-add_subdirectory(vision)
-add_subdirectory(reconstruct)
+
+if (BUILD_RENDERER)
+ add_subdirectory(renderer)
+ set(HAVE_RENDER)
+endif()
+
+if (BUILD_VISION)
+ add_subdirectory(vision)
+endif()
+
+if (BUILD_RECONSTRUCT)
+ add_subdirectory(reconstruct)
+endif()
### Generate Build Configuration Files =========================================
@@ -131,3 +164,13 @@ configure_file(${CMAKE_SOURCE_DIR}/common/cpp/src/config.cpp.in
${CMAKE_SOURCE_DIR}/common/cpp/src/config.cpp
)
+# For issue #17
+# https://gitlab.kitware.com/cmake/cmake/issues/16915#note_456382
+if ( TARGET Qt5::Core )
+ get_property( core_options TARGET Qt5::Core PROPERTY INTERFACE_COMPILE_OPTIONS )
+ string( REPLACE "-fPIC" "" new_core_options "${core_options}" )
+ set_property( TARGET Qt5::Core PROPERTY INTERFACE_COMPILE_OPTIONS ${new_core_options} )
+ set_property( TARGET Qt5::Core PROPERTY INTERFACE_POSITION_INDEPENDENT_CODE "ON" )
+ set( CMAKE_CXX_COMPILE_OPTIONS_PIE "-fPIC" )
+endif()
+
diff --git a/common/config/config.json b/common/config/config.json
index 4c0ec32253deeac8ac77c89a1f6659f8f437ee2d..186f3821754d22a57367c832415d18263810d622 100644
--- a/common/config/config.json
+++ b/common/config/config.json
@@ -10,7 +10,10 @@
"nostereo": false,
"scale": 1.0,
"flip_vert": false,
- "max_fps": 25
+ "max_fps": 25,
+ "width": 640,
+ "height": 480,
+ "crosshair": false
},
"calibrate": false,
"calibration": {
@@ -55,7 +58,8 @@
"points": true,
"depth": false,
"left": false,
- "right": false
+ "right": false,
+ "crosshair": false
},
"net": {
"listen": "tcp://*:9001",
@@ -69,7 +73,7 @@
"net": {
"peers": ["tcp://localhost:9001"]
},
- "source": "ftl://utu.fi/dummy/rgb-d",
+ "sources": ["ftl://utu.fi/node1/rgb-d"],
"display": {
"flip_vert": false,
"disparity": false,
@@ -77,6 +81,15 @@
"depth": false,
"left": false,
"right": false
+ },
+ "registration": {
+ "reference-source" : "ftl://utu.fi/node1/rgb-d",
+ "calibration" : {
+ "run": false,
+ "iterations" : 10,
+ "delay" : 1000,
+ "patternsize" : [9, 6]
+ }
}
}
}
diff --git a/common/cpp/CMakeLists.txt b/common/cpp/CMakeLists.txt
index 2a9be0f3b3f32963d088d6af06809db57281d3b7..c8248564d8629021ce24b40f7952a20a4d7337bc 100644
--- a/common/cpp/CMakeLists.txt
+++ b/common/cpp/CMakeLists.txt
@@ -1,13 +1,15 @@
set(COMMONSRC
src/config.cpp
src/configuration.cpp
+ src/opencv_to_pcl.cpp
)
add_library(ftlcommon ${COMMONSRC})
target_include_directories(ftlcommon PUBLIC
+ ${PCL_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlcommon glog::glog)
+target_link_libraries(ftlcommon glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
diff --git a/common/cpp/include/ftl/config.h.in b/common/cpp/include/ftl/config.h.in
index b03e4eeeff946c3e8c282500dd4717fa11fd3539..ca51cebb894ed858580b823201f859aaa5e1f675 100644
--- a/common/cpp/include/ftl/config.h.in
+++ b/common/cpp/include/ftl/config.h.in
@@ -5,6 +5,9 @@
#cmakedefine HAVE_OPENCVCUDA
#cmakedefine HAVE_URIPARSESINGLE
#cmakedefine HAVE_CUDA
+#cmakedefine HAVE_OPENCV
+#cmakedefine HAVE_PCL
+#cmakedefine HAVE_RENDER
extern const char *FTL_VERSION_LONG;
extern const char *FTL_VERSION;
diff --git a/common/cpp/include/ftl/utility/opencv_to_pcl.hpp b/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f273d5e23e460bc70b4fb35ecfd16ce97308ffef
--- /dev/null
+++ b/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
@@ -0,0 +1,22 @@
+#ifndef _FTL_COMMON_OPENCV_TO_PCL_HPP_
+#define _FTL_COMMON_OPENCV_TO_PCL_HPP_
+
+#include <ftl/config.h>
+#include <opencv2/opencv.hpp>
+
+#if defined HAVE_PCL
+#include <pcl/common/common_headers.h>
+
+namespace ftl {
+namespace utility {
+
+/**
+ * Convert an OpenCV point cloud matrix and RGB image to a PCL XYZRGB point cloud.
+ */
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage);
+
+};
+};
+#endif // HAVE_PCL
+
+#endif // _FTL_COMMON_OPENCV_TO_PCL_HPP_
diff --git a/common/cpp/src/configuration.cpp b/common/cpp/src/configuration.cpp
index d42d487e9cbb7db6d3351d57a74d42c1342dc4dd..a2674dcb08d101b73a4835dde91d31b97f6865e8 100644
--- a/common/cpp/src/configuration.cpp
+++ b/common/cpp/src/configuration.cpp
@@ -129,20 +129,36 @@ static bool mergeConfig(const string &path) {
*/
static bool findConfiguration(const string &file, const vector<string> &paths,
const std::string &app) {
+ bool f = false;
bool found = false;
- found |= mergeConfig(FTL_GLOBAL_CONFIG_ROOT "/config.json");
- found |= mergeConfig(FTL_LOCAL_CONFIG_ROOT "/config.json");
- found |= mergeConfig("./config.json");
+ f = mergeConfig(FTL_GLOBAL_CONFIG_ROOT "/config.json");
+ found |= f;
+ if (f) LOG(INFO) << "Loaded config: " << FTL_GLOBAL_CONFIG_ROOT "/config.json";
+ f = mergeConfig(FTL_LOCAL_CONFIG_ROOT "/config.json");
+ found |= f;
+ if (f) LOG(INFO) << "Loaded config: " << FTL_LOCAL_CONFIG_ROOT "/config.json";
+ f = mergeConfig("./config.json");
+ found |= f;
+ if (f) LOG(INFO) << "Loaded config: " << "./config.json";
for (auto p : paths) {
if (is_directory(p)) {
- found |= mergeConfig(p+"/config.json");
+ f = mergeConfig(p+"/config.json");
+ found |= f;
+ if (f) LOG(INFO) << "Loaded config: " << p << "/config.json";
}
}
if (file != "") {
- found |= mergeConfig(file);
+ f = mergeConfig(file);
+ found |= f;
+
+ if (!f) {
+ LOG(ERROR) << "Specific config file (" << file << ") was not found";
+ } else {
+ LOG(INFO) << "Loaded config: " << file;
+ }
}
if (found) {
diff --git a/common/cpp/src/opencv_to_pcl.cpp b/common/cpp/src/opencv_to_pcl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7fb1eabe0e762677b46df8a91e029f7867e8dd1a
--- /dev/null
+++ b/common/cpp/src/opencv_to_pcl.cpp
@@ -0,0 +1,35 @@
+#include <ftl/utility/opencv_to_pcl.hpp>
+
+#if defined HAVE_PCL
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr ftl::utility::matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage) {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
+ point_cloud_ptr->width = cvcloud.cols * cvcloud.rows;
+ point_cloud_ptr->height = 1;
+
+ for(int i=0;i<cvcloud.rows;i++) {
+ for(int j=0;j<cvcloud.cols;j++) {
+ //std::cout<<i<<endl;
+
+ pcl::PointXYZRGB point;
+ cv::Vec3f cvpoint = cvcloud.at<cv::Vec3f>(i,j);
+ point.x = cvpoint[0];
+ point.y = cvpoint[1];
+ point.z = cvpoint[2];
+
+ if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 600.0f) {
+ point.x = 0; point.y = 0; point.z = 0;
+ }
+
+ cv::Point3_<uchar> prgb = rgbimage.at<cv::Point3_<uchar>>(i, j);
+
+ // when color needs to be added:
+ uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
+ point.rgb = *reinterpret_cast<float*>(&rgb);
+
+ point_cloud_ptr -> points.push_back(point);
+ }
+ }
+
+ return point_cloud_ptr;
+}
+#endif // HAVE_PCL
\ No newline at end of file
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..becacbf57317bc96a19d2d4904f0107160ff3324
--- /dev/null
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -0,0 +1,614 @@
+/*
+ * Copyright 2019 Nicolas Pope
+ */
+
+#include <glog/logging.h>
+#include <ftl/config.h>
+#include <ftl/configuration.hpp>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <ctime>
+#include <cstdio>
+
+#include "calibrate.hpp"
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/videoio.hpp>
+#include <opencv2/highgui.hpp>
+
+using ftl::Calibrate;
+using cv::FileStorage;
+using cv::CALIB_FIX_PRINCIPAL_POINT;
+using cv::CALIB_ZERO_TANGENT_DIST;
+using cv::CALIB_FIX_ASPECT_RATIO;
+using cv::CALIB_FIX_K1;
+using cv::CALIB_FIX_K2;
+using cv::CALIB_FIX_K3;
+using cv::CALIB_FIX_K4;
+using cv::CALIB_FIX_K5;
+using cv::CALIB_ZERO_DISPARITY;
+using cv::CALIB_USE_INTRINSIC_GUESS;
+using cv::CALIB_SAME_FOCAL_LENGTH;
+using cv::CALIB_RATIONAL_MODEL;
+using cv::CALIB_CB_ADAPTIVE_THRESH;
+using cv::CALIB_CB_NORMALIZE_IMAGE;
+using cv::CALIB_CB_FAST_CHECK;
+using cv::CALIB_USE_LU;
+using cv::NORM_L2;
+using cv::INTER_LINEAR;
+using cv::COLOR_BGR2GRAY;
+using cv::FileNode;
+using cv::FileNodeIterator;
+using cv::Mat;
+using cv::Size;
+using cv::Point2f;
+using cv::Point3f;
+using cv::Matx33d;
+using cv::Scalar;
+using cv::Rect;
+using cv::TermCriteria;
+using cv::waitKey;
+using std::string;
+using std::vector;
+
+void Calibrate::Settings::write(FileStorage& fs) const {
+ fs << "{"
+ << "BoardSize_Width" << boardSize.width
+ << "BoardSize_Height" << boardSize.height
+ << "Square_Size" << squareSize
+ << "Calibrate_Pattern" << patternToUse
+ << "Calibrate_NrOfFrameToUse" << nrFrames
+ << "Calibrate_FixAspectRatio" << aspectRatio
+ << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
+ << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint
+ << "Write_DetectedFeaturePoints" << writePoints
+ << "Write_extrinsicParameters" << writeExtrinsics
+ << "Write_gridPoints" << writeGrid
+ << "Input_FlipAroundHorizontalAxis" << flipVertical
+ << "Input_Delay" << delay
+ << "}";
+}
+
+void Calibrate::Settings::read(const nlohmann::json& node) {
+ boardSize.width = node["board_size"][0];
+ boardSize.height = node["board_size"][1];
+ squareSize = node["square_size"];
+ nrFrames = node["num_frames"];
+ aspectRatio = node["fix_aspect_ratio"];
+ calibZeroTangentDist = node["assume_zero_tangential_distortion"];
+ calibFixPrincipalPoint = node["fix_principal_point_at_center"];
+ useFisheye = node["use_fisheye_model"];
+ flipVertical = node["flip_vertical"];
+ delay = node["frame_delay"];
+ fixK1 = node["fix_k1"];
+ fixK2 = node["fix_k2"];
+ fixK3 = node["fix_k3"];
+ fixK4 = node["fix_k4"];
+ fixK5 = node["fix_k5"];
+
+ validate();
+}
+
+void Calibrate::Settings::validate() {
+ goodInput = true;
+ if (boardSize.width <= 0 || boardSize.height <= 0) {
+ LOG(ERROR) << "Invalid Board size: " << boardSize.width << " " <<
+ boardSize.height;
+ goodInput = false;
+ }
+
+ if (squareSize <= 10e-6) {
+ LOG(ERROR) << "Invalid square size " << squareSize;
+ goodInput = false;
+ }
+
+ if (nrFrames <= 0) {
+ LOG(ERROR) << "Invalid number of frames " << nrFrames;
+ goodInput = false;
+ }
+
+ flag = 0;
+ if (calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
+ if (calibZeroTangentDist) flag |= CALIB_ZERO_TANGENT_DIST;
+ if (aspectRatio) flag |= CALIB_FIX_ASPECT_RATIO;
+ if (fixK1) flag |= CALIB_FIX_K1;
+ if (fixK2) flag |= CALIB_FIX_K2;
+ if (fixK3) flag |= CALIB_FIX_K3;
+ if (fixK4) flag |= CALIB_FIX_K4;
+ if (fixK5) flag |= CALIB_FIX_K5;
+
+ if (useFisheye) {
+ // the fisheye model has its own enum, so overwrite the flags
+ flag = cv::fisheye::CALIB_FIX_SKEW |
+ cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC;
+ if (fixK1) flag |= cv::fisheye::CALIB_FIX_K1;
+ if (fixK2) flag |= cv::fisheye::CALIB_FIX_K2;
+ if (fixK3) flag |= cv::fisheye::CALIB_FIX_K3;
+ if (fixK4) flag |= cv::fisheye::CALIB_FIX_K4;
+ if (calibFixPrincipalPoint)
+ flag |= cv::fisheye::CALIB_FIX_PRINCIPAL_POINT;
+ }
+
+ atImageList = 0;
+}
+
+Mat Calibrate::_nextImage(size_t cam) {
+ Mat result;
+ if (cam == 0) {
+ local_->left(result);
+ } else if (cam == 1 && local_->isStereo()) {
+ local_->right(result);
+ }
+ return result;
+}
+
+bool Calibrate::Settings::readStringList(const string& filename,
+ vector<string>& l) {
+ l.clear();
+ FileStorage fs(filename, FileStorage::READ);
+ if ( !fs.isOpened() )
+ return false;
+ FileNode n = fs.getFirstTopLevelNode();
+ if ( n.type() != FileNode::SEQ )
+ return false;
+ FileNodeIterator it = n.begin(), it_end = n.end();
+ for ( ; it != it_end; ++it )
+ l.push_back((string)*it);
+ return true;
+}
+
+bool Calibrate::Settings::isListOfImages(const string& filename) {
+ string s(filename);
+ // Look for file extension
+ if ( s.find(".xml") == string::npos && s.find(".yaml") == string::npos &&
+ s.find(".yml") == string::npos )
+ return false;
+ else
+ return true;
+}
+
+enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
+
+bool runCalibration(const Calibrate::Settings& s, Size imageSize,
+ Mat& cameraMatrix, Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints, float grid_width,
+ bool release_object);
+
+
+Calibrate::Calibrate(ftl::LocalSource *s, nlohmann::json &config) : local_(s) {
+ /*FileStorage fs(cal, FileStorage::READ); // Read the settings
+ if (!fs.isOpened())
+ {
+ LOG(ERROR) << "Could not open the configuration file: \"" << cal << "\"";
+ return;
+ }*/
+ // fs["Settings"] >> settings_;
+ settings_.read(config);
+ // fs.release();
+
+ if (!settings_.goodInput) {
+ LOG(ERROR) << "Invalid input detected. Application stopping.";
+ return;
+ }
+
+ map1_.resize(2);
+ map2_.resize(2);
+
+ calibrated_ = _loadCalibration();
+
+ if (calibrated_) {
+ LOG(INFO) << "Calibration loaded from file";
+ }
+}
+
+bool Calibrate::_loadCalibration() {
+ // Capture one frame to get size;
+ Mat l, r;
+ local_->get(l, r);
+ Size img_size = l.size();
+ float scale = 1.0f;
+
+ Rect roi1, roi2;
+ FileStorage fs;
+
+ // reading intrinsic parameters
+ auto ifile = ftl::locateFile("intrinsics.yml");
+ if (ifile) {
+ fs.open((*ifile).c_str(), FileStorage::READ);
+ if (!fs.isOpened()) {
+ LOG(WARNING) << "Could not open intrinsics file";
+ return false;
+ }
+
+ LOG(INFO) << "Intrinsics from: " << *ifile;
+ } else {
+ LOG(WARNING) << "Calibration intrinsics file not found";
+ return false;
+ }
+
+ Mat M1, D1, M2, D2;
+ fs["M1"] >> M1;
+ fs["D1"] >> D1;
+ fs["M2"] >> M2;
+ fs["D2"] >> D2;
+
+ M1 *= scale;
+ M2 *= scale;
+
+ auto efile = ftl::locateFile("extrinsics.yml");
+ if (efile) {
+ fs.open((*efile).c_str(), FileStorage::READ);
+ if (!fs.isOpened()) {
+ LOG(WARNING) << "Could not open extrinsics file";
+ return false;
+ }
+
+ LOG(INFO) << "Extrinsics from: " << *efile;
+ } else {
+ LOG(WARNING) << "Calibration extrinsics file not found";
+ return false;
+ }
+
+ 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);
+
+ Mat map11, map12, map21, map22;
+ initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2,
+ map1_[0], map2_[0]);
+ initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2,
+ map1_[1], map2_[1]);
+ return true;
+}
+
+bool Calibrate::recalibrate() {
+ vector<vector<Point2f>> imagePoints[2];
+ Mat cameraMatrix[2], distCoeffs[2];
+ Size imageSize[2];
+
+ bool r = _recalibrate(imagePoints, cameraMatrix, distCoeffs, imageSize);
+
+ 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_LOCAL_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 {
+ LOG(ERROR) << "Error: can not save the intrinsic parameters";
+ }
+
+ 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_,
+ CALIB_ZERO_DISPARITY, 1, imageSize[0],
+ &validRoi[0], &validRoi[1]);
+
+ fs.open(FTL_LOCAL_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 {
+ LOG(ERROR) << "Error: can not save the extrinsic parameters";
+ }
+
+ // 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]);
+
+ } else {
+ int cam = 0;
+ if (settings_.useFisheye) {
+ Mat newCamMat;
+ cv::fisheye::estimateNewCameraMatrixForUndistortRectify(
+ cameraMatrix[cam], distCoeffs[cam], imageSize[cam],
+ Matx33d::eye(), newCamMat, 1);
+ cv::fisheye::initUndistortRectifyMap(
+ cameraMatrix[cam], distCoeffs[cam], Matx33d::eye(),
+ newCamMat, imageSize[cam],
+ CV_16SC2, map1_[cam], map2_[cam]);
+ } else {
+ initUndistortRectifyMap(
+ cameraMatrix[cam], distCoeffs[cam], Mat(),
+ getOptimalNewCameraMatrix(cameraMatrix[cam], distCoeffs[cam],
+ imageSize[cam], 1, imageSize[cam], 0),
+ imageSize[cam], CV_16SC2, map1_[cam], map2_[cam]);
+ }
+ }
+
+ return r;
+}
+
+bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
+ Mat *cameraMatrix, Mat *distCoeffs, Size *imageSize) {
+ int winSize = 11; // parser.get<int>("winSize");
+
+ float grid_width = settings_.squareSize * (settings_.boardSize.width - 1);
+ bool release_object = false;
+ double prevTimestamp = 0.0;
+ const Scalar RED(0, 0, 255), GREEN(0, 255, 0);
+
+ int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
+
+ if (!settings_.useFisheye) {
+ // fast check erroneously fails with high distortions like fisheye
+ chessBoardFlags |= CALIB_CB_FAST_CHECK;
+ }
+
+ for (;;) {
+ Mat view[2];
+ local_->get(view[0], view[1]);
+ LOG(INFO) << "Grabbing calibration image...";
+
+ if (view[0].empty() || (local_->isStereo() && view[1].empty()) ||
+ imagePoints[0].size() >= (size_t)settings_.nrFrames) {
+ settings_.outputFileName = FTL_LOCAL_CONFIG_ROOT "/cam0.xml";
+ bool r = runCalibration(settings_, imageSize[0],
+ cameraMatrix[0], distCoeffs[0], imagePoints[0],
+ grid_width, release_object);
+
+ if (local_->isStereo()) {
+ settings_.outputFileName = FTL_LOCAL_CONFIG_ROOT "/cam1.xml";
+ r &= runCalibration(settings_, imageSize[1],
+ cameraMatrix[1], distCoeffs[1], imagePoints[1],
+ grid_width, release_object);
+ }
+
+ if (!r && view[0].empty()) {
+ LOG(ERROR) << "Not enough frames to calibrate";
+ return false;
+ } else if (r) {
+ LOG(INFO) << "Calibration successful";
+ break;
+ }
+ }
+
+ imageSize[0] = view[0].size(); // Format input image.
+ imageSize[1] = view[1].size();
+ // if( settings_.flipVertical ) flip( view[cam], view[cam], 0 );
+
+ vector<Point2f> pointBuf[2];
+ bool found1, found2;
+ found1 = findChessboardCorners(view[0], settings_.boardSize,
+ pointBuf[0], chessBoardFlags);
+ found2 = !local_->isStereo() || findChessboardCorners(view[1],
+ settings_.boardSize, pointBuf[1], chessBoardFlags);
+
+ if (found1 && found2 &&
+ local_->getTimestamp()-prevTimestamp > settings_.delay) {
+ prevTimestamp = local_->getTimestamp();
+ // improve the found corners' coordinate accuracy for chessboard
+ Mat viewGray;
+ cvtColor(view[0], viewGray, COLOR_BGR2GRAY);
+ cornerSubPix(viewGray, pointBuf[0], Size(winSize, winSize),
+ Size(-1, -1), TermCriteria(
+ TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
+ imagePoints[0].push_back(pointBuf[0]);
+
+ if (local_->isStereo()) {
+ cvtColor(view[1], viewGray, COLOR_BGR2GRAY);
+ cornerSubPix(viewGray, pointBuf[1], Size(winSize, winSize),
+ Size(-1, -1), TermCriteria(
+ TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
+ imagePoints[1].push_back(pointBuf[1]);
+ }
+
+ // Draw the corners.
+ drawChessboardCorners(view[0], settings_.boardSize,
+ Mat(pointBuf[0]), found1);
+ if (local_->isStereo()) drawChessboardCorners(view[1],
+ settings_.boardSize, Mat(pointBuf[1]), found2);
+ } else {
+ LOG(WARNING) << "No calibration pattern found";
+ }
+
+ imshow("Left", view[0]);
+ if (local_->isStereo()) imshow("Right", view[1]);
+ char key = static_cast<char>(waitKey(50));
+
+ if (key == 27)
+ break;
+ }
+
+ return true;
+}
+
+bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
+ local_->get(l, r);
+ if (!calibrated_) return false;
+ if (l.empty()) return false;
+ remap(l, l, map1_[0], map2_[0], INTER_LINEAR);
+ if (local_->isStereo()) remap(r, r, map1_[1], map2_[1], INTER_LINEAR);
+ return true;
+}
+
+bool Calibrate::rectified(cv::Mat &l, cv::Mat &r) {
+ return undistort(l, r);
+}
+
+bool Calibrate::isCalibrated() {
+ return calibrated_;
+}
+
+//! [compute_errors]
+static double computeReprojectionErrors(
+ const vector<vector<Point3f> >& objectPoints,
+ const vector<vector<Point2f> >& imagePoints,
+ const vector<Mat>& rvecs, const vector<Mat>& tvecs,
+ const Mat& cameraMatrix , const Mat& distCoeffs,
+ vector<float>& perViewErrors, bool fisheye) {
+ vector<Point2f> imagePoints2;
+ size_t totalPoints = 0;
+ double totalErr = 0;
+ perViewErrors.resize(objectPoints.size());
+
+ for (size_t i = 0; i < objectPoints.size(); ++i) {
+ if (fisheye) {
+ cv::fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i],
+ tvecs[i], cameraMatrix, distCoeffs);
+ } else {
+ projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix,
+ distCoeffs, imagePoints2);
+ }
+ double err = norm(imagePoints[i], imagePoints2, NORM_L2);
+
+ size_t n = objectPoints[i].size();
+ perViewErrors[i] = static_cast<float>(std::sqrt(err*err/n));
+ totalErr += err*err;
+ totalPoints += n;
+ }
+
+ return std::sqrt(totalErr/totalPoints);
+}
+//! [compute_errors]
+//! [board_corners]
+static void calcBoardCornerPositions(Size boardSize, float squareSize,
+ vector<Point3f>& corners, Calibrate::Settings::Pattern patternType) {
+ corners.clear();
+
+ switch (patternType) {
+ case Calibrate::Settings::CHESSBOARD:
+ case Calibrate::Settings::CIRCLES_GRID:
+ for (int i = 0; i < boardSize.height; ++i)
+ for (int j = 0; j < boardSize.width; ++j)
+ corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
+ break;
+
+ case Calibrate::Settings::ASYMMETRIC_CIRCLES_GRID:
+ for (int i = 0; i < boardSize.height; i++)
+ for (int j = 0; j < boardSize.width; j++)
+ corners.push_back(Point3f((2*j + i % 2)*squareSize,
+ i*squareSize, 0));
+ break;
+ default:
+ break;
+ }
+}
+//! [board_corners]
+static bool _runCalibration(const Calibrate::Settings& s, Size& imageSize,
+ Mat& cameraMatrix, Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs,
+ vector<Mat>& tvecs, vector<float>& reprojErrs, double& totalAvgErr,
+ vector<Point3f>& newObjPoints, float grid_width, bool release_object) {
+ cameraMatrix = Mat::eye(3, 3, CV_64F);
+ if (s.flag & CALIB_FIX_ASPECT_RATIO)
+ cameraMatrix.at<double>(0, 0) = s.aspectRatio;
+
+ if (s.useFisheye) {
+ distCoeffs = Mat::zeros(4, 1, CV_64F);
+ } else {
+ distCoeffs = Mat::zeros(8, 1, CV_64F);
+ }
+
+ vector<vector<Point3f> > objectPoints(1);
+ calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0],
+ Calibrate::Settings::CHESSBOARD);
+ objectPoints[0][s.boardSize.width - 1].x = objectPoints[0][0].x +
+ grid_width;
+ newObjPoints = objectPoints[0];
+
+ objectPoints.resize(imagePoints.size(), objectPoints[0]);
+
+ // Find intrinsic and extrinsic camera parameters
+ double rms;
+
+ if (s.useFisheye) {
+ Mat _rvecs, _tvecs;
+ rms = cv::fisheye::calibrate(objectPoints, imagePoints, imageSize,
+ cameraMatrix, distCoeffs, _rvecs, _tvecs, s.flag);
+
+ rvecs.reserve(_rvecs.rows);
+ tvecs.reserve(_tvecs.rows);
+ for (int i = 0; i < static_cast<int>(objectPoints.size()); i++) {
+ rvecs.push_back(_rvecs.row(i));
+ tvecs.push_back(_tvecs.row(i));
+ }
+ } else {
+ rms = calibrateCamera(objectPoints, imagePoints, imageSize,
+ cameraMatrix, distCoeffs, rvecs, tvecs,
+ s.flag | CALIB_USE_LU);
+ }
+
+ LOG(INFO) << "Re-projection error reported by calibrateCamera: "<< rms;
+
+ bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
+
+ objectPoints.clear();
+ objectPoints.resize(imagePoints.size(), newObjPoints);
+ totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs,
+ tvecs, cameraMatrix, distCoeffs, reprojErrs, s.useFisheye);
+
+ return ok;
+}
+
+//! [run_and_save]
+bool runCalibration(const Calibrate::Settings& s, Size imageSize,
+ Mat& cameraMatrix, Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints, float grid_width,
+ bool release_object) {
+ vector<Mat> rvecs, tvecs;
+ vector<float> reprojErrs;
+ double totalAvgErr = 0;
+ vector<Point3f> newObjPoints;
+
+ bool ok = _runCalibration(s, imageSize, cameraMatrix, distCoeffs,
+ imagePoints, rvecs, tvecs, reprojErrs, totalAvgErr, newObjPoints,
+ grid_width, release_object);
+ LOG(INFO) << (ok ? "Calibration succeeded" : "Calibration failed")
+ << ". avg re projection error = " << totalAvgErr;
+
+ return ok;
+}
+//! [run_and_save]
diff --git a/net/cpp/include/ftl/net/peer.hpp b/net/cpp/include/ftl/net/peer.hpp
index da30ee52011406d2ee8c3550aae6999c003ec718..bf0aae4c9c937d02aef8566a1cc365e373b9c416 100644
--- a/net/cpp/include/ftl/net/peer.hpp
+++ b/net/cpp/include/ftl/net/peer.hpp
@@ -86,6 +86,11 @@ class Peer {
bool isConnected() const {
return sock_ != INVALID_SOCKET && status_ == kConnected;
};
+
+ /**
+ * Block until the connection and handshake has completed.
+ */
+ bool waitConnection();
bool isValid() const {
return status_ != kInvalid && sock_ != INVALID_SOCKET;
@@ -298,7 +303,7 @@ int Peer::asyncCall(
// Register the CB
callbacks_[rpcid] = std::make_unique<caller<T>>(cb);
-
+
_send();
return rpcid;
}
diff --git a/net/cpp/include/ftl/net/universe.hpp b/net/cpp/include/ftl/net/universe.hpp
index b29dc10ac5f2c39964ba43b54ba8cfa3bbe9e0a4..c8b10b7651a9cc9cf64ec8237156b0fcf0d0596b 100644
--- a/net/cpp/include/ftl/net/universe.hpp
+++ b/net/cpp/include/ftl/net/universe.hpp
@@ -56,9 +56,11 @@ class Universe {
*
* @param addr URI giving protocol, interface and port
*/
- bool connect(const std::string &addr);
+ Peer *connect(const std::string &addr);
int numberOfPeers() const { return peers_.size(); }
+
+ int waitConnections();
Peer *getPeer(const ftl::UUID &pid) const;
@@ -126,6 +128,8 @@ class Universe {
bool createResource(const std::string &uri);
std::optional<ftl::UUID> findOwner(const std::string &res);
+
+ void setLocalID(const ftl::UUID &u) { this_peer = u; };
private:
void _run();
@@ -138,6 +142,7 @@ class Universe {
private:
bool active_;
+ ftl::UUID this_peer;
nlohmann::json config_;
std::mutex net_mutex_;
fd_set sfderror_;
@@ -199,7 +204,7 @@ std::optional<R> Universe::findOne(const std::string &name, ARGS... args) {
std::map<Peer*, int> record;
for (auto p : peers_) {
- record[p] = p->asyncCall<std::optional<R>>(name, handler, std::forward<ARGS>(args)...);
+ record[p] = p->asyncCall<std::optional<R>>(name, handler, args...);
}
{ // Block thread until async callback notifies us
diff --git a/net/cpp/src/dispatcher.cpp b/net/cpp/src/dispatcher.cpp
index 269db7a6d38de8be0ac8fe25d82b222db63f0c04..a2dfd5ad64ddc9f84b719f633fcd3ef3ab54d5f6 100644
--- a/net/cpp/src/dispatcher.cpp
+++ b/net/cpp/src/dispatcher.cpp
@@ -136,8 +136,8 @@ void ftl::net::Dispatcher::dispatch_notification(Peer &s, msgpack::object const
if (binding) {
try {
auto result = (*binding)(args);
- } catch (int e) {
- throw e;
+ } catch (const int &e) {
+ throw &e;
}
} else {
LOG(ERROR) << "Missing handler for incoming message";
diff --git a/net/cpp/src/peer.cpp b/net/cpp/src/peer.cpp
index 25a1721cb1af4efbb1341e2c43aeb28e61910f30..fb69ce699850bed6550098fdfae0e3a652ba3ae6 100644
--- a/net/cpp/src/peer.cpp
+++ b/net/cpp/src/peer.cpp
@@ -39,6 +39,7 @@
#include <memory>
#include <algorithm>
#include <tuple>
+#include <chrono>
using std::tuple;
using std::get;
@@ -46,6 +47,7 @@ using ftl::net::Peer;
using ftl::URI;
using ftl::net::ws_connect;
using ftl::net::Dispatcher;
+using std::chrono::seconds;
/*static std::string hexStr(const std::string &s)
{
@@ -178,7 +180,8 @@ Peer::Peer(const char *pUri, Dispatcher *d) : uri_(pUri) {
scheme_ = uri.getProtocol();
if (uri.getProtocol() == URI::SCHEME_TCP) {
sock_ = tcpConnect(uri);
- status_ = kConnecting;
+ if (sock_ != INVALID_SOCKET) status_ = kConnecting;
+ else status_ = kReconnecting;
} else if (uri.getProtocol() == URI::SCHEME_WS) {
LOG(INFO) << "Websocket connect " << uri.getPath();
sock_ = tcpConnect(uri);
@@ -480,6 +483,20 @@ void Peer::_sendResponse(uint32_t id, const msgpack::object &res) {
_send();
}
+bool Peer::waitConnection() {
+ if (status_ == kConnected) return true;
+
+ std::unique_lock<std::mutex> lk(send_mtx_);
+ std::condition_variable cv;
+
+ onConnect([&](Peer &p) {
+ cv.notify_all();
+ });
+
+ cv.wait_for(lk, seconds(5));
+ return status_ == kConnected;
+}
+
void Peer::onConnect(const std::function<void(Peer&)> &f) {
if (status_ == kConnected) {
f(*this);
diff --git a/net/cpp/src/universe.cpp b/net/cpp/src/universe.cpp
index e2bb8d8caca3e4f693a86c1aa27d93ee83b53d2e..194b9d32687bf224fdefaf891b6dff90d4c03fa4 100644
--- a/net/cpp/src/universe.cpp
+++ b/net/cpp/src/universe.cpp
@@ -18,12 +18,12 @@ using std::optional;
using std::unique_lock;
using std::mutex;
-Universe::Universe() : active_(true), thread_(Universe::__start, this) {
+Universe::Universe() : active_(true), this_peer(ftl::net::this_peer), thread_(Universe::__start, this) {
_installBindings();
}
Universe::Universe(nlohmann::json &config) :
- active_(true), config_(config), thread_(Universe::__start, this) {
+ active_(true), this_peer(ftl::net::this_peer), config_(config), thread_(Universe::__start, this) {
if (config["listen"].is_array()) {
for (auto &l : config["listen"]) {
listen(l);
@@ -66,9 +66,9 @@ bool Universe::listen(const string &addr) {
return l->isListening();
}
-bool Universe::connect(const string &addr) {
+Peer *Universe::connect(const string &addr) {
auto p = new Peer(addr.c_str(), &disp_);
- if (!p) return false;
+ if (!p) return nullptr;
if (p->status() != Peer::kInvalid) {
unique_lock<mutex> lk(net_mutex_);
@@ -81,7 +81,15 @@ bool Universe::connect(const string &addr) {
peer_ids_[p.id()] = &p;
});
- return p->status() == Peer::kConnecting;
+ return p;
+}
+
+int Universe::waitConnections() {
+ int count = 0;
+ for (auto p : peers_) {
+ if (p->waitConnection()) count++;
+ }
+ return count;
}
int Universe::_setDescriptors() {
@@ -133,7 +141,7 @@ void Universe::_installBindings() {
bind("__owner__", [this](const std::string &res) -> optional<UUID> {
LOG(INFO) << "SOMEONE ASKS FOR " << res;
- if (owned_.count(res) > 0) return ftl::net::this_peer;
+ if (owned_.count(res) > 0) return this_peer;
else return {};
});
}
@@ -179,7 +187,7 @@ bool Universe::_subscribe(const std::string &res) {
optional<UUID> pid = findOwner(res);
if (pid) {
- return call<bool>(*pid, "__subscribe__", ftl::net::this_peer, res);
+ return call<bool>(*pid, "__subscribe__", this_peer, res);
} else {
// No resource found
LOG(WARNING) << "Subscribe to unknown resource: " << res;
@@ -219,7 +227,7 @@ void Universe::_run() {
//Some kind of error occured, it is usually possible to recover from this.
if (selres < 0) {
- std::cout << "SELECT ERROR " << selres << std::endl;
+ std::cout << "SELECT ERROR " << selres << " - " << strerror(errno) << std::endl;
//return false;
continue;
} else if (selres == 0) {
diff --git a/net/cpp/test/net_integration.cpp b/net/cpp/test/net_integration.cpp
index aa698dbe8668fd7082cba84d26ca4f7b3713c2d7..3532c5b7f85fd1f49959a733119506ddf82b4b4e 100644
--- a/net/cpp/test/net_integration.cpp
+++ b/net/cpp/test/net_integration.cpp
@@ -17,27 +17,31 @@ TEST_CASE("Universe::connect()", "[net]") {
Universe b;
a.listen("tcp://localhost:7077");
+ //sleep_for(milliseconds(100));
SECTION("valid tcp connection using ipv4") {
- REQUIRE( b.connect("tcp://127.0.0.1:7077") );
+ auto p = b.connect("tcp://127.0.0.1:7077");
+ REQUIRE( p );
- sleep_for(milliseconds(100));
+ p->waitConnection();
REQUIRE( a.numberOfPeers() == 1 );
REQUIRE( b.numberOfPeers() == 1 );
}
SECTION("valid tcp connection using hostname") {
- REQUIRE( b.connect("tcp://localhost:7077") );
+ auto p = b.connect("tcp://localhost:7077");
+ REQUIRE( p );
- sleep_for(milliseconds(100));
+ p->waitConnection();
REQUIRE( a.numberOfPeers() == 1 );
REQUIRE( b.numberOfPeers() == 1 );
}
SECTION("invalid protocol") {
- REQUIRE( !b.connect("http://127.0.0.1:7077") );
+ auto p = b.connect("http://127.0.0.1:7077");
+ REQUIRE( !p->isValid() );
sleep_for(milliseconds(100));
@@ -87,11 +91,11 @@ TEST_CASE("Universe::broadcast()", "[net]") {
b.broadcast("done");
sleep_for(milliseconds(100));
+ REQUIRE( !done );
}
SECTION("no arguments to one peer") {
- b.connect("tcp://localhost:7077");
- while (a.numberOfPeers() == 0) sleep_for(milliseconds(20));
+ b.connect("tcp://localhost:7077")->waitConnection();
bool done = false;
a.bind("hello", [&done]() {
@@ -106,8 +110,7 @@ TEST_CASE("Universe::broadcast()", "[net]") {
}
SECTION("one argument to one peer") {
- b.connect("tcp://localhost:7077");
- while (a.numberOfPeers() == 0) sleep_for(milliseconds(20));
+ b.connect("tcp://localhost:7077")->waitConnection();
int done = 0;
a.bind("hello", [&done](int v) {
@@ -124,9 +127,8 @@ TEST_CASE("Universe::broadcast()", "[net]") {
SECTION("one argument to two peers") {
Universe c;
- b.connect("tcp://localhost:7077");
- c.connect("tcp://localhost:7077");
- while (a.numberOfPeers() < 2) sleep_for(milliseconds(20));
+ b.connect("tcp://localhost:7077")->waitConnection();
+ c.connect("tcp://localhost:7077")->waitConnection();
int done1 = 0;
b.bind("hello", [&done1](int v) {
@@ -151,8 +153,7 @@ TEST_CASE("Universe::findOwner()", "") {
Universe a;
Universe b;
a.listen("tcp://localhost:7077");
- b.connect("tcp://localhost:7077");
- while (a.numberOfPeers() == 0) sleep_for(milliseconds(20));
+ b.connect("tcp://localhost:7077")->waitConnection();
SECTION("no owners exist") {
REQUIRE( !b.findOwner("ftl://test") );
@@ -165,11 +166,22 @@ TEST_CASE("Universe::findOwner()", "") {
SECTION("three peers and one owner") {
Universe c;
- c.connect("tcp://localhost:7077");
- while (a.numberOfPeers() < 2) sleep_for(milliseconds(20));
+ c.connect("tcp://localhost:7077")->waitConnection();
+ b.setLocalID(ftl::UUID(7));
b.createResource("ftl://test");
- REQUIRE( *(a.findOwner("ftl://test")) == ftl::net::this_peer );
+ REQUIRE( *(a.findOwner("ftl://test")) == ftl::UUID(7) );
+ }
+
+ SECTION("three peers and one owner (2)") {
+ Universe c;
+ c.connect("tcp://localhost:7077")->waitConnection();
+ c.setLocalID(ftl::UUID(7));
+
+ c.createResource("ftl://test");
+ auto r = a.findOwner("ftl://test");
+ REQUIRE( r );
+ REQUIRE( *r == ftl::UUID(7) );
}
}
@@ -177,8 +189,7 @@ TEST_CASE("Universe::subscribe()", "") {
Universe a;
Universe b;
a.listen("tcp://localhost:7077");
- b.connect("tcp://localhost:7077");
- while (a.numberOfPeers() == 0) sleep_for(milliseconds(20));
+ b.connect("tcp://localhost:7077")->waitConnection();
SECTION("no resource exists") {
REQUIRE( !b.subscribe("ftl://test", []() {}) );
@@ -196,8 +207,7 @@ TEST_CASE("Universe::publish()", "") {
Universe a;
Universe b;
a.listen("tcp://localhost:7077");
- b.connect("tcp://localhost:7077");
- while (a.numberOfPeers() == 0) sleep_for(milliseconds(20));
+ b.connect("tcp://localhost:7077")->waitConnection();
SECTION("no subscribers") {
a.createResource("ftl://test");
diff --git a/net/cpp/test/peer_unit.cpp b/net/cpp/test/peer_unit.cpp
index b7bebbcb1cf3b17db40d4dbc5250b792ec2164e2..583f615d4b9c77735342972116bb15a3bac461fb 100644
--- a/net/cpp/test/peer_unit.cpp
+++ b/net/cpp/test/peer_unit.cpp
@@ -185,7 +185,7 @@ TEST_CASE("Peer(int)", "[]") {
sleep_for(milliseconds(50));
- REQUIRE( s.getFTLVersion() == (8 << 16) + (5 << 8) + 2 );
+ REQUIRE( (s.getFTLVersion() == (8 << 16) + (5 << 8) + 2) );
}
SECTION("has correct peer id on full handshake") {
@@ -342,7 +342,7 @@ TEST_CASE("Peer::bind()", "[rpc]") {
sleep_for(milliseconds(50));
REQUIRE( (done == 55) );
- REQUIRE( readRPCReturn<int>(0) == 55 );
+ REQUIRE( (readRPCReturn<int>(0) == 55) );
}
SECTION("vector return value") {
@@ -361,7 +361,7 @@ TEST_CASE("Peer::bind()", "[rpc]") {
REQUIRE( (done == 55) );
auto res = readRPCReturn<vector<int>>(0);
- REQUIRE( res[1] == 45 );
+ REQUIRE( (res[1] == 45) );
}
}
diff --git a/net/cpp/test/uri_unit.cpp b/net/cpp/test/uri_unit.cpp
index e11b320f4c18d599f64ff7361a2e91fcda6c5223..a9fde4e8080ef98732c15149e6bd824dd1b31723 100644
--- a/net/cpp/test/uri_unit.cpp
+++ b/net/cpp/test/uri_unit.cpp
@@ -21,8 +21,8 @@ SCENARIO( "URI() can parse valid URIs", "[utility]" ) {
REQUIRE( uri.isValid() );
REQUIRE( uri.getScheme() == URI::SCHEME_HTTP );
REQUIRE( uri.getHost() == "localhost" );
- REQUIRE( uri.getPort() == 8080 );
- REQUIRE (uri.getPath() == "/test/case.html" );
+ REQUIRE( (uri.getPort() == 8080) );
+ REQUIRE( uri.getPath() == "/test/case.html" );
}
GIVEN( "a valid scheme with path and query" ) {
@@ -85,7 +85,7 @@ SCENARIO( "URI::getAttribute() from query" ) {
GIVEN( "an integer value" ) {
URI uri("http://localhost:1000/hello?x=56");
- REQUIRE( uri.getAttribute<int>("x") == 56 );
+ REQUIRE( (uri.getAttribute<int>("x") == 56) );
}
}
diff --git a/reconstruct/CMakeLists.txt b/reconstruct/CMakeLists.txt
index 3d851d469c66f19ce084a3bf0bbbf1f390adc636..2bbdfa4976d21e4323847580347d7ba87e8d7318 100644
--- a/reconstruct/CMakeLists.txt
+++ b/reconstruct/CMakeLists.txt
@@ -2,9 +2,9 @@
include_directories(${PROJECT_SOURCE_DIR}/reconstruct/include)
#include_directories(${PROJECT_BINARY_DIR})
-
set(REPSRC
src/main.cpp
+ src/registration.cpp
)
add_executable(ftl-reconstruct ${REPSRC})
diff --git a/reconstruct/include/ftl/registration.hpp b/reconstruct/include/ftl/registration.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..58893623e483152618af8eab88fc7d5bd0619769
--- /dev/null
+++ b/reconstruct/include/ftl/registration.hpp
@@ -0,0 +1,34 @@
+#ifndef _FTL_RECONSTRUCT_REGISTRATION_HPP_
+#define _FTL_RECONSTRUCT_REGISTRATION_HPP_
+
+#include <ftl/config.h>
+#include <opencv2/opencv.hpp>
+
+#ifdef HAVE_PCL
+
+#include <pcl/common/common_headers.h>
+#include <pcl/point_cloud.h>
+
+namespace ftl {
+namespace registration {
+
+/* Find transformation matrix for transforming clouds_source to clouds_target
+ * Assumes that corresponding points in clouds_source[i] and clouds_target[i] have same indices
+ */
+Eigen::Matrix4f findTransformation( std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_source,
+ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_target);
+
+
+/* Convert chessboard corners found with OpenCV's findChessboardCorners to PCL point cloud.
+ */
+pcl::PointCloud<pcl::PointXYZ>::Ptr cornersToPointCloud(const std::vector<cv::Point2f> &corners, const cv::Mat &disp, const cv::Mat &Q);
+
+/* Find chessboard corners from image.
+ */
+bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const cv::Mat &Q, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
+
+};
+};
+
+#endif // HAVE_PCL
+#endif // _FTL_RECONSTRUCT_REGISTRATION_HPP_
diff --git a/reconstruct/src/main.cpp b/reconstruct/src/main.cpp
index 51c346da25e0f367247cc33234a63bbac957e251..9c9400e3a2ea05f3d88b1142de444ee3d4a071d4 100644
--- a/reconstruct/src/main.cpp
+++ b/reconstruct/src/main.cpp
@@ -7,6 +7,7 @@
#include <glog/logging.h>
#include <ftl/config.h>
#include <ftl/configuration.hpp>
+
// #include <zlib.h>
// #include <lz4.h>
@@ -21,100 +22,372 @@
#include <ftl/display.hpp>
#include <nlohmann/json.hpp>
-#include "opencv2/imgproc.hpp"
-#include "opencv2/imgcodecs.hpp"
-#include "opencv2/highgui.hpp"
-#include "opencv2/core/utility.hpp"
+#include <opencv2/imgproc.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/core/utility.hpp>
+
+#include <ftl/utility/opencv_to_pcl.hpp>
+#include <ftl/registration.hpp>
#ifdef WIN32
#pragma comment(lib, "Rpcrt4.lib")
#endif
+#ifdef HAVE_PCL
+#include <pcl/point_cloud.h>
+#include <pcl/common/transforms.h>
+#include <pcl/filters/uniform_sampling.h>
+#endif
+
using ftl::net::Universe;
using ftl::Display;
using ftl::config;
using std::string;
using std::vector;
-using cv::Mat;
+
using json = nlohmann::json;
using std::this_thread::sleep_for;
using std::chrono::milliseconds;
using std::mutex;
using std::unique_lock;
-static void run() {
- Universe net(config["net"]);
- Mat rgb, depth, Q;
- mutex m;
+using std::vector;
+
+#ifdef HAVE_PCL
+using pcl::PointCloud;
+using pcl::PointXYZ;
+using pcl::PointXYZRGB;
+#endif
+
+using cv::Mat;
+
+class InputStereo {
+private:
+ string uri_;
+ Mat rgb_, disp_;
+ Mat q_;
+ std::mutex m_;
+
+#ifdef HAVE_PCL
+ static PointCloud<PointXYZRGB>::Ptr _getPointCloud(Mat rgb, Mat disp, Mat q) {
+ cv::Mat_<cv::Vec3f> XYZ(disp.rows, disp.cols);
+ reprojectImageTo3D(disp, XYZ, q, false);
+ return ftl::utility::matToPointXYZ(XYZ, rgb);
+ }
+#endif
+
+public:
+ InputStereo(string uri, const Mat Q): uri_(uri), q_(Q) {};
- Display disp(config["display"]);
+ void set(Mat &rgb, Mat &disp) {
+ unique_lock<mutex> lk(m_);
+ rgb_ = rgb;
+ disp_ = disp;
+ }
- // Make sure connections are complete
- sleep_for(milliseconds(500));
+ string getURI() { return uri_; }
+ Mat getQ() { return q_; }
+
+ /* thread unsafe, use lock() */
+#ifdef HAVE_PCL
+ PointCloud<PointXYZRGB>::Ptr getPointCloud() { return _getPointCloud(rgb_, disp_, q_); }
+#endif
+ Mat getRGB() { return rgb_; }
+ Mat getDisparity() { return disp_; }
+ /* Mat getDepth() {} */
+ /* Mat getLeftRGB() {} */
+ /* Mat getRightRGB() {} */
+ unique_lock<mutex> lock() { return unique_lock<mutex>(m_); } // use recursive mutex instead (and move locking to methods)?
+};
- // TODO(nick) Allow for many sources
- net.subscribe(config["source"], [&rgb,&m,&depth](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
- unique_lock<mutex> lk(m);
- cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb);
- //LOG(INFO) << "Received JPG : " << rgb.cols;
+#ifdef HAVE_PCL
+#include <pcl/filters/uniform_sampling.h>
+
+/*
+class InputMerged {
+private:
+ // todo: Abstract input systems, can also use other 3D inputs (like InputMerged)
+ vector<InputStereo> inputs_;
+ vector<Eigen::Matrix4f> T_;
+
+public:
+ PointCloud<PointXYZRGB>::Ptr getPointCloud() {
+ PointCloud<PointXYZRGB>::Ptr result(new PointCloud<PointXYZRGB>);
+ for (size_t i = 0; i < T_.size(); i++) {
+ inputs_[i].lock();
+ PointCloud<PointXYZRGB>::Ptr cloud = inputs_[i].getPointCloud();
+ // Documentation: Can be used with cloud_in equal to cloud_out
+ pcl::transformPointCloud(*cloud, *cloud, T_[i]);
+ *result += *cloud;
+ }
- depth = Mat(rgb.size(), CV_16UC1);
+ PointCloud<PointXYZRGB>::Ptr result_sampled(new PointCloud<PointXYZRGB>);
+ pcl::UniformSampling<PointXYZRGB> uniform_sampling;
+ uniform_sampling.setInputCloud(result);
+ uniform_sampling.setRadiusSearch(0.1f); // todo parametrize
+ uniform_sampling.filter(*result_sampled);
- /*z_stream infstream;
- infstream.zalloc = Z_NULL;
- infstream.zfree = Z_NULL;
- infstream.opaque = Z_NULL;
- // setup "b" as the input and "c" as the compressed output
- infstream.avail_in = (uInt)d.size(); // size of input
- infstream.next_in = (Bytef *)d.data(); // input char array
- infstream.avail_out = (uInt)depth.step*depth.rows; // size of output
- infstream.next_out = (Bytef *)depth.data; // output char array
-
- // the actual DE-compression work.
- inflateInit(&infstream);
- inflate(&infstream, Z_NO_FLUSH);
- inflateEnd(&infstream);*/
+ return result_sampled;
+ }
+};
+*/
+std::map<string, Eigen::Matrix4f> loadRegistration() {
+ std::map<string, Eigen::Matrix4f> registration;
+ std::ifstream file(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json");
+
+ // Use identity transform if no registration
+ if (!file.is_open()) {
+ Eigen::Matrix4f T;
+ registration["default"] = T.setIdentity();
+ return registration;
+ }
+
+ nlohmann::json load;
+ file >> load;
+
+ for (auto it = load.begin(); it != load.end(); ++it) {
+ Eigen::Matrix4f m;
+ auto data = m.data();
- //LZ4_decompress_safe((char*)d.data(), (char*)depth.data, d.size(), depth.step*depth.rows);
+ for(size_t i = 0; i < 16; i++) {;
+ data[i] = it.value()[i];
+ }
- cv::imdecode(d, cv::IMREAD_UNCHANGED, &depth);
- depth.convertTo(depth, CV_32FC1, 1.0f/16.0f);
- });
+ registration[it.key()] = m;
+
+ }
+ return registration;
+}
+
+void saveRegistration(std::map<string, Eigen::Matrix4f> registration) {
+ nlohmann::json save;
+ for (auto &item : registration) {
+ auto val = nlohmann::json::array();
+ for(size_t j = 0; j < 16; j++) { val.push_back((float) item.second.data()[j]); }
+ save[item.first] = val;
+ }
+
+ std::ofstream file(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json");
+ file << save;
+}
+
+template <template<class> class Container>
+std::map<string, Eigen::Matrix4f> runRegistration(Container<InputStereo> &inputs) {
+
+ std::map<string, Eigen::Matrix4f> registration;
+
+ // NOTE: uses config["registration"]
+
+ if (!config["registration"].is_object()) {
+ LOG(FATAL) << "Configuration missing \"registration\" entry!";
+ return registration;
+ }
+
+ int iter = (int) config["registration"]["calibration"]["iterations"];
+ int delay = (int) config["registration"]["calibration"]["delay"];
+ string ref_uri = (string) config["registration"]["reference-source"];
+ cv::Size pattern_size( (int) config["registration"]["calibration"]["patternsize"][0],
+ (int) config["registration"]["calibration"]["patternsize"][1]);
+
+ // config["registration"] done
+
+ size_t ref_i;
+ bool found = false;
+ for (size_t i = 0; i < inputs.size(); i++) {
+ if (inputs[i].getURI() == ref_uri) {
+ ref_i = i;
+ found = true;
+ break;
+ }
+ }
+
+ if (!found) { LOG(ERROR) << "Reference input not found!"; return registration; }
- while (disp.active()) {
- Mat idepth;
+ for (auto &input : inputs) {
+ cv::namedWindow("Registration: " + input.getURI(), cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
+ }
+
+ // vector for every input: vector of point clouds of patterns
+ vector<vector<PointCloud<PointXYZ>::Ptr>> patterns(inputs.size());
+
+ while (iter > 0) {
+ bool retval = true; // set to false if pattern not found in one of the sources
+ vector<PointCloud<PointXYZ>::Ptr> patterns_iter(inputs.size());
- unique_lock<mutex> lk(m);
- if (depth.cols > 0) {
- // If no calibration data then get it from the remote machine
- if (Q.rows == 0) {
- // Must unlock before calling findOne to prevent net block!!
+ for (size_t i = 0; i < inputs.size(); i++) {
+ InputStereo &input = inputs[i];
+ Mat rgb, disp, Q;
+
+ while(true) {
+ auto lk = input.lock();
+ rgb = input.getRGB();
+ disp = input.getDisparity();
+ Q = input.getQ();
lk.unlock();
- auto buf = net.findOne<vector<unsigned char>>((string)config["source"]+"/calibration");
- lk.lock();
- if (buf) {
- Q = Mat(cv::Size(4,4), CV_32F);
- memcpy(Q.data, (*buf).data(), (*buf).size());
- if (Q.step*Q.rows != (*buf).size()) LOG(ERROR) << "Corrupted calibration";
- disp.setCalibration(Q);
- }
+
+ // todo solve this somewhere else
+ if ((rgb.cols > 0) && (disp.cols > 0)) { break; }
+ else { sleep_for(milliseconds(500)); }
}
+
+ retval &= ftl::registration::findChessboardCorners(rgb, disp, Q, pattern_size, patterns_iter[i]);
+
+ cv::imshow("Registration: " + input.getURI(), rgb);
+ }
+ cv::waitKey(delay);
+
+ // every camera found the pattern
+ if (retval) {
+ for (size_t i = 0; i < patterns_iter.size(); i++) {
+ patterns[i].push_back(patterns_iter[i]);
+ }
+ iter--;
+ }
+ else { LOG(WARNING) << "Pattern not detected on all inputs";}
+ }
+
+ for (auto &input : inputs) { cv::destroyWindow("Registration: " + input.getURI()); }
+
+ for (size_t i = 0; i < inputs.size(); i++) {
+ Eigen::Matrix4f T;
+ if (i == ref_i) {
+ T.setIdentity();
+ }
+ else {
+ T = ftl::registration::findTransformation(patterns[i], patterns[ref_i]);
+ }
+ registration[inputs[i].getURI()] = T;
+ }
+ saveRegistration(registration);
+ return registration;
+}
+#endif
+
+bool getCalibration(Universe &net, string src, Mat &Q) {
+ Q = Mat(cv::Size(4,4), CV_32F);
+ while(true) {
+ auto buf = net.findOne<vector<unsigned char>>((string) src +"/calibration");
+ if (buf) {
+ memcpy(Q.data, (*buf).data(), (*buf).size());
+
+ if (Q.step*Q.rows != (*buf).size()) {
+ LOG(ERROR) << "Corrupted calibration";
+ return false;
+ }
+
+ return true;
+ }
+ else {
+ LOG(INFO) << "Could not get calibration, retrying";
+ sleep_for(milliseconds(500));
+ }
+ }
+}
+
+static void run() {
+ Universe net(config["net"]);
+
+ // Make sure connections are complete
+ sleep_for(milliseconds(500));
+
+ if (!config["sources"].is_array()) {
+ LOG(ERROR) << "No sources configured!";
+ return;
+ }
+
+ std::deque<InputStereo> inputs;
+ std::vector<Display> displays;
+
+ // todo networking and initilization for input should possibly be implemented in their own class
+ // with error handling etc.
+ //
+
+ for (auto &src : config["sources"]) {
+ Mat Q;
+ if (!getCalibration(net, src, Q)) { continue; } // skips input if calibration bad
+
+ InputStereo &in = inputs.emplace_back(src, Q);
+ displays.emplace_back(config["display"], src);
+
+ LOG(INFO) << src << " loaded";
+
+ net.subscribe(src, [&in](const vector<unsigned char> &jpg, const vector<unsigned char> &d) {
+ Mat rgb, disp;
+ cv::imdecode(jpg, cv::IMREAD_COLOR, &rgb);
+ Mat(rgb.size(), CV_16UC1);
+ cv::imdecode(d, cv::IMREAD_UNCHANGED, &disp);
+ disp.convertTo(disp, CV_32FC1, 1.0f/16.0f);
+ in.set(rgb, disp);
+ });
+ }
+
+ // Displays and Inputs configured
+
+ // load point cloud transformations
+
+#ifdef HAVE_PCL
+ std::map<string, Eigen::Matrix4f> registration;
+ if (config["registration"]["calibration"]["run"]) {
+ registration = runRegistration(inputs);
+ }
+ else {
+ registration = loadRegistration();
+ }
+ vector<Eigen::Matrix4f> T;
+ for (auto &input : inputs) {
+ Eigen::Matrix4f RT = (registration.count(input.getURI()) > 0) ? registration[input.getURI()] : registration["default"];
+ T.push_back(RT);
+ }
+
+ //
+ vector<PointCloud<PointXYZRGB>::Ptr> clouds(inputs.size());
+ Display display_merged(config["display"], "Merged"); // todo
+
+ int active = displays.size();
+ while (active > 0) {
+ active = 0;
+
+ net.broadcast("grab"); // To sync cameras
+
+ PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
+
+ for (size_t i = 0; i < inputs.size(); i++) {
+ Display &display = displays[i];
+ InputStereo &input = inputs[i];
+
+ if (!display.active()) continue;
+ active += 1;
+
+ auto lk = input.lock();
+ //Mat rgb = input.getRGB();
+ //Mat disparity = input.getDisparity();
+ clouds[i] = input.getPointCloud();
+ lk.unlock();
+
+ display.render(clouds[i]);
+ //display.render(rgb, disparity);
+ display.wait(50);
}
- if (rgb.cols > 0) {
- //cv::imshow("RGB", rgb);
- disp.render(rgb,depth);
+ for (size_t i = 0; i < clouds.size(); i++) {
+ pcl::transformPointCloud(*clouds[i], *clouds[i], T[i]);
+ *cloud += *clouds[i];
}
- // TODO(nick) Use a swap buffer so this can be unlocked earlier
- lk.unlock();
- //if (cv::waitKey(40) == 27) break;
- disp.wait(40);
+ pcl::UniformSampling<PointXYZRGB> uniform_sampling;
+ uniform_sampling.setInputCloud(cloud);
+ uniform_sampling.setRadiusSearch(0.1f);
+ uniform_sampling.filter(*cloud);
+
+ display_merged.render(cloud);
+ display_merged.wait(50);
}
+#endif
+ // TODO non-PCL (?)
}
int main(int argc, char **argv) {
ftl::configure(argc, argv, "representation"); // TODO(nick) change to "reconstruction"
run();
}
-
diff --git a/reconstruct/src/registration.cpp b/reconstruct/src/registration.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7020db090142332364769ea3c1d899f95c220d82
--- /dev/null
+++ b/reconstruct/src/registration.cpp
@@ -0,0 +1,168 @@
+#include <ftl/registration.hpp>
+
+#ifdef HAVE_PCL
+
+#include <glog/logging.h>
+#include <pcl/common/transforms.h>
+
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <pcl/registration/transformation_estimation_svd_scale.h>
+
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/filters/project_inliers.h>
+#include <pcl/ModelCoefficients.h>
+
+#include <pcl/io/pcd_io.h>
+
+#include <pcl/registration/transformation_validation.h>
+#include <pcl/registration/transformation_validation_euclidean.h>
+
+//#include <pcl/registration/icp_nl.h>
+
+namespace ftl {
+namespace registration {
+
+using std::vector;
+using pcl::PointCloud;
+using pcl::PointXYZ;
+using pcl::PointXYZRGB;
+
+using cv::Mat;
+
+// todo template: fitPlane<typename T>(PointCloud<T> cloud_in, PointCloud<T> cloud_out)
+//
+// Fit calibration pattern into plane using RANSAC + project points
+//
+void fitPlane(PointCloud<PointXYZ>::Ptr cloud_in, PointCloud<PointXYZ>::Ptr cloud_out) {
+ // TODO: include pattern in model (find best alignment of found points and return transformed reference?)
+
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+
+ // Estimate plane with RANSAC
+ pcl::SACSegmentation<PointXYZ> seg;
+
+ seg.setOptimizeCoefficients(true);
+ seg.setModelType(pcl::SACMODEL_PLANE);
+ seg.setMethodType(pcl::SAC_RANSAC);
+ seg.setDistanceThreshold(0.01);
+ seg.setInputCloud(cloud_in);
+ seg.segment(*inliers, *coefficients);
+
+ // Project points into plane
+ pcl::ProjectInliers<PointXYZ> proj;
+ proj.setModelType(pcl::SACMODEL_PLANE);
+ proj.setInputCloud(cloud_in);
+ proj.setModelCoefficients(coefficients);
+ proj.filter(*cloud_out);
+}
+
+//template<typename T = PointXYZ> typename
+PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners, const Mat &disp, const Mat &Q) {
+
+ cv::Matx44d Q_;
+ Q.convertTo(Q_, CV_64F);
+
+ int corners_len = corners.size();
+ vector<cv::Vec3f> points(corners_len);
+
+ // Output point cloud
+ PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
+ cloud->width = corners_len;
+ cloud->height = 1;
+
+ // Follows cv::reprojectImageTo3D(..)
+ // https://github.com/opencv/opencv/blob/371bba8f54560b374fbcd47e7e02f015ac4969ad/modules/calib3d/src/calibration.cpp#L2998
+ // documentation suggests using cv::perspectiveTransform(...) with sparse set of points
+
+ for (int i = 0; i < corners_len; i++) {
+ double x = corners[i].x;
+ double y = corners[i].y;
+ double d = disp.at<float>((int) y, (int) x); // todo: better estimation
+
+ cv::Vec4d homg_pt = Q_ * cv::Vec4d(x, y, d, 1.0);
+ cv::Vec3d p = cv::Vec3d(homg_pt.val) / homg_pt[3];
+
+ // no check since disparities assumed to be good in the calibration pattern
+ // if (fabs(d-minDisparity) <= FLT_EPSILON)
+
+ PointXYZ point;
+ point.x = p[0];
+ point.y = p[1];
+ point.z = p[2];
+ cloud->push_back(point);
+ }
+
+ return cloud;
+}
+
+bool findChessboardCorners(Mat &rgb, const Mat &disp, const Mat &Q, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out) {
+ vector<cv::Point2f> corners(pattern_size.width * pattern_size.height);
+ bool retval = cv::findChessboardCorners(rgb, pattern_size, corners); // (todo) change to findChessboardCornersSB (OpenCV > 4)
+ // todo: verify that disparities are good
+ cv::drawChessboardCorners(rgb, pattern_size, Mat(corners), retval);
+ if (!retval) { return false; }
+ auto corners_cloud = cornersToPointCloud(corners, disp, Q);
+ if (out) { *out += *corners_cloud; } // if cloud is valid, add the points
+ else { out = corners_cloud; }
+ return true;
+}
+
+Eigen::Matrix4f findTransformation(vector<PointCloud<PointXYZ>::Ptr> clouds_source, vector<PointCloud<PointXYZ>::Ptr> clouds_target) {
+ size_t n_clouds = clouds_source.size();
+
+ Eigen::Matrix4f T, T_tmp, T_new;
+ T.setIdentity();
+
+ if ((clouds_source.size() != clouds_target.size()) || (n_clouds == 0)) {
+ LOG(ERROR) << "Input vectors have invalid sizes: clouds_source " << clouds_source.size()
+ << ", clouds_target " << clouds_target.size() << ", transformation can not be estimated";
+
+ return T; // identity
+ }
+
+ // corresponding points have same indices (!!!)
+ int n_points = clouds_source[0]->width * clouds_source[0]->height;
+ vector<int> idx(n_points);
+ for (int i = 0; i < n_points; i++) { idx[i] = i; }
+
+ pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
+ pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+
+ double score_prev = std::numeric_limits<float>::max();
+
+ for (size_t i = 0; i < n_clouds; ++i) {
+ PointCloud<PointXYZ> source;
+ PointCloud<PointXYZ> target = *clouds_target[i];
+
+ pcl::transformPointCloud(*clouds_source[i], source, T);
+ svd.estimateRigidTransformation(source, idx, target, idx, T_new);
+
+ // calculate new transformation
+ T_tmp = T_new * T;
+
+ // score new transformation
+ double score = 0.0;
+ for (size_t j = 0; j < n_clouds; ++j) {
+ score += validate.validateTransformation(clouds_source[j], clouds_target[j], T);
+ }
+ score /= n_clouds;
+
+ // if score doesn't improve, do not use as T, otherwise update T and score
+ if (score < score_prev) {
+ T = T_tmp;
+ score_prev = score;
+ }
+
+ LOG(INFO) << "Validation score: " << score;
+ }
+
+ return T;
+}
+
+} // namespace registration
+} // namespace ftl
+
+#endif // HAVE_PCL
\ No newline at end of file
diff --git a/renderer/cpp/CMakeLists.txt b/renderer/cpp/CMakeLists.txt
index d87223c42bb5637d83f2c8de25d04a86c3144a69..6ef0af73b9e9f21de44ab64c6e9ecbe30b6ee6a8 100644
--- a/renderer/cpp/CMakeLists.txt
+++ b/renderer/cpp/CMakeLists.txt
@@ -1,18 +1,15 @@
-# Need to include staged files and libs
-#include_directories(${PROJECT_SOURCE_DIR}/net/cpp/include)
-#include_directories(${PROJECT_BINARY_DIR})
-
-
add_library(ftlrender
src/display.cpp
)
+# These cause errors in CI build and are being removed from PCL in newer versions
+# target_compile_options(ftlrender PUBLIC ${PCL_DEFINITIONS})
+
target_include_directories(ftlrender PUBLIC
+ ${PCL_INCLUDE_DIRS}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
PRIVATE src)
-target_link_libraries(ftlrender Threads::Threads glog::glog ${OpenCV_LIBS})
-
-
+target_link_libraries(ftlrender ftlcommon Threads::Threads glog::glog ${OpenCV_LIBS} ${PCL_LIBRARIES})
#ADD_SUBDIRECTORY(test)
diff --git a/renderer/cpp/include/ftl/display.hpp b/renderer/cpp/include/ftl/display.hpp
index c6d63ca4f8685c1c0cfa751c2fb87b5b10fb412e..04ff003de8b723f0af5d0475a1946fc6f7a4d302 100644
--- a/renderer/cpp/include/ftl/display.hpp
+++ b/renderer/cpp/include/ftl/display.hpp
@@ -11,31 +11,50 @@
#include <opencv2/opencv.hpp>
#include "opencv2/highgui.hpp"
+#if defined HAVE_PCL
+#include <pcl/common/common_headers.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#endif // HAVE_PCL
+
namespace ftl {
/**
* Multiple local display options for disparity or point clouds.
*/
class Display {
+ private:
+ std::string name_;
public:
- explicit Display(nlohmann::json &config);
+ enum style_t {
+ STYLE_NORMAL, STYLE_DISPARITY, STYLE_DEPTH
+ };
+
+ public:
+ explicit Display(nlohmann::json &config, std::string name);
~Display();
- void setCalibration(const cv::Mat &q) { q_ = q; }
+ inline bool render(const cv::Mat &rgb, const cv::Mat &depth, const cv::Mat &q) { return render(rgb, cv::Mat(), q); }
+ bool render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &depth, const cv::Mat &q);
- bool render(const cv::Mat &rgb, const cv::Mat &depth);
+#if defined HAVE_PCL
+ bool render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr);
+#endif // HAVE_PCL
+ bool render(const cv::Mat &img, style_t s=STYLE_NORMAL);
bool active() const;
void wait(int ms);
private:
- cv::Mat q_;
nlohmann::json config_;
- #if defined HAVE_VIZ
+#if defined HAVE_VIZ
cv::viz::Viz3d *window_;
- #endif // HAVE_VIZ
+#endif // HAVE_VIZ
+
+#if defined HAVE_PCL
+ pcl::visualization::PCLVisualizer::Ptr pclviz_;
+#endif // HAVE_PCL
bool active_;
};
diff --git a/renderer/cpp/src/display.cpp b/renderer/cpp/src/display.cpp
index 7ee8efe3c7a5099656bd89bc680049a0e8949f82..4a64fbb85988d8584b023f4ff65e73b5e3f5fcda 100644
--- a/renderer/cpp/src/display.cpp
+++ b/renderer/cpp/src/display.cpp
@@ -5,16 +5,77 @@
#include <glog/logging.h>
#include <ftl/display.hpp>
+#include <ftl/utility/opencv_to_pcl.hpp>
using ftl::Display;
using cv::Mat;
using cv::Vec3f;
-Display::Display(nlohmann::json &config) : config_(config) {
- #if defined HAVE_VIZ
- window_ = new cv::viz::Viz3d("FTL");
+Display::Display(nlohmann::json &config, std::string name) : config_(config) {
+ name_ = name;
+#if defined HAVE_VIZ
+ window_ = new cv::viz::Viz3d("FTL: " + name);
window_->setBackgroundColor(cv::viz::Color::white());
- #endif // HAVE_VIZ
+#endif // HAVE_VIZ
+
+#if defined HAVE_PCL
+ pclviz_ = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer ("FTL Cloud: " + name));
+ pclviz_->setBackgroundColor (255, 255, 255);
+ pclviz_->addCoordinateSystem (1.0);
+ pclviz_->setShowFPS(true);
+ pclviz_->initCameraParameters ();
+
+ pclviz_->registerPointPickingCallback(
+ [](const pcl::visualization::PointPickingEvent& event, void* viewer_void) {
+ if (event.getPointIndex () == -1) return;
+ float x, y, z;
+ event.getPoint(x, y, z);
+ LOG(INFO) << "( " << x << ", " << y << ", " << z << ")";
+ }, (void*) &pclviz_);
+
+ pclviz_->registerKeyboardCallback (
+ [](const pcl::visualization::KeyboardEvent &event, void* viewer_void) {
+ auto viewer = *static_cast<pcl::visualization::PCLVisualizer::Ptr*>(viewer_void);
+ pcl::visualization::Camera cam;
+ viewer->getCameraParameters(cam);
+
+ Eigen::Vector3f pos(cam.pos[0], cam.pos[1], cam.pos[2]);
+ Eigen::Vector3f focal(cam.focal[0], cam.focal[1], cam.focal[2]);
+ Eigen::Vector3f dir = focal - pos; //.normalize();
+ dir.normalize();
+
+ const float speed = 40.0f;
+
+ if (event.getKeySym() == "Up") {
+ pos += speed*dir;
+ focal += speed*dir;
+ } else if (event.getKeySym() == "Down") {
+ pos -= speed*dir;
+ focal -= speed*dir;
+ } else if (event.getKeySym() == "Left") {
+ Eigen::Matrix3f m = Eigen::AngleAxisf(-0.5f*M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
+ dir = m*dir;
+ pos += speed*dir;
+ focal += speed*dir;
+ } else if (event.getKeySym() == "Right") {
+ Eigen::Matrix3f m = Eigen::AngleAxisf(0.5f*M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
+ dir = m*dir;
+ pos += speed*dir;
+ focal += speed*dir;
+ }
+
+
+ cam.pos[0] = pos[0];
+ cam.pos[1] = pos[1];
+ cam.pos[2] = pos[2];
+ cam.focal[0] = focal[0];
+ cam.focal[1] = focal[1];
+ cam.focal[2] = focal[2];
+ viewer->setCameraParameters(cam);
+
+ }, (void*)&pclviz_);
+#endif // HAVE_PCL
+
active_ = true;
}
@@ -24,15 +85,26 @@ Display::~Display() {
#endif // HAVE_VIZ
}
-bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
+bool Display::render(const cv::Mat &rgb, const cv::Mat &rgbr, const cv::Mat &depth, const cv::Mat &q) {
Mat idepth;
- if (config_["points"] && q_.rows != 0) {
- #if defined HAVE_VIZ
+ if (config_["points"] && q.rows != 0) {
+#if defined HAVE_PCL
+ cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
+ reprojectImageTo3D(depth, XYZ, q, false);
+ auto pc = ftl::utility::matToPointXYZ(XYZ, rgb);
+
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
+ if (!pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {
+ pclviz_->addPointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction");
+ pclviz_->setCameraPosition(-878.0, -71.0, -2315.0, -0.1, -0.99, 0.068, 0.0, -1.0, 0.0);
+ pclviz_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "reconstruction");
+ }
+#elif defined HAVE_VIZ
//cv::Mat Q_32F;
//calibrate_.getQ().convertTo(Q_32F, CV_32F);
cv::Mat_<cv::Vec3f> XYZ(depth.rows, depth.cols); // Output point cloud
- reprojectImageTo3D(depth+20.0f, XYZ, q_, true);
+ reprojectImageTo3D(depth+20.0f, XYZ, q, true);
// Remove all invalid pixels from point cloud
XYZ.setTo(Vec3f(NAN, NAN, NAN), depth == 0.0f);
@@ -45,15 +117,28 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
//window_->spinOnce(40, true);
- #else // HAVE_VIZ
+#else // HAVE_VIZ
LOG(ERROR) << "Need OpenCV Viz module to display points";
- #endif // HAVE_VIZ
+#endif // HAVE_VIZ
}
if (config_["left"]) {
- cv::imshow("RGB", rgb);
+ if (config_["crosshair"]) {
+ cv::line(rgb, cv::Point(0, rgb.rows/2), cv::Point(rgb.cols-1, rgb.rows/2), cv::Scalar(0,0,255), 1);
+ cv::line(rgb, cv::Point(rgb.cols/2, 0), cv::Point(rgb.cols/2, rgb.rows-1), cv::Scalar(0,0,255), 1);
+ }
+ cv::namedWindow("Left: " + name_, cv::WINDOW_KEEPRATIO);
+ cv::imshow("Left: " + name_, rgb);
+ }
+ if (config_["right"]) {
+ if (config_["crosshair"]) {
+ cv::line(rgbr, cv::Point(0, rgbr.rows/2), cv::Point(rgbr.cols-1, rgbr.rows/2), cv::Scalar(0,0,255), 1);
+ cv::line(rgbr, cv::Point(rgbr.cols/2, 0), cv::Point(rgbr.cols/2, rgbr.rows-1), cv::Scalar(0,0,255), 1);
+ }
+ cv::namedWindow("Right: " + name_, cv::WINDOW_KEEPRATIO);
+ cv::imshow("Right: " + name_, rgbr);
}
if (config_["depth"]) {
@@ -74,7 +159,7 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
idepth.convertTo(idepth, CV_8U, 255.0f / 256.0f); // TODO(nick)
applyColorMap(idepth, idepth, cv::COLORMAP_JET);
- cv::imshow("Disparity", idepth);
+ cv::imshow("Disparity:" + name_, idepth);
//if(cv::waitKey(40) == 27) {
// exit if ESC is pressed
// active_ = false;
@@ -84,14 +169,48 @@ bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
return true;
}
+#if defined HAVE_PCL
+bool Display::render(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pc) {
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pc);
+ if (!pclviz_->updatePointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction")) {
+ pclviz_->addPointCloud<pcl::PointXYZRGB> (pc, rgb, "reconstruction");
+ pclviz_->setCameraPosition(-878.0, -71.0, -2315.0, -0.1, -0.99, 0.068, 0.0, -1.0, 0.0);
+ pclviz_->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "reconstruction");
+ }
+ return true;
+}
+#endif // HAVE_PCL
+bool Display::render(const cv::Mat &img, style_t s) {
+ if (s == STYLE_NORMAL) {
+ cv::imshow("Image", img);
+ } else if (s = STYLE_DISPARITY) {
+ Mat idepth;
+
+ if ((bool)config_["flip_vert"]) {
+ cv::flip(img, idepth, 0);
+ } else {
+ idepth = img;
+ }
+
+ idepth.convertTo(idepth, CV_8U, 255.0f / 256.0f);
+
+ applyColorMap(idepth, idepth, cv::COLORMAP_JET);
+ cv::imshow("Disparity", idepth);
+ }
+
+ return true;
+}
+
void Display::wait(int ms) {
- if (config_["points"] && q_.rows != 0) {
- #if defined HAVE_VIZ
+ if (config_["points"]) {
+ #if defined HAVE_PCL
+ pclviz_->spinOnce(20);
+ #elif defined HAVE_VIZ
window_->spinOnce(1, true);
#endif // HAVE_VIZ
}
- if (config_["disparity"]) {
+ if (config_["disparity"] || config_["left"] || config_["right"]) {
if(cv::waitKey(ms) == 27) {
// exit if ESC is pressed
active_ = false;
@@ -100,7 +219,9 @@ void Display::wait(int ms) {
}
bool Display::active() const {
- #if defined HAVE_VIZ
+ #if defined HAVE_PCL
+ return active_ && !pclviz_->wasStopped();
+ #elif defined HAVE_VIZ
return active_ && !window_->wasStopped();
#else
return active_;
diff --git a/vision/include/ftl/calibrate.hpp b/vision/include/ftl/calibrate.hpp
index 79fb310b348db16bbebde7f259e95db17c83b9f8..83a2bd9c139bee3a1920545ff0c9244bae43a2bc 100644
--- a/vision/include/ftl/calibrate.hpp
+++ b/vision/include/ftl/calibrate.hpp
@@ -29,7 +29,12 @@ class Calibrate {
// TODO(nick) replace or remove this class.
class Settings {
public:
- Settings() : goodInput(false) {}
+ Settings() : calibrationPattern(CHESSBOARD), squareSize(50.0f),
+ nrFrames(30), aspectRatio(1.0f), delay(100), writePoints(false), writeExtrinsics(true),
+ writeGrid(false), calibZeroTangentDist(false), calibFixPrincipalPoint(true),
+ flipVertical(false), showUndistorsed(true), useFisheye(false), fixK1(false),
+ fixK2(false), fixK3(false), fixK4(false), fixK5(false), cameraID(0), atImageList(0),
+ inputType(INVALID), goodInput(false), flag(0) {}
enum Pattern { NOT_EXISTING, CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID };
enum InputType { INVALID, CAMERA, VIDEO_FILE, IMAGE_LIST };
diff --git a/vision/include/ftl/local.hpp b/vision/include/ftl/local.hpp
index 80b527cc74c50122274f9568c1ce8b5b9ad2b653..9a4204e65bab87eb71af8768decb12d116116b20 100644
--- a/vision/include/ftl/local.hpp
+++ b/vision/include/ftl/local.hpp
@@ -12,7 +12,7 @@ namespace cv {
namespace ftl {
class LocalSource {
public:
- LocalSource(nlohmann::json &config);
+ explicit LocalSource(nlohmann::json &config);
LocalSource(const std::string &vid, nlohmann::json &config);
bool left(cv::Mat &m);
diff --git a/vision/lib/elas/triangle.cpp b/vision/lib/elas/triangle.cpp
index 8de1557bc5c7192530d4594c03d0ef9e90eef72c..14d4a2d03c06537a24547bb8c6c44526964ab207 100644
--- a/vision/lib/elas/triangle.cpp
+++ b/vision/lib/elas/triangle.cpp
@@ -1419,27 +1419,27 @@ void printtriangle(struct mesh *m, struct behavior *b, struct otri *t)
struct osub printsh;
vertex printvertex;
- printf("triangle x%lx with orientation %d:\n", (unsigned long long) t->tri,
+ printf("triangle x%lx with orientation %d:\n", (unsigned long) t->tri,
t->orient);
decode(t->tri[0], printtri);
if (printtri.tri == m->dummytri) {
printf(" [0] = Outer space\n");
} else {
- printf(" [0] = x%lx %d\n", (unsigned long long) printtri.tri,
+ printf(" [0] = x%lx %d\n", (unsigned long) printtri.tri,
printtri.orient);
}
decode(t->tri[1], printtri);
if (printtri.tri == m->dummytri) {
printf(" [1] = Outer space\n");
} else {
- printf(" [1] = x%lx %d\n", (unsigned long long) printtri.tri,
+ printf(" [1] = x%lx %d\n", (unsigned long) printtri.tri,
printtri.orient);
}
decode(t->tri[2], printtri);
if (printtri.tri == m->dummytri) {
printf(" [2] = Outer space\n");
} else {
- printf(" [2] = x%lx %d\n", (unsigned long long) printtri.tri,
+ printf(" [2] = x%lx %d\n", (unsigned long) printtri.tri,
printtri.orient);
}
@@ -1448,37 +1448,37 @@ void printtriangle(struct mesh *m, struct behavior *b, struct otri *t)
printf(" Origin[%d] = NULL\n", (t->orient + 1) % 3 + 3);
else
printf(" Origin[%d] = x%lx (%.12g, %.12g)\n",
- (t->orient + 1) % 3 + 3, (unsigned long long) printvertex,
+ (t->orient + 1) % 3 + 3, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
dest(*t, printvertex);
if (printvertex == (vertex) NULL)
printf(" Dest [%d] = NULL\n", (t->orient + 2) % 3 + 3);
else
printf(" Dest [%d] = x%lx (%.12g, %.12g)\n",
- (t->orient + 2) % 3 + 3, (unsigned long long) printvertex,
+ (t->orient + 2) % 3 + 3, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
apex(*t, printvertex);
if (printvertex == (vertex) NULL)
printf(" Apex [%d] = NULL\n", t->orient + 3);
else
printf(" Apex [%d] = x%lx (%.12g, %.12g)\n",
- t->orient + 3, (unsigned long long) printvertex,
+ t->orient + 3, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
if (b->usesegments) {
sdecode(t->tri[6], printsh);
if (printsh.ss != m->dummysub) {
- printf(" [6] = x%lx %d\n", (unsigned long long) printsh.ss,
+ printf(" [6] = x%lx %d\n", (unsigned long) printsh.ss,
printsh.ssorient);
}
sdecode(t->tri[7], printsh);
if (printsh.ss != m->dummysub) {
- printf(" [7] = x%lx %d\n", (unsigned long long) printsh.ss,
+ printf(" [7] = x%lx %d\n", (unsigned long) printsh.ss,
printsh.ssorient);
}
sdecode(t->tri[8], printsh);
if (printsh.ss != m->dummysub) {
- printf(" [8] = x%lx %d\n", (unsigned long long) printsh.ss,
+ printf(" [8] = x%lx %d\n", (unsigned long) printsh.ss,
printsh.ssorient);
}
}
@@ -1506,19 +1506,19 @@ void printsubseg(struct mesh *m, struct behavior *b, struct osub *s)
vertex printvertex;
printf("subsegment x%lx with orientation %d and mark %d:\n",
- (unsigned long long) s->ss, s->ssorient, mark(*s));
+ (unsigned long) s->ss, s->ssorient, mark(*s));
sdecode(s->ss[0], printsh);
if (printsh.ss == m->dummysub) {
printf(" [0] = No subsegment\n");
} else {
- printf(" [0] = x%lx %d\n", (unsigned long long) printsh.ss,
+ printf(" [0] = x%lx %d\n", (unsigned long) printsh.ss,
printsh.ssorient);
}
sdecode(s->ss[1], printsh);
if (printsh.ss == m->dummysub) {
printf(" [1] = No subsegment\n");
} else {
- printf(" [1] = x%lx %d\n", (unsigned long long) printsh.ss,
+ printf(" [1] = x%lx %d\n", (unsigned long) printsh.ss,
printsh.ssorient);
}
@@ -1527,28 +1527,28 @@ void printsubseg(struct mesh *m, struct behavior *b, struct osub *s)
printf(" Origin[%d] = NULL\n", 2 + s->ssorient);
else
printf(" Origin[%d] = x%lx (%.12g, %.12g)\n",
- 2 + s->ssorient, (unsigned long long) printvertex,
+ 2 + s->ssorient, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
sdest(*s, printvertex);
if (printvertex == (vertex) NULL)
printf(" Dest [%d] = NULL\n", 3 - s->ssorient);
else
printf(" Dest [%d] = x%lx (%.12g, %.12g)\n",
- 3 - s->ssorient, (unsigned long long) printvertex,
+ 3 - s->ssorient, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
decode(s->ss[6], printtri);
if (printtri.tri == m->dummytri) {
printf(" [6] = Outer space\n");
} else {
- printf(" [6] = x%lx %d\n", (unsigned long long) printtri.tri,
+ printf(" [6] = x%lx %d\n", (unsigned long) printtri.tri,
printtri.orient);
}
decode(s->ss[7], printtri);
if (printtri.tri == m->dummytri) {
printf(" [7] = Outer space\n");
} else {
- printf(" [7] = x%lx %d\n", (unsigned long long) printtri.tri,
+ printf(" [7] = x%lx %d\n", (unsigned long) printtri.tri,
printtri.orient);
}
@@ -1557,14 +1557,14 @@ void printsubseg(struct mesh *m, struct behavior *b, struct osub *s)
printf(" Segment origin[%d] = NULL\n", 4 + s->ssorient);
else
printf(" Segment origin[%d] = x%lx (%.12g, %.12g)\n",
- 4 + s->ssorient, (unsigned long long) printvertex,
+ 4 + s->ssorient, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
segdest(*s, printvertex);
if (printvertex == (vertex) NULL)
printf(" Segment dest [%d] = NULL\n", 5 - s->ssorient);
else
printf(" Segment dest [%d] = x%lx (%.12g, %.12g)\n",
- 5 - s->ssorient, (unsigned long long) printvertex,
+ 5 - s->ssorient, (unsigned long) printvertex,
printvertex[0], printvertex[1]);
}
diff --git a/vision/src/algorithms/elas.cpp b/vision/src/algorithms/elas.cpp
index ca37357442b2f7ecfcba7996601ae741cd463392..b99c309c96717739327a488efcdfd2e510c9bbe7 100644
--- a/vision/src/algorithms/elas.cpp
+++ b/vision/src/algorithms/elas.cpp
@@ -35,7 +35,7 @@ void ELAS::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
disp = Mat(cv::Size(l.cols, l.rows), CV_32F);
Mat dispr(cv::Size(l.cols, l.rows), CV_32F);
- const int32_t dims[3] = {l.cols,l.rows,lbw.step};
+ const int32_t dims[3] = {l.cols,l.rows,static_cast<int32_t>(lbw.step)};
if (disp.step/sizeof(float) != lbw.step) LOG(WARNING) << "Incorrect disparity step";
diff --git a/vision/src/calibrate.cpp b/vision/src/calibrate.cpp
index 7151a01bb2dfab9fa7b480612c3839f344e666a7..e5025a9d85a0c2470e504e11e7a8ffaa146914e7 100644
--- a/vision/src/calibrate.cpp
+++ b/vision/src/calibrate.cpp
@@ -491,7 +491,7 @@ static double computeReprojectionErrors(
vector<float>& perViewErrors, bool fisheye) {
vector<Point2f> imagePoints2;
size_t totalPoints = 0;
- double totalErr = 0, err;
+ double totalErr = 0;
perViewErrors.resize(objectPoints.size());
for (size_t i = 0; i < objectPoints.size(); ++i) {
@@ -502,7 +502,7 @@ static double computeReprojectionErrors(
projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix,
distCoeffs, imagePoints2);
}
- err = norm(imagePoints[i], imagePoints2, NORM_L2);
+ double err = norm(imagePoints[i], imagePoints2, NORM_L2);
size_t n = objectPoints[i].size();
perViewErrors[i] = static_cast<float>(std::sqrt(err*err/n));
@@ -576,10 +576,6 @@ static bool _runCalibration(const Calibrate::Settings& s, Size& imageSize,
tvecs.push_back(_tvecs.row(i));
}
} else {
- int iFixedPoint = -1;
- if (release_object)
- iFixedPoint = s.boardSize.width - 1;
-
rms = calibrateCamera(objectPoints, imagePoints, imageSize,
cameraMatrix, distCoeffs, rvecs, tvecs,
s.flag | CALIB_USE_LU);
diff --git a/vision/src/local.cpp b/vision/src/local.cpp
index d8d5229e84ccb39b7639a914941185c423b36a27..f26bea1d31c571dbb9b3fc133add138e0e59df3e 100644
--- a/vision/src/local.cpp
+++ b/vision/src/local.cpp
@@ -58,6 +58,15 @@ LocalSource::LocalSource(nlohmann::json &config)
stereo_ = false;
LOG(WARNING) << "Not able to find second camera for stereo";
} else {
+ camera_a_->set(cv::CAP_PROP_FRAME_WIDTH,(int)config["width"]);
+ camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT,(int)config["height"]);
+ camera_b_->set(cv::CAP_PROP_FRAME_WIDTH,(int)config["width"]);
+ camera_b_->set(cv::CAP_PROP_FRAME_HEIGHT,(int)config["height"]);
+
+ Mat frame;
+ camera_a_->grab();
+ camera_a_->retrieve(frame);
+ LOG(INFO) << "Video size : " << frame.cols << "x" << frame.rows;
stereo_ = true;
}
diff --git a/vision/src/main.cpp b/vision/src/main.cpp
index 0665b797cdb017c09dbafdfcf5f0dd9011f5ead4..49d3f43dbf0fa1bceda2b64b42d1b67d7f88bb83 100644
--- a/vision/src/main.cpp
+++ b/vision/src/main.cpp
@@ -108,18 +108,30 @@ static void run(const string &file) {
return buf;
});
+ Mat l, r, disp;
+ bool grabbed = false;
+ mutex datam;
+ condition_variable datacv;
+
+ // Wait for grab message to sync camera capture
+ net.bind("grab", [&calibrate,&l,&r,&datam,&datacv,&grabbed]() -> void {
+ unique_lock<mutex> datalk(datam);
+ if (grabbed) return;
+ calibrate.rectified(l, r);
+ grabbed = true;
+ datacv.notify_one();
+ });
+
// Choose and configure disparity algorithm
auto disparity = Disparity::create(config["disparity"]);
if (!disparity) LOG(FATAL) << "Unknown disparity algorithm : " << config["disparity"];
- Mat l, r, disp;
Mat pl, pdisp;
vector<unsigned char> rgb_buf;
vector<unsigned char> d_buf;
string uri = string("ftl://utu.fi/")+(string)config["stream"]["name"]+string("/rgb-d");
- Display display(config["display"]);
- display.setCalibration(Q_32F);
+ Display display(config["display"], "local");
Streamer stream(net, config["stream"]);
@@ -130,23 +142,22 @@ static void run(const string &file) {
condition_variable cv;
int jobs = 0;
- // Pipeline for disparity
- pool.push([&](int id) {
- auto start = std::chrono::high_resolution_clock::now();
- // Read calibrated images.
+ // Fake grab if no peers to allow visualisation locally
+ if (net.numberOfPeers() == 0) {
+ grabbed = true;
calibrate.rectified(l, r);
+ }
- // Feed into sync buffer and network forward
- //sync->feed(ftl::LEFT, l, lsrc->getTimestamp());
- //sync->feed(ftl::RIGHT, r, lsrc->getTimestamp());
-
- // Read back from buffer
- //sync->get(ftl::LEFT, l);
- //sync->get(ftl::RIGHT, r);
+ // Pipeline for disparity
+ pool.push([&](int id) {
+ // Wait for image grab
+ unique_lock<mutex> datalk(datam);
+ datacv.wait(datalk, [&grabbed](){ return grabbed; });
+ grabbed = false;
- LOG(INFO) << "PRE DISPARITY";
+ auto start = std::chrono::high_resolution_clock::now();
disparity->compute(l, r, disp);
- LOG(INFO) << "POST DISPARITY";
+ datalk.unlock();
unique_lock<mutex> lk(m);
jobs++;
@@ -158,50 +169,6 @@ static void run(const string &file) {
LOG(INFO) << "Disparity in " << elapsed.count() << "s";
});
- // Pipeline for jpeg compression
- /*pool.push([&](int id) {
- auto start = std::chrono::high_resolution_clock::now();
- if (pl.rows != 0) cv::imencode(".jpg", pl, rgb_buf);
- unique_lock<mutex> lk(m);
- jobs++;
- lk.unlock();
- cv.notify_one();
-
- std::chrono::duration<double> elapsed =
- std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "JPG in " << elapsed.count() << "s";
- });*/
-
- // Pipeline for zlib compression
- /*pool.push([&](int id) {
- auto start = std::chrono::high_resolution_clock::now();
- if (pl.rows != 0) {
- d_buf.resize(pdisp.step*pdisp.rows);
- z_stream defstream;
- defstream.zalloc = Z_NULL;
- defstream.zfree = Z_NULL;
- defstream.opaque = Z_NULL;
- defstream.avail_in = pdisp.step*pdisp.rows;
- defstream.next_in = (Bytef *)pdisp.data; // input char array
- defstream.avail_out = (uInt)pdisp.step*pdisp.rows; // size of output
- defstream.next_out = (Bytef *)d_buf.data(); // output char array
-
- deflateInit(&defstream, Z_BEST_COMPRESSION);
- deflate(&defstream, Z_FINISH);
- deflateEnd(&defstream);
-
- d_buf.resize(defstream.total_out);
- }
- unique_lock<mutex> lk(m);
- jobs++;
- lk.unlock();
- cv.notify_one();
-
- std::chrono::duration<double> elapsed =
- std::chrono::high_resolution_clock::now() - start;
- LOG(INFO) << "ZLIB in " << elapsed.count() << "s";
- });*/
-
// Pipeline for stream compression
pool.push([&](int id) {
auto start = std::chrono::high_resolution_clock::now();
@@ -217,7 +184,7 @@ static void run(const string &file) {
});
// Send RGB+Depth images for local rendering
- if (pl.rows > 0) display.render(pl, pdisp);
+ if (pl.rows > 0) display.render(pl, r, pdisp, Q_32F);
display.wait(1);
// Wait for both pipelines to complete