Initial commit: Speckle-Scanner 3D pipeline with setup README

05_disparity/libsgm/sample/CMakeLists.txt

@@ -0,0 +1,62 @@
+cmake_minimum_required(VERSION 3.18)
+
+project(samples LANGUAGES CXX CUDA)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+# required packages
+find_package(OpenCV REQUIRED)
+
+set(SRCS_COMMON sample_common.cpp sample_common.h)
+
+# sample image
+add_executable(stereosgm_image stereosgm_image.cpp ${SRCS_COMMON})
+target_include_directories(stereosgm_image PRIVATE ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(stereosgm_image sgm ${OpenCV_LIBS})
+
+# sample movie
+add_executable(stereosgm_movie stereosgm_movie.cpp ${SRCS_COMMON})
+target_include_directories(stereosgm_movie PRIVATE ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(stereosgm_movie sgm ${OpenCV_LIBS})
+
+# sample mynew
+add_executable(stereosgm_new stereosgm_new.cpp ${SRCS_COMMON})
+target_include_directories(stereosgm_new PRIVATE ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(stereosgm_new sgm ${OpenCV_LIBS})
+
+# sample benchmark
+add_executable(stereosgm_benchmark stereosgm_benchmark.cpp ${SRCS_COMMON})
+target_include_directories(stereosgm_benchmark PRIVATE ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(stereosgm_benchmark sgm ${OpenCV_LIBS})
+
+# sample reprojection
+add_executable(stereosgm_reprojection stereosgm_reprojection.cpp ${SRCS_COMMON})
+target_include_directories(stereosgm_reprojection PRIVATE ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(stereosgm_reprojection sgm ${OpenCV_LIBS})
+
+# sample image with cv::GpuMat
+if(BUILD_OPENCV_WRAPPER)
+	add_executable(stereosgm_image_cv_gpumat stereosgm_image_cv_gpumat.cpp ${SRCS_COMMON})
+	target_include_directories(stereosgm_image_cv_gpumat PRIVATE ${OpenCV_INCLUDE_DIRS})
+	target_link_libraries(stereosgm_image_cv_gpumat sgm ${OpenCV_LIBS})
+endif()
+
+# sample ZED camera
+if(ENABLE_ZED_DEMO)
+	if(WIN32)
+		set(ZED_SDK_LIB "C:\\Program Files (x86)\\ZED SDK\\lib\\sl_zed64.lib" CACHE STRING "ZED SDK library(sl_zed**.llb) path.")
+		set(ZED_SDK_INCLUDE_DIR "C:\\Program Files (x86)\\ZED SDK\\include" CACHE STRING "ZED SDK include path.")
+	else()
+		set(ZED_SDK_LIB "/usr/local/zed/lib/libsl_zed.so" CACHE STRING "ZED SDK library(sl_zed**.llb) path.")
+		set(ZED_SDK_INCLUDE_DIR "/usr/local/zed/include" CACHE STRING "ZED SDK include path.")
+	endif()
+
+	find_package(ZED 3 REQUIRED)
+	string(REGEX REPLACE [[; +]] [[;]] CUDA_NPP_LIBRARIES_ZED "${CUDA_NPP_LIBRARIES_ZED}")
+
+	add_executable(stereosgm_zed stereosgm_zed.cpp ${SRCS_COMMON})
+	target_include_directories(stereosgm_zed PRIVATE ${OpenCV_INCLUDE_DIRS} ${ZED_INCLUDE_DIRS})
+	target_link_directories(stereosgm_zed PRIVATE ${ZED_LIBRARY_DIR})
+	target_link_libraries(stereosgm_zed sgm ${OpenCV_LIBS} ${ZED_LIBRARIES} ${CUDA_NPP_LIBRARIES_ZED})
+endif()

05_disparity/libsgm/sample/camera_parameters/camera_daimler_gt_stixel.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<opencv_storage>
+
+<!--  Intrinsic parameters -->
+<FocalLengthX>1267.485352</FocalLengthX> <!--  focal length x (pixel) -->
+<FocalLengthY>1224.548950</FocalLengthY> <!--  focal length y (pixel) -->
+<CenterX>472.735474</CenterX>            <!--  principal point x (pixel) -->
+<CenterY>175.787781</CenterY>            <!--  principal point y (pixel) -->
+
+<!--  Extrinsic parameters -->
+<BaseLine>0.214382</BaseLine>            <!--  baseline (meter) -->
+<Height>1.170000</Height>                <!--  height position (meter) -->
+<Tilt>0.081276</Tilt>                    <!--  tilt angle (radian) -->
+
+</opencv_storage>

05_disparity/libsgm/sample/camera_parameters/camera_daimler_urban_seg.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<opencv_storage>
+<FocalLengthX>1249.7700195</FocalLengthX>
+<FocalLengthY>1249.7700195</FocalLengthY>
+<CenterX>480.8460083</CenterX>
+<CenterY>237.4100037</CenterY>
+<BaseLine>0.2339240</BaseLine>
+<Height>1.2000000</Height>
+<Tilt>0.07</Tilt>
+</opencv_storage>

05_disparity/libsgm/sample/data/Untitled Document 1

@@ -0,0 +1,4 @@
+# sample mynew
+add_executable(stereosgm_new stereosgm_new.cpp ${SRCS_COMMON})
+target_include_directories(stereosgm_new PRIVATE ${OpenCV_INCLUDE_DIRS})
+target_link_libraries(stereosgm_new sgm ${OpenCV_LIBS})

05_disparity/libsgm/sample/data/stereosgm_new.cpp

@@ -0,0 +1,160 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+#include <chrono>
+#include <stdexcept>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <fstream> // Add this line to use std::ofstream for file output
+
+#include <libsgm.h>
+
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left-image-format  | <none> | format string for path to input left image  }"
+"{ @right-image-format | <none> | format string for path to input right image }"
+"{ disp_size           |    256 | maximum possible disparity value            }"
+"{ start_number        |      0 | index to start reading                      }"
+"{ help h              |        | display this help and exit                  }";
+
+class ImagePreprocessor {
+public:
+    void preprocess_image_pair(cv::Mat& img_left, cv::Mat& img_right) {
+        // Get the shape of both images
+        int h1 = img_left.rows, w1 = img_left.cols;
+        int h2 = img_right.rows, w2 = img_right.cols;
+
+        // Find the minimum height and width between the two images
+        int min_height = std::min(h1, h2);
+        int min_width = std::min(w1, w2);
+
+        // Crop both images to match the minimum height and width
+        img_left = img_left(cv::Rect(0, 0, min_width, min_height));
+        img_right = img_right(cv::Rect(0, 0, min_width, min_height));
+
+        // Convert to CV_8U grayscale
+        //cv::cvtColor(img_left, img_left, cv::COLOR_BGR2GRAY);
+        img_left.convertTo(img_left, CV_8U); // Ensure it's in CV_8U format
+        //cv::cvtColor(img_right, img_right, cv::COLOR_BGR2GRAY);
+        img_right.convertTo(img_right, CV_8U); // Ensure it's in CV_8U format
+    }
+};
+
+int main(int argc, char* argv[])
+{
+    cv::CommandLineParser parser(argc, argv, keys);
+    if (parser.has("help")) {
+        parser.printMessage();
+        return 0;
+    }
+
+    const std::string image_format_L = parser.get<cv::String>("@left-image-format");
+    const std::string image_format_R = parser.get<cv::String>("@right-image-format");
+    const int disp_size = parser.get<int>("disp_size");
+    const int start_number = parser.get<int>("start_number");
+
+    if (!parser.check()) {
+        parser.printErrors();
+        parser.printMessage();
+        std::exit(EXIT_FAILURE);
+    }
+
+    cv::Mat I1, I2;
+
+    ImagePreprocessor preprocessor; // Create an instance of the ImagePreprocessor class
+
+    for (int frame_no = start_number;; frame_no++) {
+        I1 = cv::imread(cv::format(image_format_L.c_str(), frame_no), cv::IMREAD_GRAYSCALE);
+        I2 = cv::imread(cv::format(image_format_R.c_str(), frame_no), cv::IMREAD_GRAYSCALE);
+        
+        // Check if images are empty, if so break the loop
+        if (I1.empty() || I2.empty()) {
+            std::cout << "No more images to process or image pair not found." << std::endl;
+            break;
+        }
+
+        // Preprocess the images
+        preprocessor.preprocess_image_pair(I1, I2);
+
+        const int width = I1.cols;
+        const int height = I1.rows;
+
+        const int src_depth = I1.type() == CV_8U ? 8 : 16;
+        const int dst_depth = disp_size < 256 ? 8 : 16;
+        const int src_bytes = src_depth * width * height / 8;
+        const int dst_bytes = dst_depth * width * height / 8;
+
+        sgm::StereoSGM sgm(width, height, disp_size, src_depth, dst_depth, sgm::EXECUTE_INOUT_CUDA2CUDA);
+
+        device_buffer d_I1(src_bytes), d_I2(src_bytes), d_disparity(dst_bytes);
+        cv::Mat disparity(height, width, dst_depth == 8 ? CV_8S : CV_16S), disparity_color;
+
+        const int invalid_disp = sgm.get_invalid_disparity();
+
+        d_I1.upload(I1.data);
+        d_I2.upload(I2.data);
+
+        const auto t1 = std::chrono::system_clock::now();
+
+        sgm.execute(d_I1.data, d_I2.data, d_disparity.data);
+        cudaDeviceSynchronize();
+
+        const auto t2 = std::chrono::system_clock::now();
+        const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
+        const double fps = 1e6 / duration;
+
+        d_disparity.download(disparity.data);
+        cv::imwrite(cv::format("disparity_output_%04d.png", frame_no), disparity);
+
+        // Save disparity map as text file with pixel values
+        //std::ofstream disparity_file(cv::format("disparity_output_%04d.txt", frame_no));
+        //if (disparity_file.is_open()) {
+         //   for (int y = 0; y < disparity.rows; ++y) {
+          //      for (int x = 0; x < disparity.cols; ++x) {
+           //         disparity_file << disparity.at<short>(y, x) << " "; // Assuming disparity is CV_16S
+           //     }
+            //    disparity_file << std::endl;
+          //  }
+           // disparity_file.close();
+        //} else {
+          //  std::cerr << "Error: Could not open text file for disparity output." << std::endl;
+        //}
+        
+
+        // Print the size of the disparity map in MB
+        double disparity_size_mb = static_cast<double>(dst_bytes) / (1024 * 1024);
+        std::cout << "Size of disparity map: " << disparity_size_mb << " MB" << std::endl;
+
+        // Draw results
+        if (I1.type() != CV_8U)
+            cv::normalize(I1, I1, 0, 255, cv::NORM_MINMAX, CV_8U);
+
+        colorize_disparity(disparity, disparity_color, disp_size, disparity == invalid_disp);
+        cv::putText(disparity_color, cv::format("sgm execution time: %4.1f[msec] %4.1f[FPS]",
+            1e-3 * duration, fps), cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));
+
+        cv::imshow("left image", I1);
+        cv::imshow("disparity", disparity_color);
+
+        cv::waitKey(0); // Hold the window open for inspection; press any key to continue
+    }
+
+    return 0;
+}

05_disparity/libsgm/sample/sample_common.cpp

@@ -0,0 +1,29 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include "sample_common.h"
+
+#include <opencv2/imgproc.hpp>
+
+void colorize_disparity(const cv::Mat& src, cv::Mat& dst, int disp_size, cv::InputArray mask)
+{
+	cv::Mat tmp;
+	src.convertTo(tmp, CV_8U, 255. / disp_size);
+	cv::applyColorMap(tmp, dst, cv::COLORMAP_TURBO);
+
+	if (!mask.empty())
+		dst.setTo(0, mask);
+}

05_disparity/libsgm/sample/sample_common.h

@@ -0,0 +1,45 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#ifndef __SAMPLE_COMMON_H__
+#define __SAMPLE_COMMON_H__
+
+#include <opencv2/core.hpp>
+#include <cuda_runtime.h>
+
+#define ASSERT_MSG(expr, msg) \
+if (!(expr)) { \
+	std::cerr << msg << std::endl; \
+	std::exit(EXIT_FAILURE); \
+} \
+
+struct device_buffer
+{
+	device_buffer() : data(nullptr), size(0) {}
+	device_buffer(size_t count) : device_buffer() { allocate(count); }
+	~device_buffer() { cudaFree(data); }
+
+	void allocate(size_t count) { cudaMalloc(&data, count); size = count; }
+	void upload(const void* h_data) { cudaMemcpy(data, h_data, size, cudaMemcpyHostToDevice); }
+	void download(void* h_data) { cudaMemcpy(h_data, data, size, cudaMemcpyDeviceToHost); }
+
+	void* data;
+	size_t size;
+};
+
+void colorize_disparity(const cv::Mat& src, cv::Mat& dst, int disp_size, cv::InputArray mask = cv::noArray());
+
+#endif // !__SAMPLE_COMMON_H__

05_disparity/libsgm/sample/stereosgm_benchmark.cpp

@@ -0,0 +1,140 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+#include <iomanip>
+#include <chrono>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <libsgm.h>
+
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left_img   | <none> | path to input left image                                       }"
+"{ @right_img  | <none> | path to input right image                                      }"
+"{ disp_size   |    128 | maximum possible disparity value                               }"
+"{ out_depth   |      8 | disparity image's bits per pixel                               }"
+"{ subpixel    |        | enable subpixel estimation                                     }"
+"{ num_paths   |      8 | number of scanlines used in cost aggregation                   }"
+"{ census_type |      1 | type of census transform (0:CENSUS_9x7 1:SYMMETRIC_CENSUS_9x7) }"
+"{ iterations  |    100 | number of iterations for measuring performance                 }"
+"{ help h      |        | display this help and exit                                     }";
+
+int main(int argc, char* argv[])
+{
+	cv::CommandLineParser parser(argc, argv, keys);
+	if (parser.has("help")) {
+		parser.printMessage();
+		return 0;
+	}
+
+	cv::Mat I1 = cv::imread(parser.get<cv::String>("@left_img"), cv::IMREAD_UNCHANGED);
+	cv::Mat I2 = cv::imread(parser.get<cv::String>("@right_img"), cv::IMREAD_UNCHANGED);
+
+	const int disp_size = parser.get<int>("disp_size");
+	const int dst_depth = parser.get<int>("out_depth");
+	const bool subpixel = parser.has("subpixel");
+	const int num_paths = parser.get<int>("num_paths");
+	const auto census_type = static_cast<sgm::CensusType>(parser.get<int>("census_type"));
+	const int iterations = parser.get<int>("iterations");
+
+	if (!parser.check()) {
+		parser.printErrors();
+		parser.printMessage();
+		std::exit(EXIT_FAILURE);
+	}
+
+	ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
+	ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
+	ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
+	ASSERT_MSG(disp_size == 64 || disp_size == 128 || disp_size == 256, "disparity size must be 64, 128 or 256.");
+	ASSERT_MSG(num_paths == 4 || num_paths == 8, "number of scanlines must be 4 or 8.");
+	ASSERT_MSG(census_type == sgm::CensusType::CENSUS_9x7 || census_type == sgm::CensusType::SYMMETRIC_CENSUS_9x7, "census type must be 0 or 1.");
+	ASSERT_MSG(dst_depth == 8 || dst_depth == 16, "output depth bits must be 8 or 16");
+	if (subpixel)
+		ASSERT_MSG(dst_depth == 16, "output depth bits must be 16 if subpixel option is enabled.");
+
+	const int width = I1.cols;
+	const int height = I1.rows;
+
+	const int src_depth = I1.type() == CV_8U ? 8 : 16;
+	const int src_bytes = src_depth * width * height / 8;
+	const int dst_bytes = dst_depth * width * height / 8;
+	const sgm::PathType path_type = num_paths == 8 ? sgm::PathType::SCAN_8PATH : sgm::PathType::SCAN_4PATH;
+
+	const sgm::StereoSGM::Parameters param(10, 120, 0.95f, subpixel, path_type, 0, 1, census_type);
+	sgm::StereoSGM sgm(width, height, disp_size, src_depth, dst_depth, sgm::EXECUTE_INOUT_CUDA2CUDA, param);
+
+	device_buffer d_I1(src_bytes), d_I2(src_bytes), d_disparity(dst_bytes);
+	cv::Mat disparity(height, width, dst_depth == 8 ? CV_8S : CV_16S);
+
+	d_I1.upload(I1.data);
+	d_I2.upload(I2.data);
+
+	cudaDeviceProp prop;
+	int version;
+	cudaGetDeviceProperties(&prop, 0);
+	cudaRuntimeGetVersion(&version);
+
+	// show settings
+	std::cout << "# Settings" << std::endl;
+	std::cout << "device name         : " << prop.name << std::endl;
+	std::cout << "CUDA runtime version: " << version << std::endl;
+	std::cout << "image size          : " << I1.size() << std::endl;
+	std::cout << "disparity size      : " << disp_size << std::endl;
+	std::cout << "output depth        : " << dst_depth << std::endl;
+	std::cout << "subpixel option     : " << (subpixel ? "true" : "false") << std::endl;
+	std::cout << "sgm path            : " << num_paths << " path" << std::endl;
+	std::cout << "census type         : " << (census_type == sgm::CensusType::CENSUS_9x7 ? "CENSUS_9x7" : "SYMMETRIC_CENSUS_9x7") << std::endl;
+	std::cout << "iterations          : " << iterations << std::endl;
+	std::cout << std::endl;
+
+	// run benchmark
+	std::cout << "Running benchmark..." << std::endl;
+	uint64_t sum = 0;
+	for (int i = 0; i <= iterations; i++) {
+		const auto t1 = std::chrono::system_clock::now();
+
+		sgm.execute(d_I1.data, d_I2.data, d_disparity.data);
+		cudaDeviceSynchronize();
+
+		const auto t2 = std::chrono::system_clock::now();
+		if (i > 0)
+			sum += std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
+	}
+	std::cout << "Done." << std::endl << std::endl;
+
+	// show results
+	const double time_millisec = 1e-3 * sum / iterations;
+	const double fps = 1e3 / time_millisec;
+	std::cout << "# Results" << std::endl;
+	std::cout.setf(std::ios::fixed);
+	std::cout << std::setprecision(1) << "Processing Time[Milliseconds]: " << time_millisec << std::endl;
+	std::cout << std::setprecision(1) << "FPS                          : " << fps << std::endl;
+	std::cout << std::endl;
+
+	// save disparity image
+	const int disp_scale = subpixel ? sgm::StereoSGM::SUBPIXEL_SCALE : 1;
+	d_disparity.download(disparity.data);
+	colorize_disparity(disparity, disparity, disp_scale * disp_size, disparity == sgm.get_invalid_disparity());
+	cv::imwrite("disparity.png", disparity);
+
+	return 0;
+}

05_disparity/libsgm/sample/stereosgm_image.cpp

@@ -0,0 +1,118 @@
+#include <iostream>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/core/utils/filesystem.hpp>
+#include <libsgm.h>
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left-image-format  | <none> | format string for path to input left image  }"
+"{ @right-image-format | <none> | format string for path to input right image }"
+"{ disp_size           |    256 | maximum possible disparity value            }"
+"{ P1                  |     10 | penalty on the disparity change by plus or minus 1 }"
+"{ P2                  |    120 | penalty on the disparity change by more than 1 }"
+"{ uniqueness          |   0.80 | margin in ratio for best cost function value }"
+"{ num_paths           |      8 | number of scanlines used in cost aggregation }"
+"{ min_disp            |   -160 | minimum disparity value                     }"
+"{ LR_max_diff         |      1 | max allowed difference between L/R disparity }"
+"{ census_type         |      1 | type of census transform                    }"
+"{ interval            |      1 | polling interval in seconds                 }"
+"{ help h              |        | display this help and exit                  }";
+
+class ImagePreprocessor {
+public:
+    void preprocess_image_pair(cv::Mat& img_left, cv::Mat& img_right) {
+        if (img_left.channels() > 1) cv::cvtColor(img_left, img_left, cv::COLOR_BGR2GRAY);
+        if (img_right.channels() > 1) cv::cvtColor(img_right, img_right, cv::COLOR_BGR2GRAY);
+
+        int min_height = std::min(img_left.rows, img_right.rows);
+        int min_width = std::min(img_left.cols, img_right.cols);
+        img_left = img_left(cv::Rect(0, 0, min_width, min_height));
+        img_right = img_right(cv::Rect(0, 0, min_width, min_height));
+    }
+};
+
+bool disparityAlreadyProcessed(int frame_no) {
+    std::string xml_path = cv::format("output/disparity_%04d.xml", frame_no);
+    return cv::utils::fs::exists(xml_path);
+}
+
+int main(int argc, char* argv[]) {
+    cv::CommandLineParser parser(argc, argv, keys);
+    if (parser.has("help")) {
+        parser.printMessage();
+        return 0;
+    }
+
+    const std::string format_L = parser.get<cv::String>("@left-image-format");
+    const std::string format_R = parser.get<cv::String>("@right-image-format");
+    const int disp_size = parser.get<int>("disp_size");
+    const int P1 = parser.get<int>("P1");
+    const int P2 = parser.get<int>("P2");
+    const float uniqueness = parser.get<float>("uniqueness");
+    const int num_paths = parser.get<int>("num_paths");
+    const int min_disp = parser.get<int>("min_disp");
+    const int LR_max_diff = parser.get<int>("LR_max_diff");
+    const int interval = parser.get<int>("interval");
+    const auto census_type = static_cast<sgm::CensusType>(parser.get<int>("census_type"));
+
+    if (!parser.check()) {
+        parser.printErrors();
+        parser.printMessage();
+        std::exit(EXIT_FAILURE);
+    }
+
+    if (!cv::utils::fs::exists("output")) {
+        cv::utils::fs::createDirectory("output");
+    }
+
+    ImagePreprocessor preprocessor;
+    const sgm::PathType path_type = num_paths == 8 ? sgm::PathType::SCAN_8PATH : sgm::PathType::SCAN_4PATH;
+    const sgm::StereoSGM::Parameters param(P1, P2, uniqueness, false, path_type, min_disp, LR_max_diff, census_type);
+
+    int last_checked = 0;
+    while (true) {
+        const std::string left_path = cv::format(format_L.c_str(), last_checked);
+        const std::string right_path = cv::format(format_R.c_str(), last_checked);
+
+        if (cv::utils::fs::exists(left_path) && cv::utils::fs::exists(right_path) && !disparityAlreadyProcessed(last_checked)) {
+            cv::TickMeter timer;
+            timer.start();
+            std::cout << "Processing frame " << last_checked;
+
+            cv::Mat I1 = cv::imread(left_path, cv::IMREAD_UNCHANGED);
+            cv::Mat I2 = cv::imread(right_path, cv::IMREAD_UNCHANGED);
+
+            if (I1.empty() || I2.empty()) {
+                std::cerr << "Error reading images." << std::endl;
+                break;
+            }
+
+            preprocessor.preprocess_image_pair(I1, I2);
+
+            ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "Mismatched image size/type.");
+            ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "Images must be CV_8U or CV_16U.");
+
+            const int src_depth = I1.type() == CV_8U ? 8 : 16;
+            const int dst_depth = 16;
+
+            sgm::StereoSGM ssgm(I1.cols, I1.rows, disp_size, src_depth, dst_depth, sgm::EXECUTE_INOUT_HOST2HOST, param);
+            cv::Mat disparity(I1.size(), CV_16S);
+            ssgm.execute(I1.data, I2.data, disparity.data);
+
+            cv::FileStorage fs(cv::format("output/disparity_%04d.xml", last_checked), cv::FileStorage::WRITE);
+            fs << "disparity" << disparity;
+            fs.release();
+
+            timer.stop();
+            std::cout << " - " << timer.getTimeSec() << " seconds" << std::endl;
+        }
+
+        last_checked++;
+        cv::waitKey(interval * 1000);  // Sleep for polling interval
+    }
+
+    return 0;
+}
+

05_disparity/libsgm/sample/stereosgm_image_cv_gpumat.cpp

@@ -0,0 +1,120 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <libsgm.h>
+
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left_img   | <none> | path to input left image                                                            }"
+"{ @right_img  | <none> | path to input right image                                                           }"
+"{ disp_size   |     64 | maximum possible disparity value                                                    }"
+"{ P1          |     10 | penalty on the disparity change by plus or minus 1 between nieghbor pixels          }"
+"{ P2          |    120 | penalty on the disparity change by more than 1 between neighbor pixels              }"
+"{ uniqueness  |   0.95 | margin in ratio by which the best cost function value should be at least second one }"
+"{ num_paths   |      8 | number of scanlines used in cost aggregation                                        }"
+"{ min_disp    |      0 | minimum disparity value                                                             }"
+"{ LR_max_diff |      1 | maximum allowed difference between left and right disparity                         }"
+"{ census_type |      1 | type of census transform (0:CENSUS_9x7 1:SYMMETRIC_CENSUS_9x7)                      }"
+"{ help h      |        | display this help and exit                                                          }";
+
+int main(int argc, char* argv[])
+{
+	cv::CommandLineParser parser(argc, argv, keys);
+	if (parser.has("help")) {
+		parser.printMessage();
+		return 0;
+	}
+
+	cv::Mat I1 = cv::imread(parser.get<cv::String>("@left_img"), cv::IMREAD_UNCHANGED);
+	cv::Mat I2 = cv::imread(parser.get<cv::String>("@right_img"), cv::IMREAD_UNCHANGED);
+
+	const int disp_size = parser.get<int>("disp_size");
+	const int P1 = parser.get<int>("P1");
+	const int P2 = parser.get<int>("P2");
+	const float uniqueness = parser.get<float>("uniqueness");
+	const int num_paths = parser.get<int>("num_paths");
+	const int min_disp = parser.get<int>("min_disp");
+	const int LR_max_diff = parser.get<int>("LR_max_diff");
+	const auto census_type = static_cast<sgm::CensusType>(parser.get<int>("census_type"));
+
+	if (!parser.check()) {
+		parser.printErrors();
+		parser.printMessage();
+		std::exit(EXIT_FAILURE);
+	}
+
+	ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
+	ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
+	ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
+	ASSERT_MSG(disp_size == 64 || disp_size == 128 || disp_size == 256, "disparity size must be 64, 128 or 256.");
+	ASSERT_MSG(num_paths == 4 || num_paths == 8, "number of scanlines must be 4 or 8.");
+	ASSERT_MSG(census_type == sgm::CensusType::CENSUS_9x7 || census_type == sgm::CensusType::SYMMETRIC_CENSUS_9x7, "census type must be 0 or 1.");
+
+	const sgm::PathType path_type = num_paths == 8 ? sgm::PathType::SCAN_8PATH : sgm::PathType::SCAN_4PATH;
+	sgm::LibSGMWrapper sgm(disp_size, P1, P2, uniqueness, false, path_type, min_disp, LR_max_diff, census_type);
+	cv::Mat disparity;
+
+	try {
+		cv::cuda::GpuMat d_I1(I1), d_I2(I2), d_disparity;
+		sgm.execute(d_I1, d_I2, d_disparity);
+		d_disparity.download(disparity);
+	}
+	catch (const cv::Exception& e) {
+		std::cerr << e.what() << std::endl;
+		return e.code == cv::Error::GpuNotSupported ? 1 : -1;
+	}
+
+	// create mask for invalid disp
+	const cv::Mat mask = disparity == sgm.getInvalidDisparity();
+
+	// show image
+	cv::Mat disparity_8u, disparity_color;
+	disparity.convertTo(disparity_8u, CV_8U, 255. / disp_size);
+	cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_TURBO);
+	disparity_8u.setTo(0, mask);
+	disparity_color.setTo(cv::Scalar::all(0), mask);
+	if (I1.type() != CV_8U)
+		cv::normalize(I1, I1, 0, 255, cv::NORM_MINMAX, CV_8U);
+
+	const std::vector<cv::Mat> images = { disparity_8u, disparity_color, I1 };
+	const std::vector<std::string> titles = { "disparity", "disparity color", "input" };
+
+	std::cout << "Hot keys:" << std::endl;
+	std::cout << "\tESC - quit the program" << std::endl;
+	std::cout << "\ts - switch display (disparity | colored disparity | input image)" << std::endl;
+
+	int mode = 0;
+	while (true) {
+
+		cv::setWindowTitle("image", titles[mode]);
+		cv::imshow("image", images[mode]);
+
+		const char c = cv::waitKey(0);
+		if (c == 's')
+			mode = (mode < 2 ? mode + 1 : 0);
+		if (c == 27)
+			break;
+	}
+
+	return 0;
+}

05_disparity/libsgm/sample/stereosgm_movie.cpp

@@ -0,0 +1,121 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+#include <chrono>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <libsgm.h>
+
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left-image-format  | <none> | format string for path to input left image  }"
+"{ @right-image-format | <none> | format string for path to input right image }"
+"{ disp_size           |    128 | maximum possible disparity value            }"
+"{ start_number        |      0 | index to start reading                      }"
+"{ help h              |        | display this help and exit                  }";
+
+int main(int argc, char* argv[])
+{
+	cv::CommandLineParser parser(argc, argv, keys);
+	if (parser.has("help")) {
+		parser.printMessage();
+		return 0;
+	}
+
+	const std::string image_format_L = parser.get<cv::String>("@left-image-format");
+	const std::string image_format_R = parser.get<cv::String>("@right-image-format");
+	const int disp_size = parser.get<int>("disp_size");
+	const int start_number = parser.get<int>("start_number");
+
+	if (!parser.check()) {
+		parser.printErrors();
+		parser.printMessage();
+		std::exit(EXIT_FAILURE);
+	}
+
+	cv::Mat I1 = cv::imread(cv::format(image_format_L.c_str(), start_number), cv::IMREAD_UNCHANGED);
+	cv::Mat I2 = cv::imread(cv::format(image_format_R.c_str(), start_number), cv::IMREAD_UNCHANGED);
+
+	ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
+	if (I1.channels() > 1) cv::cvtColor(I1, I1, cv::COLOR_BGR2GRAY);
+	if (I2.channels() > 1) cv::cvtColor(I2, I2, cv::COLOR_BGR2GRAY);
+	ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
+	ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
+	ASSERT_MSG(disp_size == 64 || disp_size == 128 || disp_size == 256, "disparity size must be 64, 128 or 256.");
+
+	const int width = I1.cols;
+	const int height = I1.rows;
+
+	const int src_depth = I1.type() == CV_8U ? 8 : 16;
+	const int dst_depth = disp_size < 256 ? 8 : 16;
+	const int src_bytes = src_depth * width * height / 8;
+	const int dst_bytes = dst_depth * width * height / 8;
+
+	sgm::StereoSGM sgm(width, height, disp_size, src_depth, dst_depth, sgm::EXECUTE_INOUT_CUDA2CUDA);
+
+	device_buffer d_I1(src_bytes), d_I2(src_bytes), d_disparity(dst_bytes);
+	cv::Mat disparity(height, width, dst_depth == 8 ? CV_8S : CV_16S), disparity_color;
+
+	const int invalid_disp = sgm.get_invalid_disparity();
+
+	for (int frame_no = start_number;; frame_no++) {
+
+		I1 = cv::imread(cv::format(image_format_L.c_str(), frame_no), cv::IMREAD_UNCHANGED);
+		I2 = cv::imread(cv::format(image_format_R.c_str(), frame_no), cv::IMREAD_UNCHANGED);
+		if (I1.empty() || I2.empty()) {
+			frame_no = start_number - 1;
+			continue;
+		}
+		if (I1.channels() > 1) cv::cvtColor(I1, I1, cv::COLOR_BGR2GRAY);
+		if (I2.channels() > 1) cv::cvtColor(I2, I2, cv::COLOR_BGR2GRAY);
+
+		d_I1.upload(I1.data);
+		d_I2.upload(I2.data);
+
+		const auto t1 = std::chrono::system_clock::now();
+
+		sgm.execute(d_I1.data, d_I2.data, d_disparity.data);
+		cudaDeviceSynchronize();
+
+		const auto t2 = std::chrono::system_clock::now();
+		const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
+		const double fps = 1e6 / duration;
+
+		d_disparity.download(disparity.data);
+
+		// draw results
+		if (I1.type() != CV_8U)
+			cv::normalize(I1, I1, 0, 255, cv::NORM_MINMAX, CV_8U);
+
+		colorize_disparity(disparity, disparity_color, disp_size, disparity == invalid_disp);
+		cv::putText(disparity_color, cv::format("sgm execution time: %4.1f[msec] %4.1f[FPS]",
+			1e-3 * duration, fps), cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));
+
+		cv::imshow("left image", I1);
+		cv::imshow("disparity", disparity_color);
+
+		const char c = cv::waitKey(1);
+		if (c == 27) // ESC
+			break;
+	}
+
+	return 0;
+}

05_disparity/libsgm/sample/stereosgm_new.cpp

@@ -0,0 +1,124 @@
+#include <iostream>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <libsgm.h>
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left_img   | <none> | path to input left image                                                            }"
+"{ @right_img  | <none> | path to input right image                                                           }"
+"{ disp_size   |    256 | maximum possible disparity value                                                    }"
+"{ P1          |     10 | penalty on the disparity change by plus or minus 1 between neighbor pixels          }"
+"{ P2          |    120 | penalty on the disparity change by more than 1 between neighbor pixels              }"
+"{ uniqueness  |   0.80 | margin in ratio by which the best cost function value should be at least second one }"
+"{ num_paths   |      8 | number of scanlines used in cost aggregation                                        }"
+"{ min_disp    |   -160 | minimum disparity value                                                             }"
+"{ LR_max_diff |      1 | maximum allowed difference between left and right disparity                         }"
+"{ census_type |      1 | type of census transform (0:CENSUS_9x7 1:SYMMETRIC_CENSUS_9x7)                      }"
+"{ output_dir  |      . | directory to save disparity.xml and disparity_color.png                             }"
+"{ no_display  |      0 | set to 1 to skip interactive display window (for pipeline/headless use)             }"
+"{ help h      |        | display this help and exit                                                          }";
+
+int main(int argc, char* argv[])
+{
+    double start_time = cv::getTickCount();  // Start total execution time
+
+    cv::CommandLineParser parser(argc, argv, keys);
+    if (parser.has("help")) {
+        parser.printMessage();
+        return 0;
+    }
+
+    double load_start = cv::getTickCount(); // Start loading time
+    cv::Mat I1 = cv::imread(parser.get<cv::String>("@left_img"), cv::IMREAD_UNCHANGED);
+    cv::Mat I2 = cv::imread(parser.get<cv::String>("@right_img"), cv::IMREAD_UNCHANGED);
+    double load_end = cv::getTickCount();
+    double load_time_s = (load_end - load_start) / cv::getTickFrequency();  // Seconds
+    double load_time_ms = load_time_s * 1000.0;  // Milliseconds
+    std::cout << "Image Loading Time: " << load_time_s << " s (" << load_time_ms << " ms)" << std::endl;
+
+    if (I1.channels() > 1) cv::cvtColor(I1, I1, cv::COLOR_BGR2GRAY);
+    if (I2.channels() > 1) cv::cvtColor(I2, I2, cv::COLOR_BGR2GRAY);
+
+    const int disp_size = parser.get<int>("disp_size");
+    const int P1 = parser.get<int>("P1");
+    const int P2 = parser.get<int>("P2");
+    const float uniqueness = parser.get<float>("uniqueness");
+    const int num_paths = parser.get<int>("num_paths");
+    const int min_disp = parser.get<int>("min_disp");
+    const int LR_max_diff = parser.get<int>("LR_max_diff");
+    const auto census_type = static_cast<sgm::CensusType>(parser.get<int>("census_type"));
+
+    if (!parser.check()) {
+        parser.printErrors();
+        parser.printMessage();
+        std::exit(EXIT_FAILURE);
+    }
+
+    ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
+    ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
+    ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
+    ASSERT_MSG(disp_size == 64 || disp_size == 128 || disp_size == 256, "disparity size must be 64, 128 or 256.");
+    ASSERT_MSG(num_paths == 4 || num_paths == 8, "number of scanlines must be 4 or 8.");
+    ASSERT_MSG(census_type == sgm::CensusType::CENSUS_9x7 || census_type == sgm::CensusType::SYMMETRIC_CENSUS_9x7, "census type must be 0 or 1.");
+
+    const int src_depth = I1.type() == CV_8U ? 8 : 16;
+    const int dst_depth = 16;
+    const sgm::PathType path_type = num_paths == 8 ? sgm::PathType::SCAN_8PATH : sgm::PathType::SCAN_4PATH;
+
+    const sgm::StereoSGM::Parameters param(P1, P2, uniqueness, false, path_type, min_disp, LR_max_diff, census_type);
+    sgm::StereoSGM ssgm(I1.cols, I1.rows, disp_size, src_depth, dst_depth, sgm::EXECUTE_INOUT_HOST2HOST, param);
+
+    cv::Mat disparity(I1.size(), CV_16S);
+
+    double disparity_start = cv::getTickCount(); // Start disparity computation time
+    ssgm.execute(I1.data, I2.data, disparity.data);
+    double disparity_end = cv::getTickCount();
+    double disparity_time_s = (disparity_end - disparity_start) / cv::getTickFrequency();  // Seconds
+    double disparity_time_ms = disparity_time_s * 1000.0;  // Milliseconds
+    std::cout << "Disparity Computation Time: " << disparity_time_s << " s (" << disparity_time_ms << " ms)" << std::endl;
+
+    const std::string output_dir = parser.get<std::string>("output_dir");
+
+    // Save disparity
+    cv::FileStorage fs(output_dir + "/disparity.xml", cv::FileStorage::WRITE);
+    fs << "disparity" << disparity;
+    fs.release();
+
+    // Convert disparity to 8-bit for visualization
+    cv::Mat disparity_8u, disparity_color;
+    disparity.convertTo(disparity_8u, CV_8U, 255.0 / disp_size);
+    cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_TURBO);
+
+    // Save colored disparity image
+    cv::imwrite(output_dir + "/disparity_color.png", disparity_color);
+
+    double total_end = cv::getTickCount();
+    double total_time_s = (total_end - start_time) / cv::getTickFrequency();  // Seconds
+    double total_time_ms = total_time_s * 1000.0;  // Milliseconds
+    std::cout << "Total Execution Time: " << total_time_s << " s (" << total_time_ms << " ms)" << std::endl;
+
+    // Display images
+    const std::vector<cv::Mat> images = { disparity_8u, disparity_color, I1 };
+    const std::vector<std::string> titles = { "Disparity", "Colored Disparity", "Input Image" };
+
+    if (!parser.get<int>("no_display")) {
+        std::cout << "Hot keys:\n";
+        std::cout << "\tESC - Quit the program\n";
+        std::cout << "\ts - Switch display (Disparity | Colored Disparity | Input Image)\n";
+
+        int mode = 0;
+        while (true) {
+            cv::setWindowTitle("Image", titles[mode]);
+            cv::imshow("Image", images[mode]);
+
+            const char c = cv::waitKey(0);
+            if (c == 's') mode = (mode < 2 ? mode + 1 : 0);
+            if (c == 27) break;
+        }
+    }
+
+    return 0;
+}
+

05_disparity/libsgm/sample/stereosgm_our.cpp

@@ -0,0 +1,120 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+#include <libsgm.h>
+#include "sample_common.h"
+
+static const std::string keys =
+"{ @left_img   | <none> | path to input left image                                                            }"
+"{ @right_img  | <none> | path to input right image                                                           }"
+"{ disp_size   |     64 | maximum possible disparity value                                                    }"
+"{ P1          |     10 | penalty on the disparity change by plus or minus 1 between neighbor pixels          }"
+"{ P2          |    120 | penalty on the disparity change by more than 1 between neighbor pixels              }"
+"{ uniqueness  |   0.95 | margin in ratio by which the best cost function value should be at least second one }"
+"{ num_paths   |      8 | number of scanlines used in cost aggregation                                        }"
+"{ min_disp    |      0 | minimum disparity value                                                             }"
+"{ LR_max_diff |      1 | maximum allowed difference between left and right disparity                         }"
+"{ census_type |      1 | type of census transform (0:CENSUS_9x7 1:SYMMETRIC_CENSUS_9x7)                      }"
+"{ help h      |        | display this help and exit                                                          }";
+
+int main(int argc, char* argv[]) {
+    cv::CommandLineParser parser(argc, argv, keys);
+    if (parser.has("help")) {
+        parser.printMessage();
+        return 0;
+    }
+
+    cv::Mat I1 = cv::imread(parser.get<cv::String>("@left_img"), cv::IMREAD_UNCHANGED);
+    cv::Mat I2 = cv::imread(parser.get<cv::String>("@right_img"), cv::IMREAD_UNCHANGED);
+
+    // Preprocessing: Convert images to grayscale if necessary
+    if (I1.channels() > 1) cv::cvtColor(I1, I1, cv::COLOR_BGR2GRAY);
+    if (I2.channels() > 1) cv::cvtColor(I2, I2, cv::COLOR_BGR2GRAY);
+
+    // Ensure images have the same size by cropping
+    int new_width = std::min(I1.cols, I2.cols);
+    int new_height = std::min(I1.rows, I2.rows);
+    I1 = I1(cv::Rect(0, 0, new_width, new_height));
+    I2 = I2(cv::Rect(0, 0, new_width, new_height));
+
+    const int disp_size = parser.get<int>("disp_size");
+    const int P1 = parser.get<int>("P1");
+    const int P2 = parser.get<int>("P2");
+    const float uniqueness = parser.get<float>("uniqueness");
+    const int num_paths = parser.get<int>("num_paths");
+    const int min_disp = parser.get<int>("min_disp");
+    const int LR_max_diff = parser.get<int>("LR_max_diff");
+    const auto census_type = static_cast<sgm::CensusType>(parser.get<int>("census_type"));
+
+    if (!parser.check()) {
+        parser.printErrors();
+        parser.printMessage();
+        std::exit(EXIT_FAILURE);
+    }
+
+    ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
+    ASSERT_MSG(I1.size() == I2.size(), "input images must be the same size.");
+    ASSERT_MSG(I1.type() == CV_8U, "input image format must be CV_8U.");
+    ASSERT_MSG(disp_size == 64 || disp_size == 128 || disp_size == 256, "disparity size must be 64, 128 or 256.");
+    ASSERT_MSG(num_paths == 4 || num_paths == 8, "number of scanlines must be 4 or 8.");
+
+    const sgm::StereoSGM::Parameters param(P1, P2, uniqueness, false, sgm::PathType::SCAN_8PATH, min_disp, LR_max_diff, census_type);
+    sgm::StereoSGM ssgm(I1.cols, I1.rows, disp_size, 8, 16, sgm::EXECUTE_INOUT_HOST2HOST, param);
+
+    cv::Mat disparity(I1.size(), CV_16S);
+    ssgm.execute(I1.data, I2.data, disparity.data);
+
+    // Convert disparity to 8-bit and apply colormap
+    cv::Mat disparity_8u, disparity_color;
+    disparity.convertTo(disparity_8u, CV_8U, 255. / disp_size);
+    cv::applyColorMap(disparity_8u, disparity_color, cv::COLORMAP_TURBO);
+
+    // Save disparity map
+    cv::imwrite("disparity_map.png", disparity_8u);
+
+    // Optionally save disparity values as a text file
+    std::ofstream file("disparity_values.txt");
+    if (file.is_open()) {
+        for (int i = 0; i < disparity.rows; ++i) {
+            for (int j = 0; j < disparity.cols; ++j) {
+                file << static_cast<int>(disparity.at<int16_t>(i, j)) << " ";
+            }
+            file << "\n";
+        }
+        file.close();
+    }
+
+    std::cout << "Hot keys:\n\tESC - quit the program\n\ts - switch display (disparity | colored disparity | input image)\n";
+
+    const std::vector<cv::Mat> images = { disparity_8u, disparity_color, I1 };
+    const std::vector<std::string> titles = { "disparity", "disparity color", "input" };
+
+    int mode = 0;
+    while (true) {
+        cv::setWindowTitle("image", titles[mode]);
+        cv::imshow("image", images[mode]);
+        char c = cv::waitKey(0);
+        if (c == 's') mode = (mode + 1) % 3;
+        if (c == 27) break;
+    }
+
+    return 0;
+}
+

05_disparity/libsgm/sample/stereosgm_reprojection.cpp

@@ -0,0 +1,253 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+#include <chrono>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <libsgm.h>
+
+#include "sample_common.h"
+
+// Camera Parameters
+struct CameraParameters
+{
+	float fu;                 //!< focal length x (pixel)
+	float fv;                 //!< focal length y (pixel)
+	float u0;                 //!< principal point x (pixel)
+	float v0;                 //!< principal point y (pixel)
+	float baseline;           //!< baseline (meter)
+	float height;             //!< height position (meter), ignored when ROAD_ESTIMATION_AUTO
+	float tilt;               //!< tilt angle (radian), ignored when ROAD_ESTIMATION_AUTO
+};
+
+// Transformation between pixel coordinate and world coordinate
+struct CoordinateTransform
+{
+	CoordinateTransform(const CameraParameters& camera) : camera(camera)
+	{
+		sinTilt = sinf(camera.tilt);
+		cosTilt = cosf(camera.tilt);
+		bf = camera.baseline * camera.fu;
+		invfu = 1.f / camera.fu;
+		invfv = 1.f / camera.fv;
+	}
+
+	inline cv::Point3f imageToWorld(const cv::Point2f& pt, float d) const
+	{
+		const float u = pt.x;
+		const float v = pt.y;
+
+		const float Zc = bf / d;
+		const float Xc = invfu * (u - camera.u0) * Zc;
+		const float Yc = invfv * (v - camera.v0) * Zc;
+
+		const float Xw = Xc;
+		const float Yw = Yc * cosTilt + Zc * sinTilt;
+		const float Zw = Zc * cosTilt - Yc * sinTilt;
+
+		return cv::Point3f(Xw, Yw, Zw);
+	}
+
+	CameraParameters camera;
+	float sinTilt, cosTilt, bf, invfu, invfv;
+};
+
+void reprojectPointsTo3D(const cv::Mat& disparity, const CameraParameters& camera, std::vector<cv::Point3f>& points, bool subpixeled)
+{
+	CV_Assert(disparity.type() == CV_32F);
+
+	CoordinateTransform tf(camera);
+
+	points.clear();
+	points.reserve(disparity.rows * disparity.cols);
+
+	for (int y = 0; y < disparity.rows; y++)
+	{
+		for (int x = 0; x < disparity.cols; x++)
+		{
+			const float d = disparity.at<float>(y, x);
+			if (d > 0)
+				points.push_back(tf.imageToWorld(cv::Point(x, y), d));
+		}
+	}
+}
+
+static cv::Vec3b computeColor(float val)
+{
+	const float hscale = 6.f;
+	float h = 0.6f * (1.f - val), s = 1.f, v = 1.f;
+
+	static const int sector_data[][3] =
+	{ { 1,3,0 },{ 1,0,2 },{ 3,0,1 },{ 0,2,1 },{ 0,1,3 },{ 2,1,0 } };
+	float tab[4];
+	int sector;
+	h *= hscale;
+	if (h < 0)
+		do h += 6; while (h < 0);
+	else if (h >= 6)
+		do h -= 6; while (h >= 6);
+	sector = cvFloor(h);
+	h -= sector;
+	if ((unsigned)sector >= 6u)
+	{
+		sector = 0;
+		h = 0.f;
+	}
+
+	tab[0] = v;
+	tab[1] = v * (1.f - s);
+	tab[2] = v * (1.f - s * h);
+	tab[3] = v * (1.f - s * (1.f - h));
+
+	const uchar b = (uchar)(255 * tab[sector_data[sector][0]]);
+	const uchar g = (uchar)(255 * tab[sector_data[sector][1]]);
+	const uchar r = (uchar)(255 * tab[sector_data[sector][2]]);
+	return cv::Vec3b(b, g, r);
+}
+
+void drawPoints3D(const std::vector<cv::Point3f>& points, cv::Mat& draw)
+{
+	const int SIZE_X = 512;
+	const int SIZE_Z = 1024;
+	const int maxz = 20; // [meter]
+	const double pixelsPerMeter = 1. * SIZE_Z / maxz;
+
+	draw = cv::Mat::zeros(SIZE_Z, SIZE_X, CV_8UC3);
+
+	const int tableSize = 256;
+	const float scaleZ = 1.f * (tableSize - 1) / maxz;
+	static std::vector<cv::Vec3b> colorTable;
+	if (colorTable.empty())
+	{
+		colorTable.resize(tableSize);
+		for (int i = 0; i < tableSize; i++)
+			colorTable[i] = computeColor(1.f * i / tableSize);
+	}
+
+	for (const cv::Point3f& pt : points)
+	{
+		const float X = pt.x;
+		const float Z = pt.z;
+
+		const int u = cvRound(pixelsPerMeter * X) + SIZE_X / 2;
+		const int v = SIZE_Z - cvRound(pixelsPerMeter * Z);
+
+		const auto& color = colorTable[cvRound(scaleZ * std::min(Z, 1.f * maxz))];
+		cv::circle(draw, cv::Point(u, v), 1, color);
+	}
+}
+
+int main(int argc, char* argv[])
+{
+	if (argc < 4) {
+		std::cout << "usage: " << argv[0] << " left-image-format right-image-format camera.xml [disp_size] [subpixel_enable(0: false, 1:true)]" << std::endl;
+		std::exit(EXIT_FAILURE);
+	}
+
+	const int start_number = 1;
+
+	cv::Mat I1 = cv::imread(cv::format(argv[1], start_number), cv::IMREAD_UNCHANGED);
+	cv::Mat I2 = cv::imread(cv::format(argv[2], start_number), cv::IMREAD_UNCHANGED);
+
+	const cv::FileStorage fs(argv[3], cv::FileStorage::READ);
+	const int disp_size = argc >= 5 ? std::stoi(argv[4]) : 128;
+	const bool subpixel = argc >= 6 ? std::stoi(argv[5]) != 0 : true;
+
+	ASSERT_MSG(!I1.empty() && !I2.empty(), "imread failed.");
+	ASSERT_MSG(fs.isOpened(), "camera.xml read failed.");
+	ASSERT_MSG(I1.size() == I2.size() && I1.type() == I2.type(), "input images must be same size and type.");
+	ASSERT_MSG(I1.type() == CV_8U || I1.type() == CV_16U, "input image format must be CV_8U or CV_16U.");
+	ASSERT_MSG(disp_size == 64 || disp_size == 128 || disp_size == 256, "disparity size must be 64, 128 or 256.");
+
+	// read camera parameters
+	CameraParameters camera;
+	camera.fu = fs["FocalLengthX"];
+	camera.fv = fs["FocalLengthY"];
+	camera.u0 = fs["CenterX"];
+	camera.v0 = fs["CenterY"];
+	camera.baseline = fs["BaseLine"];
+	camera.tilt = fs["Tilt"];
+
+	const int width = I1.cols;
+	const int height = I1.rows;
+
+	const int src_depth = I1.type() == CV_8U ? 8 : 16;
+	const int dst_depth = 16;
+	const int src_bytes = src_depth * width * height / 8;
+	const int dst_bytes = dst_depth * width * height / 8;
+
+	const sgm::StereoSGM::Parameters param(10, 120, 0.95f, subpixel);
+	sgm::StereoSGM sgm(width, height, disp_size, src_depth, dst_depth, sgm::EXECUTE_INOUT_CUDA2CUDA, param);
+
+	device_buffer d_I1(src_bytes), d_I2(src_bytes), d_disparity(dst_bytes);
+	cv::Mat disparity(height, width, dst_depth == 8 ? CV_8S : CV_16S), disparity_color, disparity_32f, draw;
+	std::vector<cv::Point3f> points;
+
+	const int invalid_disp = sgm.get_invalid_disparity();
+	const int disp_scale = subpixel ? sgm::StereoSGM::SUBPIXEL_SCALE : 1;
+
+	for (int frame_no = start_number;; frame_no++) {
+
+		I1 = cv::imread(cv::format(argv[1], frame_no), cv::IMREAD_UNCHANGED);
+		I2 = cv::imread(cv::format(argv[2], frame_no), cv::IMREAD_UNCHANGED);
+		if (I1.empty() || I2.empty()) {
+			frame_no = start_number - 1;
+			continue;
+		}
+
+		d_I1.upload(I1.data);
+		d_I2.upload(I2.data);
+
+		const auto t1 = std::chrono::system_clock::now();
+
+		sgm.execute(d_I1.data, d_I2.data, d_disparity.data);
+		cudaDeviceSynchronize();
+
+		const auto t2 = std::chrono::system_clock::now();
+		const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
+		const double fps = 1e6 / duration;
+
+		d_disparity.download(disparity.data);
+
+		// reproject points
+		disparity.convertTo(disparity_32f, CV_32F, 1. / disp_scale);
+		reprojectPointsTo3D(disparity_32f, camera, points, subpixel);
+
+		// draw results
+		if (I1.type() != CV_8U)
+			cv::normalize(I1, I1, 0, 255, cv::NORM_MINMAX, CV_8U);
+
+		colorize_disparity(disparity, disparity_color, disp_scale * disp_size, disparity == invalid_disp);
+		cv::putText(disparity_color, cv::format("sgm execution time: %4.1f[msec] %4.1f[FPS]",
+			1e-3 * duration, fps), cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));
+
+		drawPoints3D(points, draw);
+
+		cv::imshow("left image", I1);
+		cv::imshow("disparity", disparity_color);
+		cv::imshow("points", draw);
+
+		const char c = cv::waitKey(1);
+		if (c == 27) // ESC
+			break;
+	}
+
+	return 0;
+}

05_disparity/libsgm/sample/stereosgm_zed.cpp

@@ -0,0 +1,114 @@
+/*
+Copyright 2016 Fixstars Corporation
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+http ://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+*/
+
+#include <iostream>
+#include <chrono>
+
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <sl/Camera.hpp>
+
+#include <libsgm.h>
+
+#include "sample_common.h"
+
+static const std::string keys =
+"{ disp_size           |    128 | maximum possible disparity value                  }"
+"{ camera_resolution   |      3 | camera resolution (0:HD2K 1:HD1080 2:HD720 3:VGA) }"
+"{ help h              |        | display this help and exit                        }";
+
+int main(int argc, char* argv[])
+{
+	cv::CommandLineParser parser(argc, argv, keys);
+	if (parser.has("help")) {
+		parser.printMessage();
+		return 0;
+	}
+
+	const int disp_size = parser.get<int>("disp_size");
+	const sl::RESOLUTION camera_resolution = parser.get<sl::RESOLUTION>("camera_resolution");
+
+	sl::Camera zed;
+	sl::InitParameters initParameters;
+	initParameters.camera_resolution = camera_resolution;
+	const sl::ERROR_CODE err = zed.open(initParameters);
+	if (err != sl::ERROR_CODE::SUCCESS) {
+		std::cerr << sl::toString(err) << std::endl;
+		std::exit(EXIT_FAILURE);
+	}
+
+	const auto& resolution = zed.getCameraInformation().camera_configuration.resolution;
+	sl::Mat d_zed_image_L(resolution, sl::MAT_TYPE::U8_C1, sl::MEM::GPU);
+	sl::Mat d_zed_image_R(resolution, sl::MAT_TYPE::U8_C1, sl::MEM::GPU);
+	CV_Assert(d_zed_image_L.getStep(sl::MEM::GPU) == d_zed_image_R.getStep(sl::MEM::GPU));
+
+	const int width = resolution.width;
+	const int height = resolution.height;
+	const int src_pitch = static_cast<int>(d_zed_image_L.getStep(sl::MEM::GPU));
+	const int dst_pitch = width;
+
+	const int src_depth = 8;
+	const int dst_depth = disp_size < 256 ? 8 : 16;
+	const int src_bytes = src_depth * width * height / 8;
+	const int dst_bytes = dst_depth * width * height / 8;
+
+	sgm::StereoSGM sgm(width, height, disp_size, src_depth, dst_depth, src_pitch, dst_pitch, sgm::EXECUTE_INOUT_CUDA2CUDA);
+
+	device_buffer d_disparity(dst_bytes);
+	cv::Mat disparity(height, width, dst_depth == 8 ? CV_8S : CV_16S), disparity_color;
+
+	const int invalid_disp = sgm.get_invalid_disparity();
+
+	std::cout << "max disparity    : " << disp_size << std::endl;
+	std::cout << "camera resolution: " << sl::toString(initParameters.camera_resolution) << " " << cv::Size(width, height) << std::endl;
+
+	while (1) {
+
+		if (zed.grab() == sl::ERROR_CODE::SUCCESS) {
+			zed.retrieveImage(d_zed_image_L, sl::VIEW::LEFT_GRAY, sl::MEM::GPU);
+			zed.retrieveImage(d_zed_image_R, sl::VIEW::RIGHT_GRAY, sl::MEM::GPU);
+		}
+		else {
+			continue;
+		}
+
+		const auto t1 = std::chrono::system_clock::now();
+
+		sgm.execute(d_zed_image_L.getPtr<uchar>(sl::MEM::GPU), d_zed_image_R.getPtr<uchar>(sl::MEM::GPU), d_disparity.data);
+		cudaDeviceSynchronize();
+
+		const auto t2 = std::chrono::system_clock::now();
+		const auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count();
+		const double fps = 1e6 / duration;
+
+		d_disparity.download(disparity.data);
+
+		// draw results
+		colorize_disparity(disparity, disparity_color, disp_size, disparity == invalid_disp);
+		cv::putText(disparity_color, cv::format("sgm execution time: %4.1f[msec] %4.1f[FPS]",
+			1e-3 * duration, fps), cv::Point(50, 50), 2, 0.75, cv::Scalar(255, 255, 255));
+
+		cv::imshow("disparity", disparity_color);
+
+		const char c = cv::waitKey(1);
+		if (c == 27) // ESC
+			break;
+	}
+
+	return 0;
+}