diff --git a/.gitignore b/.gitignore index 8f8e405..b9bc9ef 100644 --- a/.gitignore +++ b/.gitignore @@ -44,6 +44,9 @@ install # IDE .idea +#macOS +.DS_Store + # Build dir cmake-build-debug cmake-build-release diff --git a/CMakeLists.txt b/CMakeLists.txt index 19125a5..13a131b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,6 +27,7 @@ set(DEPLEX_LIB_DIR ${CMAKE_BINARY_DIR}/lib) add_subdirectory(external) add_subdirectory(libs) add_subdirectory(cpp) +add_subdirectory(benchmark/cpp-benchmark) if (${BUILD_EXAMPLES}) add_subdirectory(examples) endif() \ No newline at end of file diff --git a/benchmark/cpp-benchmark/CMakeLists.txt b/benchmark/cpp-benchmark/CMakeLists.txt new file mode 100644 index 0000000..61fda88 --- /dev/null +++ b/benchmark/cpp-benchmark/CMakeLists.txt @@ -0,0 +1,15 @@ + target_compile_options(deplex PRIVATE -O3) + + ##################################### + # process_cloud.cpp + ##################################### + add_executable(benchmark_process_cloud benchmark_process_cloud.cpp) + target_compile_features(benchmark_process_cloud PRIVATE cxx_std_17) + target_link_libraries(benchmark_process_cloud PRIVATE ${PROJECT_NAME}) + + ##################################### + # process_sequence.cpp + ##################################### + add_executable(benchmark_process_sequence benchmark_process_sequence.cpp) + target_compile_features(benchmark_process_sequence PRIVATE cxx_std_17) + target_link_libraries(benchmark_process_sequence PRIVATE ${PROJECT_NAME}) \ No newline at end of file diff --git a/benchmark/cpp-benchmark/benchmark_process_cloud.cpp b/benchmark/cpp-benchmark/benchmark_process_cloud.cpp new file mode 100644 index 0000000..8e7aaf9 --- /dev/null +++ b/benchmark/cpp-benchmark/benchmark_process_cloud.cpp @@ -0,0 +1,156 @@ +#include +#include +#include + +#include +#include +#include +#include + +double variance(const Eigen::VectorXd& data, double mean) { + double sum = 0; + + for (auto x : data) { + sum += (x - mean) * (x - mean); + } + + sum /= static_cast(data.size()); + return sum; +} + +int main() { + std::filesystem::path data_dir = + std::filesystem::current_path().parent_path().parent_path().parent_path() / "benchmark/data"; + std::filesystem::path image_path = data_dir / "depth/000004415622.png"; + std::filesystem::path intrinsics_path = data_dir / "config/intrinsics.K"; + std::filesystem::path config_path = data_dir / "config/TUM_fr3_long_val.ini"; + + auto start_time = std::chrono::high_resolution_clock::now(); + auto end_time = std::chrono::high_resolution_clock::now(); + + const int NUMBER_OF_RUNS = 10; + + std::vector execution_time_stage(NUMBER_OF_RUNS, Eigen::VectorXd::Zero(3)); + + deplex::config::Config config = deplex::config::Config(config_path.string()); + Eigen::Matrix3f intrinsics(deplex::utils::readIntrinsics(intrinsics_path.string())); + deplex::utils::DepthImage image(image_path.string()); + + auto algorithm = deplex::PlaneExtractor(image.getHeight(), image.getWidth(), config); + Eigen::VectorXi labels; + + std::cout << "Image Height: " << image.getHeight() << " Image Width: " << image.getWidth() << "\n\n"; + + for (int i = 0; i < NUMBER_OF_RUNS; ++i) { + start_time = std::chrono::high_resolution_clock::now(); + image.reset(image_path.string()); + end_time = std::chrono::high_resolution_clock::now(); + + execution_time_stage[i][0] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); + + start_time = std::chrono::high_resolution_clock::now(); + auto pcd_array = image.toPointCloud(intrinsics); + end_time = std::chrono::high_resolution_clock::now(); + + execution_time_stage[i][1] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); + + start_time = std::chrono::high_resolution_clock::now(); + labels = algorithm.process(pcd_array); + end_time = std::chrono::high_resolution_clock::now(); + + execution_time_stage[i][2] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); + + std::cout << "Iteration #" << i + 1 << " Planes found: " << labels.maxCoeff() << std::endl; + } + + auto execution_time_segmentation_stage = algorithm.GetExecutionTime(); + + Eigen::VectorXd elements = Eigen::VectorXd::Zero(NUMBER_OF_RUNS); + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_stage[i][0]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_read_image.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_stage[i][1]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_translate_image.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_stage[i][2]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_segmentation.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_segmentation_stage[i][0]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_segmentation_cell_grid.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_segmentation_stage[i][1]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_segmentation_region_growing.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_segmentation_stage[i][2]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_segmentation_merge_planes.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + elements[i] = execution_time_segmentation_stage[i][3]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_cloud_stage_segmentation_labels.csv")).string()); + + Eigen::VectorXd total_time = Eigen::VectorXd::Zero(NUMBER_OF_RUNS); + + for (auto i = 0; i < NUMBER_OF_RUNS; ++i) { + total_time[i] = execution_time_stage[i][0] + execution_time_stage[i][1] + execution_time_stage[i][2]; + } + + deplex::utils::savePointCloudCSV(total_time.cast().transpose(), + (data_dir / ("process_sequence_total_time.csv")).string()); + + double elapsed_time_min = *std::min_element(total_time.begin(), total_time.end()); + double elapsed_time_max = *std::max_element(total_time.begin(), total_time.end()); + double elapsed_time_mean = std::accumulate(total_time.begin(), total_time.end(), 0.0) / NUMBER_OF_RUNS; + + double dispersion = variance(total_time, elapsed_time_mean); + double standard_deviation = sqrt(dispersion); + double standard_error = standard_deviation / sqrt(NUMBER_OF_RUNS); + + // 95% confidence interval + const float t_value = 1.96; + double lower_bound = elapsed_time_mean - t_value * standard_error; + double upper_bound = elapsed_time_mean + t_value * standard_error; + + std::cout << "\nDispersion: " << dispersion << '\n'; + std::cout << "Standard deviation: " << standard_deviation << '\n'; + std::cout << "Standard error: " << standard_error << '\n'; + std::cout << "Confidence interval (95%): [" << lower_bound << "; " << upper_bound << "]\n\n"; + + std::cout << "Elapsed time (ms.) (min): " << elapsed_time_min << '\n'; + std::cout << "Elapsed time (ms.) (max): " << elapsed_time_max << '\n'; + std::cout << "Elapsed time (ms.) (mean): " << elapsed_time_mean << '\n'; + std::cout << "FPS (max): " << 1000 / elapsed_time_min << '\n'; + std::cout << "FPS (min): " << 1000 / elapsed_time_max << '\n'; + std::cout << "FPS (mean): " << 1000 / elapsed_time_mean << '\n'; + + return 0; +} \ No newline at end of file diff --git a/benchmark/cpp-benchmark/benchmark_process_sequence.cpp b/benchmark/cpp-benchmark/benchmark_process_sequence.cpp new file mode 100644 index 0000000..3c73848 --- /dev/null +++ b/benchmark/cpp-benchmark/benchmark_process_sequence.cpp @@ -0,0 +1,181 @@ +#include +#include +#include + +#include +#include +#include +#include + +double variance(const Eigen::VectorXd& data, double mean) { + double sum = 0; + + for (auto x : data) { + sum += (x - mean) * (x - mean); + } + + sum /= static_cast(data.size()); + return sum; +} + +int main(int argc, char* argv[]) { + std::filesystem::path data_dir = + std::filesystem::current_path().parent_path().parent_path().parent_path() / "benchmark/data"; + std::filesystem::path image_path = data_dir / "depth/000004415622.png"; + std::filesystem::path intrinsics_path = data_dir / "config/intrinsics.K"; + std::filesystem::path config_path = data_dir / "config/TUM_fr3_long_val.ini"; + + auto start_time = std::chrono::high_resolution_clock::now(); + auto end_time = std::chrono::high_resolution_clock::now(); + + const int NUMBER_OF_RUNS = 10; + const int NUMBER_OF_SNAPSHOT = 1; + + std::vector execution_time_stage(NUMBER_OF_SNAPSHOT, Eigen::VectorXd::Zero(3)); + + std::string dataset_path = (argc > 1 ? argv[1] : (data_dir / "depth").string()); + + deplex::config::Config config = deplex::config::Config(config_path.string()); + Eigen::Matrix3f intrinsics(deplex::utils::readIntrinsics(intrinsics_path.string())); + deplex::utils::DepthImage image(image_path.string()); + + // Sort data entries + std::vector sorted_input_data; + for (auto const& entry : std::filesystem::directory_iterator(dataset_path)) { + if (entry.path().extension() == ".png") { + sorted_input_data.push_back(entry); + } + } + sort(sorted_input_data.begin(), sorted_input_data.end()); + + Eigen::VectorXi labels; + + deplex::PlaneExtractor algorithm(image.getHeight(), image.getWidth(), config); + std::cout << "Image Height: " << image.getHeight() << " Image Width: " << image.getWidth() << "\n\n"; + + for (int t = 0; t < NUMBER_OF_RUNS; ++t) { + std::cout << "LAUNCH #" << t + 1 << std::endl; + + for (Eigen::Index i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + start_time = std::chrono::high_resolution_clock::now(); + image.reset(sorted_input_data[i].path().string()); + end_time = std::chrono::high_resolution_clock::now(); + execution_time_stage[i][0] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); + + start_time = std::chrono::high_resolution_clock::now(); + auto pcd_array = image.toPointCloud(intrinsics); + end_time = std::chrono::high_resolution_clock::now(); + execution_time_stage[i][1] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); + + start_time = std::chrono::high_resolution_clock::now(); + labels = algorithm.process(pcd_array); + end_time = std::chrono::high_resolution_clock::now(); + execution_time_stage[i][2] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); + + std::cout << "Snapshot #" << i + 1 << " Planes found: " << labels.maxCoeff() << std::endl; + } + } + + auto execution_time_segmentation_stage = algorithm.GetExecutionTime(); + + for (auto& v : execution_time_segmentation_stage) { + for (auto& stage : v) { + stage /= NUMBER_OF_RUNS; + } + } + + for (auto& v : execution_time_stage) { + for (auto& stage : v) { + stage /= NUMBER_OF_RUNS; + } + } + + Eigen::VectorXd elements = Eigen::VectorXd::Zero(NUMBER_OF_SNAPSHOT); + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_stage[i][0]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_read_image.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_stage[i][1]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_translate_image.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_stage[i][2]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_segmentation.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_segmentation_stage[i][0]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_segmentation_cell_grid.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_segmentation_stage[i][1]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_segmentation_region_growing.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_segmentation_stage[i][2]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_segmentation_merge_planes.csv")).string()); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + elements[i] = execution_time_segmentation_stage[i][3]; + } + + deplex::utils::savePointCloudCSV(elements.cast().transpose(), + (data_dir / ("process_sequence_stage_segmentation_labels.csv")).string()); + + Eigen::VectorXd total_time = Eigen::VectorXd::Zero(NUMBER_OF_SNAPSHOT); + + for (auto i = 0; i < NUMBER_OF_SNAPSHOT; ++i) { + total_time[i] = execution_time_stage[i][0] + execution_time_stage[i][1] + execution_time_stage[i][2]; + } + + deplex::utils::savePointCloudCSV(total_time.cast().transpose(), + (data_dir / ("process_sequence_total_time.csv")).string()); + + double elapsed_time_min = *std::min_element(total_time.begin(), total_time.end()); + double elapsed_time_max = *std::max_element(total_time.begin(), total_time.end()); + double elapsed_time_mean = std::accumulate(total_time.begin(), total_time.end(), 0.0) / NUMBER_OF_SNAPSHOT; + + double dispersion = variance(total_time, elapsed_time_mean); + double standard_deviation = sqrt(dispersion); + double standard_error = standard_deviation / sqrt(NUMBER_OF_SNAPSHOT); + + // 95% confidence interval + const float t_value = 1.96; + double lower_bound = elapsed_time_mean - t_value * standard_error; + double upper_bound = elapsed_time_mean + t_value * standard_error; + + std::cout << "\nDispersion: " << dispersion << '\n'; + std::cout << "Standard deviation: " << standard_deviation << '\n'; + std::cout << "Standard error: " << standard_error << '\n'; + std::cout << "Confidence interval (95%): [" << lower_bound << "; " << upper_bound << "]\n\n"; + + std::cout << "Elapsed time (ms.) (min): " << elapsed_time_min << '\n'; + std::cout << "Elapsed time (ms.) (max): " << elapsed_time_max << '\n'; + std::cout << "Elapsed time (ms.) (mean): " << elapsed_time_mean << '\n'; + std::cout << "FPS (max): " << 1000 / elapsed_time_min << '\n'; + std::cout << "FPS (min): " << 1000 / elapsed_time_max << '\n'; + std::cout << "FPS (mean): " << 1000 / elapsed_time_mean << '\n'; + + return 0; +} \ No newline at end of file diff --git a/benchmark/data/color/000004415600.png b/benchmark/data/color/000004415600.png new file mode 100755 index 0000000..5362067 Binary files /dev/null and b/benchmark/data/color/000004415600.png differ diff --git a/benchmark/data/config/TUM_fr3_long_val.ini b/benchmark/data/config/TUM_fr3_long_val.ini new file mode 100644 index 0000000..1041f70 --- /dev/null +++ b/benchmark/data/config/TUM_fr3_long_val.ini @@ -0,0 +1,13 @@ +[Parameters] +patchSize=10 +histogramBinsPerCoord=20 +minCosAngleForMerge=0.90 +maxMergeDist=500 +minRegionGrowingCandidateSize=5 +minRegionGrowingCellsActivated=4 +minRegionPlanarityScore=0.55 +depthSigmaCoeff=1.425e-6 +depthSigmaMargin=10 +minPtsPerCell=3 +depthDiscontinuityThreshold=160 +maxNumberDepthDiscontinuity=1 diff --git a/benchmark/data/config/intrinsics.K b/benchmark/data/config/intrinsics.K new file mode 100644 index 0000000..7a86ff4 --- /dev/null +++ b/benchmark/data/config/intrinsics.K @@ -0,0 +1,3 @@ +944.16741943 0. 956.85647899 +0. 939.10968018 552.31795789 +0. 0. 1 \ No newline at end of file diff --git a/benchmark/data/depth/000004415622.png b/benchmark/data/depth/000004415622.png new file mode 100755 index 0000000..a866552 Binary files /dev/null and b/benchmark/data/depth/000004415622.png differ diff --git a/benchmark/python-benchmark/benchmark_process_cloud.py b/benchmark/python-benchmark/benchmark_process_cloud.py new file mode 100644 index 0000000..436b4cd --- /dev/null +++ b/benchmark/python-benchmark/benchmark_process_cloud.py @@ -0,0 +1,66 @@ +from deplex.utils import DepthImage +import deplex + +import timeit +import numpy as np + +from pathlib import Path + +data_dir = Path(__file__).parent.parent.parent.resolve() / "benchmark/data" +image_path = data_dir / Path("depth") / Path("000004415622.png") +config_path = data_dir / Path("config") / Path("TUM_fr3_long_val.ini") +intrinsics_path = data_dir / Path("config") / Path("intrinsics.K") + + +def benchmarkProcessCloud(): + NUMBER_OF_RUNS = 10 + + execution_time = [] + + config = deplex.Config(str(config_path)) + camera_intrinsic = np.genfromtxt(intrinsics_path) + image = DepthImage(str(image_path)) + + print("Image Height:", image.height, "Image Width:", image.width, '\n') + + coarse_algorithm = deplex.PlaneExtractor(image_height=image.height, image_width=image.width, config=config) + + for i in range(NUMBER_OF_RUNS): + start_time = timeit.default_timer() + + pcd_points = image.transform_to_pcd(camera_intrinsic) + + labels = coarse_algorithm.process(pcd_points) + + end_time = timeit.default_timer() + execution_time.append((end_time - start_time) * 1000) + print(f'Iteration #{i + 1} Planes found: {max(labels)}') + + elapsed_time_min = min(execution_time) + elapsed_time_max = max(execution_time) + elapsed_time_mean = np.average(execution_time) + + dispersion = np.var(execution_time) + standard_deviation = np.std(execution_time) + standard_error = standard_deviation / np.sqrt(NUMBER_OF_RUNS) + + # 95% confidence interval + t_value = 1.96 + lower_bound = elapsed_time_mean - t_value * standard_error + upper_bound = elapsed_time_mean + t_value * standard_error + + print('\nDispersion:', f"{dispersion:.5f}") + print('Standard deviation:', f"{standard_deviation:.5f}") + print('Standard error:', f"{standard_error:.5f}") + print('Confidence interval (95%):', f"[{lower_bound:.5f};", f"{upper_bound:.5f}]\n") + + print('Elapsed time (ms.) (min):', f"{elapsed_time_min:.5f}") + print('Elapsed time (ms.) (max):', f"{elapsed_time_max:.5f}") + print('Elapsed time (ms.) (mean):', f"{elapsed_time_mean:.5f}") + print('FPS (max):', f"{1000 / elapsed_time_min:.5f}") + print('FPS (min):', f"{1000 / elapsed_time_max:.5f}") + print('FPS (mean):', f"{1000 / elapsed_time_mean:.5f}") + + +if __name__ == '__main__': + benchmarkProcessCloud() diff --git a/benchmark/python-benchmark/benchmark_process_sequence.py b/benchmark/python-benchmark/benchmark_process_sequence.py new file mode 100644 index 0000000..7d2b3a2 --- /dev/null +++ b/benchmark/python-benchmark/benchmark_process_sequence.py @@ -0,0 +1,70 @@ +from deplex.utils import DepthImage +import deplex + +import os +import timeit +import numpy as np + +from pathlib import Path + +data_dir = Path(__file__).parent.parent.parent.resolve() / "benchmark/data" +image_path = data_dir / Path("depth") / Path("000004415622.png") +config_path = data_dir / Path("config") / Path("TUM_fr3_long_val.ini") +intrinsics_path = data_dir / Path("config") / Path("intrinsics.K") + + +def benchmarkProcessSequence(): + NUMBER_OF_SNAPSHOT = 1 + + execution_time = [] + + config = deplex.Config(str(config_path)) + camera_intrinsic = np.genfromtxt(intrinsics_path) + image = DepthImage(str(image_path)) + + images = [filename for filename in os.listdir(data_dir / Path("depth")) if filename.endswith('.png')] + images.sort() + + print("Image Height:", image.height, "Image Width:", image.width, '\n') + + coarse_algorithm = deplex.PlaneExtractor(image_height=image.height, image_width=image.width, config=config) + + for i in range(NUMBER_OF_SNAPSHOT): + start_time = timeit.default_timer() + + image.reset(str(os.path.join(data_dir / Path("depth"), images[i]))) + pcd_points = image.transform_to_pcd(camera_intrinsic) + labels = coarse_algorithm.process(pcd_points) + + end_time = timeit.default_timer() + execution_time.append((end_time - start_time) * 1000) + print(f'SNAPSHOT #{i + 1} Planes found: {max(labels)}') + + elapsed_time_min = min(execution_time) + elapsed_time_max = max(execution_time) + elapsed_time_mean = np.average(execution_time) + + dispersion = np.var(execution_time) + standard_deviation = np.std(execution_time) + standard_error = standard_deviation / np.sqrt(NUMBER_OF_SNAPSHOT) + + # 95% confidence interval + t_value = 1.96 + lower_bound = elapsed_time_mean - t_value * standard_error + upper_bound = elapsed_time_mean + t_value * standard_error + + print('\nDispersion:', f"{dispersion:.5f}") + print('Standard deviation:', f"{standard_deviation:.5f}") + print('Standard error:', f"{standard_error:.5f}") + print('Confidence interval (95%):', f"[{lower_bound:.5f};", f"{upper_bound:.5f}]\n") + + print('Elapsed time (ms.) (min):', f"{elapsed_time_min:.5f}") + print('Elapsed time (ms.) (max):', f"{elapsed_time_max:.5f}") + print('Elapsed time (ms.) (mean):', f"{elapsed_time_mean:.5f}") + print('FPS (max):', f"{1000 / elapsed_time_min:.5f}") + print('FPS (min):', f"{1000 / elapsed_time_max:.5f}") + print('FPS (mean):', f"{1000 / elapsed_time_mean:.5f}") + + +if __name__ == '__main__': + benchmarkProcessSequence() diff --git a/cpp/deplex/include/deplex/plane_extractor.h b/cpp/deplex/include/deplex/plane_extractor.h index 3de7daf..74006e8 100644 --- a/cpp/deplex/include/deplex/plane_extractor.h +++ b/cpp/deplex/include/deplex/plane_extractor.h @@ -47,6 +47,8 @@ class PlaneExtractor { */ Eigen::VectorXi process(Eigen::MatrixX3f const& pcd_array); + std::vector GetExecutionTime(); + PlaneExtractor(PlaneExtractor&& op) noexcept; PlaneExtractor& operator=(PlaneExtractor&& op) noexcept; diff --git a/cpp/deplex/src/deplex/plane_extractor.cpp b/cpp/deplex/src/deplex/plane_extractor.cpp index c546635..f2449a2 100644 --- a/cpp/deplex/src/deplex/plane_extractor.cpp +++ b/cpp/deplex/src/deplex/plane_extractor.cpp @@ -16,6 +16,7 @@ #include "deplex/plane_extractor.h" #include +#include #include #include @@ -67,6 +68,9 @@ class PlaneExtractor::Impl { */ Eigen::VectorXi process(Eigen::MatrixX3f const& pcd_array); + std::vector execution_time{100, Eigen::VectorXd::Zero(4)}; + size_t index_image = 0; + private: config::Config config_; int32_t nr_horizontal_cells_; @@ -184,6 +188,8 @@ PlaneExtractor::PlaneExtractor(int32_t image_height, int32_t image_width, config Eigen::VectorXi PlaneExtractor::process(Eigen::MatrixX3f const& pcd_array) { return impl_->process(pcd_array); } +std::vector PlaneExtractor::GetExecutionTime() { return impl_->execution_time; } + Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) { if (pcd_array.rows() != image_width_ * image_height_) { std::string msg_points_size = std::to_string(pcd_array.rows()); @@ -192,11 +198,15 @@ Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) throw std::runtime_error("Error! Number of points doesn't match image shape: " + msg_points_size + " != " + msg_height + " x " + msg_width); } - // 1. Initialize cell grid (Planarity estimation) +// 1. Initialize cell grid (Planarity estimation) #ifdef BENCHMARK_LOGGING auto time_init_cell_grid = std::chrono::high_resolution_clock::now(); #endif + auto start_time = std::chrono::high_resolution_clock::now(); CellGrid cell_grid(pcd_array, config_, nr_horizontal_cells_, nr_vertical_cells_); + auto end_time = std::chrono::high_resolution_clock::now(); + execution_time[index_image][0] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); #ifdef BENCHMARK_LOGGING std::clog << "[BenchmarkLogging] Cell Grid Initialization: " << get_benchmark_time(time_init_cell_grid) << '\n'; @@ -206,7 +216,7 @@ Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) std::clog << "[DebugInfo] Planar cell found: " << std::count(cell_grid.getPlanarMask().begin(), cell_grid.getPlanarMask().end(), true) << '\n'; #endif - // 2. Find dominant cell normals +// 2. Find dominant cell normals #ifdef BENCHMARK_LOGGING auto time_init_histogram = std::chrono::high_resolution_clock::now(); #endif @@ -215,11 +225,15 @@ Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) std::clog << "[BenchmarkLogging] Histogram Initialization: " << get_benchmark_time(time_init_histogram) << '\n'; #endif - // 3. Region growing +// 3. Region growing #ifdef BENCHMARK_LOGGING auto time_region_growing = std::chrono::high_resolution_clock::now(); #endif + start_time = std::chrono::high_resolution_clock::now(); auto plane_segments = createPlaneSegments(cell_grid, hist); + end_time = std::chrono::high_resolution_clock::now(); + execution_time[index_image][1] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); #ifdef BENCHMARK_LOGGING std::clog << "[BenchmarkLogging] Region Growing: " << get_benchmark_time(time_region_growing) << '\n'; @@ -230,11 +244,15 @@ Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) if (plane_segments.empty()) { return Eigen::VectorXi::Zero(pcd_array.rows()); } - // 5. Merge planes +// 5. Merge planes #ifdef BENCHMARK_LOGGING auto time_merge_planes = std::chrono::high_resolution_clock::now(); #endif + start_time = std::chrono::high_resolution_clock::now(); std::vector merge_labels = findMergedLabels(&plane_segments); + end_time = std::chrono::high_resolution_clock::now(); + execution_time[index_image][2] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); #ifdef BENCHMARK_LOGGING std::clog << "[BenchmarkLogging] Merge Planes: " << get_benchmark_time(time_merge_planes) << '\n'; @@ -250,7 +268,11 @@ Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) #ifdef BENCHMARK_LOGGING auto time_labels_creation = std::chrono::high_resolution_clock::now(); #endif + start_time = std::chrono::high_resolution_clock::now(); Eigen::VectorXi labels = toImageLabels(merge_labels); + end_time = std::chrono::high_resolution_clock::now(); + execution_time[index_image][3] += + static_cast(std::chrono::duration_cast(end_time - start_time).count()); #ifdef BENCHMARK_LOGGING std::clog << "[BenchmarkLogging] Labels creation: " << get_benchmark_time(time_labels_creation) << '\n'; @@ -277,6 +299,10 @@ Eigen::VectorXi PlaneExtractor::Impl::process(Eigen::MatrixX3f const& pcd_array) .format(Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", "\n")); #endif } + if (++index_image == 100) { + index_image = 0; + } + // 8. Cleanup cleanArtifacts(); return labels; diff --git a/cpp/pybind/utils/utils.cpp b/cpp/pybind/utils/utils.cpp index 5925b7b..23f9fce 100644 --- a/cpp/pybind/utils/utils.cpp +++ b/cpp/pybind/utils/utils.cpp @@ -31,6 +31,7 @@ void pybind_depth_image(py::module& m) { .def(py::init(), py::arg("image_path")) .def_property_readonly("height", &utils::DepthImage::getHeight) .def_property_readonly("width", &utils::DepthImage::getWidth) - .def("transform_to_pcd", &utils::DepthImage::toPointCloud, py::arg("intrinsics")); + .def("transform_to_pcd", &utils::DepthImage::toPointCloud, py::arg("intrinsics")) + .def("reset", &utils::DepthImage::reset, py::arg("image_path")); } } // namespace deplex \ No newline at end of file