From f8cafbbc87542f66f26796af6d8ba6144cc83db1 Mon Sep 17 00:00:00 2001 From: Toni Date: Sat, 9 May 2020 15:43:57 -0400 Subject: [PATCH 01/15] Add check on rectified baseline --- include/kimera-vio/frontend/Camera.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/kimera-vio/frontend/Camera.h b/include/kimera-vio/frontend/Camera.h index 7f2944ce6..61fd25ac1 100644 --- a/include/kimera-vio/frontend/Camera.h +++ b/include/kimera-vio/frontend/Camera.h @@ -258,8 +258,9 @@ class StereoCamera { // Relative pose after rectification gtsam::Pose3 camLrect_Pose_calRrect = B_Pose_camLrect->between(B_Pose_camRrect); - // get baseline + // Get baseline (this is after rectification). *baseline = camLrect_Pose_calRrect.translation().x(); + CHECK_GT(*baseline, 0u); // Sanity check. LOG_IF(FATAL, From 59abac6bc9b59fb476d0288227b33e19f1a5922d Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 00:57:45 -0400 Subject: [PATCH 02/15] Add possibility to use another display --- include/kimera-vio/pipeline/Pipeline.h | 8 +++++++- src/frontend/StereoVisionFrontEnd.cpp | 2 +- src/pipeline/Pipeline.cpp | 15 +++++++++------ 3 files changed, 17 insertions(+), 8 deletions(-) diff --git a/include/kimera-vio/pipeline/Pipeline.h b/include/kimera-vio/pipeline/Pipeline.h index 8603ed880..e50c94762 100644 --- a/include/kimera-vio/pipeline/Pipeline.h +++ b/include/kimera-vio/pipeline/Pipeline.h @@ -45,7 +45,13 @@ class Pipeline { EIGEN_MAKE_ALIGNED_OPERATOR_NEW public: - explicit Pipeline(const VioParams& params); + /** + * @brief Pipeline + * @param params Vio parameters + * @param displayer Optional displayer for visualizing results + */ + Pipeline(const VioParams& params, + DisplayBase::UniquePtr&& displayer = nullptr); virtual ~Pipeline(); diff --git a/src/frontend/StereoVisionFrontEnd.cpp b/src/frontend/StereoVisionFrontEnd.cpp index 547d0df05..2f6c402da 100644 --- a/src/frontend/StereoVisionFrontEnd.cpp +++ b/src/frontend/StereoVisionFrontEnd.cpp @@ -362,7 +362,7 @@ StatusStereoMeasurementsPtr StereoVisionFrontEnd::processStereoFrame( if (FLAGS_log_stereo_matching_images) sendMonoTrackingToLogger(); } if (display_queue_ && FLAGS_visualize_feature_tracks) { - displayImage("Feature Tracks", + displayImage("feature_tracks", tracker_.getTrackerImage(stereoFrame_lkf_->getLeftFrame(), stereoFrame_k_->getLeftFrame()), display_queue_); diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 9e5bf4378..84d96f076 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -93,7 +93,7 @@ DEFINE_bool(use_lcd, namespace VIO { -Pipeline::Pipeline(const VioParams& params) +Pipeline::Pipeline(const VioParams& params, DisplayBase::UniquePtr&& displayer) : backend_type_(static_cast(params.backend_type_)), stereo_camera_(nullptr), data_provider_module_(nullptr), @@ -186,7 +186,7 @@ Pipeline::Pipeline(const VioParams& params) backend_output_params, FLAGS_log_output)); vio_backend_module_->registerOnFailureCallback( - std::bind(&Pipeline::signalBackendFailure, this)); + std::bind(&Pipeline::signalBackendFailure, this)); vio_backend_module_->registerImuBiasUpdateCallback( std::bind(&StereoVisionFrontEndModule::updateImuBias, // Send a cref: constant reference bcs updateImuBias is const @@ -242,8 +242,11 @@ Pipeline::Pipeline(const VioParams& params) &display_input_queue_, nullptr, parallel_run_, - DisplayFactory::makeDisplay(DisplayType::kOpenCV, - std::bind(&Pipeline::shutdown, this))); + // Use given displayer if any + displayer + ? std::move(displayer) + : DisplayFactory::makeDisplay( + DisplayType::kOpenCV, std::bind(&Pipeline::shutdown, this))); } if (FLAGS_use_lcd) { @@ -375,8 +378,8 @@ bool Pipeline::shutdownWhenFinished(const int& sleep_time_ms) { CHECK(vio_backend_module_); while ( - !shutdown_ && // Loop while not explicitly shutdown. - is_backend_ok_ && // Loop while backend is fine. + !shutdown_ && // Loop while not explicitly shutdown. + is_backend_ok_ && // Loop while backend is fine. (!is_initialized_ || // Loop while not initialized // Or, once initialized, data is not yet consumed. !(!data_provider_module_->isWorking() && From 2ea2d50fa07756c2e4e0a00c643ca9a92399fc5e Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 15:11:13 -0400 Subject: [PATCH 03/15] QtCreator was sending me to devel space --- CMakeLists.txt | 10 +++--- include/kimera-vio/backend/CMakeLists.txt | 18 +++++----- include/kimera-vio/common/CMakeLists.txt | 4 +-- .../kimera-vio/dataprovider/CMakeLists.txt | 10 +++--- include/kimera-vio/factors/CMakeLists.txt | 4 +-- include/kimera-vio/frontend/CMakeLists.txt | 34 +++++++++---------- .../frontend/feature-detector/CMakeLists.txt | 8 ++--- .../feature-detector/anms/CMakeLists.txt | 8 ++--- .../kimera-vio/imu-frontend/CMakeLists.txt | 6 ++-- include/kimera-vio/initial/CMakeLists.txt | 10 +++--- include/kimera-vio/logging/CMakeLists.txt | 2 +- include/kimera-vio/loopclosure/CMakeLists.txt | 8 ++--- include/kimera-vio/mesh/CMakeLists.txt | 12 +++---- include/kimera-vio/pipeline/CMakeLists.txt | 12 +++---- include/kimera-vio/utils/CMakeLists.txt | 34 +++++++++---------- include/kimera-vio/visualizer/CMakeLists.txt | 16 ++++----- src/backend/CMakeLists.txt | 10 +++--- src/common/CMakeLists.txt | 2 +- src/dataprovider/CMakeLists.txt | 10 +++--- src/factors/CMakeLists.txt | 4 +-- src/frontend/CMakeLists.txt | 18 +++++----- src/frontend/feature-detector/CMakeLists.txt | 6 ++-- .../feature-detector/anms/CMakeLists.txt | 2 +- src/imu-frontend/CMakeLists.txt | 4 +-- src/initial/CMakeLists.txt | 6 ++-- src/logging/CMakeLists.txt | 2 +- src/loopclosure/CMakeLists.txt | 6 ++-- src/mesh/CMakeLists.txt | 8 ++--- src/pipeline/CMakeLists.txt | 12 +++---- src/utils/CMakeLists.txt | 12 +++---- src/visualizer/CMakeLists.txt | 14 ++++---- 31 files changed, 156 insertions(+), 156 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee21df4b3..303d70cea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,7 +134,7 @@ target_include_directories(${PROJECT_NAME} ${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} - $ + $ $ ) @@ -260,7 +260,7 @@ write_basic_package_version_file( # This file is necessary to find_package the library kimera_vio. set(INSTALL_CONFIGDIR lib/cmake/kimera_vio) configure_package_config_file( - ${CMAKE_CURRENT_LIST_DIR}/cmake/kimera_vioConfig.cmake.in + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/kimera_vioConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/kimera_vioConfig.cmake INSTALL_DESTINATION ${INSTALL_CONFIGDIR} ) @@ -289,7 +289,7 @@ else(EXPORT_KIMERA) ${INSTALL_CONFIGDIR} ) # Install header files - install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/include/ + install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ DESTINATION include FILES_MATCHING PATTERN "*.h") @@ -297,8 +297,8 @@ else(EXPORT_KIMERA) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/kimera_vioConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/kimera_vioConfigVersion.cmake - ${CMAKE_CURRENT_LIST_DIR}/cmake/FindGflags.cmake - ${CMAKE_CURRENT_LIST_DIR}/cmake/FindGlog.cmake + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/FindGflags.cmake + ${CMAKE_CURRENT_SOURCE_DIR}/cmake/FindGlog.cmake DESTINATION ${INSTALL_CONFIGDIR} ) endif(EXPORT_KIMERA) diff --git a/include/kimera-vio/backend/CMakeLists.txt b/include/kimera-vio/backend/CMakeLists.txt index 0dd33f2a7..cff83e4dd 100644 --- a/include/kimera-vio/backend/CMakeLists.txt +++ b/include/kimera-vio/backend/CMakeLists.txt @@ -1,12 +1,12 @@ ### Add source code just for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/FactorGraphManagement.h" - "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd.h" - "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEndParams.h" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd.h" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEndParams.h" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEndModule.h" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEndFactory.h" + "${CMAKE_CURRENT_SOURCE_DIR}/FactorGraphManagement.h" + "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEnd-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEnd.h" + "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEndParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEnd-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEnd.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndFactory.h" ) diff --git a/include/kimera-vio/common/CMakeLists.txt b/include/kimera-vio/common/CMakeLists.txt index 37cdfbe0d..940990f7f 100644 --- a/include/kimera-vio/common/CMakeLists.txt +++ b/include/kimera-vio/common/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code just for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/vio_types.h" - "${CMAKE_CURRENT_LIST_DIR}/VioNavState.h" + "${CMAKE_CURRENT_SOURCE_DIR}/vio_types.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VioNavState.h" ) diff --git a/include/kimera-vio/dataprovider/CMakeLists.txt b/include/kimera-vio/dataprovider/CMakeLists.txt index c39e7087c..bc2c800bb 100644 --- a/include/kimera-vio/dataprovider/CMakeLists.txt +++ b/include/kimera-vio/dataprovider/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/DataProviderModule.h" - "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface.h" - "${CMAKE_CURRENT_LIST_DIR}/EurocDataProvider.h" - "${CMAKE_CURRENT_LIST_DIR}/KittiDataProvider.h" + "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface.h" + "${CMAKE_CURRENT_SOURCE_DIR}/EurocDataProvider.h" + "${CMAKE_CURRENT_SOURCE_DIR}/KittiDataProvider.h" ) diff --git a/include/kimera-vio/factors/CMakeLists.txt b/include/kimera-vio/factors/CMakeLists.txt index e758bc3d9..5f07f7c86 100644 --- a/include/kimera-vio/factors/CMakeLists.txt +++ b/include/kimera-vio/factors/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/ParallelPlaneRegularFactor.h" - "${CMAKE_CURRENT_LIST_DIR}/PointPlaneFactor.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ParallelPlaneRegularFactor.h" + "${CMAKE_CURRENT_SOURCE_DIR}/PointPlaneFactor.h" ) diff --git a/include/kimera-vio/frontend/CMakeLists.txt b/include/kimera-vio/frontend/CMakeLists.txt index 9ef19825c..65da979c5 100644 --- a/include/kimera-vio/frontend/CMakeLists.txt +++ b/include/kimera-vio/frontend/CMakeLists.txt @@ -1,22 +1,22 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Camera.h" - "${CMAKE_CURRENT_LIST_DIR}/CameraParams.h" - "${CMAKE_CURRENT_LIST_DIR}/StereoMatchingParams.h" - "${CMAKE_CURRENT_LIST_DIR}/Frame.h" - "${CMAKE_CURRENT_LIST_DIR}/StereoFrame-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/StereoFrame.h" - "${CMAKE_CURRENT_LIST_DIR}/StereoImuSyncPacket.h" - "${CMAKE_CURRENT_LIST_DIR}/StereoVisionFrontEnd-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/StereoVisionFrontEnd.h" - "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndParams.h" - "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndModule.h" - "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndFactory.h" - "${CMAKE_CURRENT_LIST_DIR}/Tracker-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/Tracker.h" - "${CMAKE_CURRENT_LIST_DIR}/OpticalFlowPredictor-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/OpticalFlowPredictor.h" - "${CMAKE_CURRENT_LIST_DIR}/OpticalFlowPredictorFactory.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Camera.h" + "${CMAKE_CURRENT_SOURCE_DIR}/CameraParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoMatchingParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Frame.h" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoFrame-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoFrame.h" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoImuSyncPacket.h" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoVisionFrontEnd-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoVisionFrontEnd.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndFactory.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Tracker-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Tracker.h" + "${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlowPredictor-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlowPredictor.h" + "${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlowPredictorFactory.h" ) add_subdirectory(feature-detector) diff --git a/include/kimera-vio/frontend/feature-detector/CMakeLists.txt b/include/kimera-vio/frontend/feature-detector/CMakeLists.txt index 207166d2c..ff73f2ef7 100644 --- a/include/kimera-vio/frontend/feature-detector/CMakeLists.txt +++ b/include/kimera-vio/frontend/feature-detector/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector.h" - "${CMAKE_CURRENT_LIST_DIR}/FeatureDetectorParams.h" - "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/NonMaximumSuppression.h" + "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetector.h" + "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetectorParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetector-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/NonMaximumSuppression.h" ) add_subdirectory(anms) diff --git a/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt b/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt index cc08258db..14246f790 100644 --- a/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt +++ b/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt @@ -2,8 +2,8 @@ target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/nanoflann.hpp" - "${CMAKE_CURRENT_LIST_DIR}/range-tree/lrtypes.h" - "${CMAKE_CURRENT_LIST_DIR}/range-tree/ranget.h" - "${CMAKE_CURRENT_LIST_DIR}/anms.h" + "${CMAKE_CURRENT_SOURCE_DIR}/nanoflann.hpp" + "${CMAKE_CURRENT_SOURCE_DIR}/range-tree/lrtypes.h" + "${CMAKE_CURRENT_SOURCE_DIR}/range-tree/ranget.h" + "${CMAKE_CURRENT_SOURCE_DIR}/anms.h" ) diff --git a/include/kimera-vio/imu-frontend/CMakeLists.txt b/include/kimera-vio/imu-frontend/CMakeLists.txt index 42ce70bf3..395dc2137 100644 --- a/include/kimera-vio/imu-frontend/CMakeLists.txt +++ b/include/kimera-vio/imu-frontend/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code for kimera_vio target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.h" - "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEnd-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEnd.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEndParams.h" ) diff --git a/include/kimera-vio/initial/CMakeLists.txt b/include/kimera-vio/initial/CMakeLists.txt index edefa6a3a..5bca8ee95 100644 --- a/include/kimera-vio/initial/CMakeLists.txt +++ b/include/kimera-vio/initial/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd.h" - "${CMAKE_CURRENT_LIST_DIR}/InitializationFromImu.h" - "${CMAKE_CURRENT_LIST_DIR}/OnlineGravityAlignment-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/OnlineGravityAlignment.h" + "${CMAKE_CURRENT_SOURCE_DIR}/InitializationBackEnd-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/InitializationBackEnd.h" + "${CMAKE_CURRENT_SOURCE_DIR}/InitializationFromImu.h" + "${CMAKE_CURRENT_SOURCE_DIR}/OnlineGravityAlignment-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/OnlineGravityAlignment.h" ) diff --git a/include/kimera-vio/logging/CMakeLists.txt b/include/kimera-vio/logging/CMakeLists.txt index 5d5bd3392..81bc21843 100644 --- a/include/kimera-vio/logging/CMakeLists.txt +++ b/include/kimera-vio/logging/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Logger.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Logger.h" ) diff --git a/include/kimera-vio/loopclosure/CMakeLists.txt b/include/kimera-vio/loopclosure/CMakeLists.txt index e76bf18fe..df19de525 100644 --- a/include/kimera-vio/loopclosure/CMakeLists.txt +++ b/include/kimera-vio/loopclosure/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for LoopClosureDetector target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector.h" - "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetectorParams.h" - "${CMAKE_CURRENT_LIST_DIR}/LcdThirdPartyWrapper.h" + "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetector-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetector.h" + "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetectorParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/LcdThirdPartyWrapper.h" ) diff --git a/include/kimera-vio/mesh/CMakeLists.txt b/include/kimera-vio/mesh/CMakeLists.txt index efb824c1f..d882bbad3 100644 --- a/include/kimera-vio/mesh/CMakeLists.txt +++ b/include/kimera-vio/mesh/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Mesh.h" - "${CMAKE_CURRENT_LIST_DIR}/Mesher.h" - "${CMAKE_CURRENT_LIST_DIR}/MesherModule.h" - "${CMAKE_CURRENT_LIST_DIR}/MesherFactory.h" - "${CMAKE_CURRENT_LIST_DIR}/Mesher-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/Mesher_cgal.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Mesh.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Mesher.h" + "${CMAKE_CURRENT_SOURCE_DIR}/MesherModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/MesherFactory.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Mesher-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Mesher_cgal.h" ) diff --git a/include/kimera-vio/pipeline/CMakeLists.txt b/include/kimera-vio/pipeline/CMakeLists.txt index fbf57dcdc..3d35ddf3b 100644 --- a/include/kimera-vio/pipeline/CMakeLists.txt +++ b/include/kimera-vio/pipeline/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Pipeline.h" - "${CMAKE_CURRENT_LIST_DIR}/Pipeline-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/PipelinePayload.h" - "${CMAKE_CURRENT_LIST_DIR}/PipelineModule.h" - "${CMAKE_CURRENT_LIST_DIR}/PipelineParams.h" - "${CMAKE_CURRENT_LIST_DIR}/QueueSynchronizer.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/PipelinePayload.h" + "${CMAKE_CURRENT_SOURCE_DIR}/PipelineModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/PipelineParams.h" + "${CMAKE_CURRENT_SOURCE_DIR}/QueueSynchronizer.h" ) diff --git a/include/kimera-vio/utils/CMakeLists.txt b/include/kimera-vio/utils/CMakeLists.txt index 9187fbbb4..8a88eb2ab 100644 --- a/include/kimera-vio/utils/CMakeLists.txt +++ b/include/kimera-vio/utils/CMakeLists.txt @@ -1,21 +1,21 @@ ### Add includes for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Accumulator.h" - "${CMAKE_CURRENT_LIST_DIR}/Histogram.h" - "${CMAKE_CURRENT_LIST_DIR}/Macros.h" - "${CMAKE_CURRENT_LIST_DIR}/Statistics.h" - "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer.h" - "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer-inl.h" - "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeQueue.h" - "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeTemporalBuffer.h" - "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeTemporalBuffer-inl.h" - "${CMAKE_CURRENT_LIST_DIR}/Timer.h" - "${CMAKE_CURRENT_LIST_DIR}/UtilsGeometry.h" - "${CMAKE_CURRENT_LIST_DIR}/UtilsGTSAM.h" - "${CMAKE_CURRENT_LIST_DIR}/UtilsOpenCV.h" - "${CMAKE_CURRENT_LIST_DIR}/UtilsNumerical.h" - "${CMAKE_CURRENT_LIST_DIR}/SerializationOpenCv.h" - "${CMAKE_CURRENT_LIST_DIR}/YamlParser.h" - "${CMAKE_CURRENT_LIST_DIR}/FilesystemUtils.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Accumulator.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Histogram.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Macros.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Statistics.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeImuBuffer.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeImuBuffer-inl.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeQueue.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeTemporalBuffer.h" + "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeTemporalBuffer-inl.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Timer.h" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsGeometry.h" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsGTSAM.h" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsOpenCV.h" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsNumerical.h" + "${CMAKE_CURRENT_SOURCE_DIR}/SerializationOpenCv.h" + "${CMAKE_CURRENT_SOURCE_DIR}/YamlParser.h" + "${CMAKE_CURRENT_SOURCE_DIR}/FilesystemUtils.h" ) diff --git a/include/kimera-vio/visualizer/CMakeLists.txt b/include/kimera-vio/visualizer/CMakeLists.txt index 3bfc60415..cb19cb322 100644 --- a/include/kimera-vio/visualizer/CMakeLists.txt +++ b/include/kimera-vio/visualizer/CMakeLists.txt @@ -1,11 +1,11 @@ ### Add includes target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.h" - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.h" - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.h" - "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/Display.h" - "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.h" - "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3D.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3D-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DFactory.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Display-definitions.h" + "${CMAKE_CURRENT_SOURCE_DIR}/Display.h" + "${CMAKE_CURRENT_SOURCE_DIR}/DisplayModule.h" + "${CMAKE_CURRENT_SOURCE_DIR}/DisplayFactory.h" ) diff --git a/src/backend/CMakeLists.txt b/src/backend/CMakeLists.txt index 2cd79690f..0d924e1c6 100644 --- a/src/backend/CMakeLists.txt +++ b/src/backend/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/VioBackEndModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd.cpp" - "${CMAKE_CURRENT_LIST_DIR}/VioBackEndParams.cpp" - "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd.cpp" - "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEndParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEnd.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEnd.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEndParams.cpp" ) diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index b851589c5..2ec1d47c3 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/VioNavState.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VioNavState.cpp" ) diff --git a/src/dataprovider/CMakeLists.txt b/src/dataprovider/CMakeLists.txt index 4a03df329..7bcaeb9f4 100644 --- a/src/dataprovider/CMakeLists.txt +++ b/src/dataprovider/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface-definitions.cpp" - "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface.cpp" - "${CMAKE_CURRENT_LIST_DIR}/DataProviderModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/EurocDataProvider.cpp" - "${CMAKE_CURRENT_LIST_DIR}/KittiDataProvider.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface-definitions.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/EurocDataProvider.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/KittiDataProvider.cpp" ) diff --git a/src/factors/CMakeLists.txt b/src/factors/CMakeLists.txt index f106a8b45..24c8edebe 100644 --- a/src/factors/CMakeLists.txt +++ b/src/factors/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/ParallelPlaneRegularFactor.cpp" - "${CMAKE_CURRENT_LIST_DIR}/PointPlaneFactor.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/ParallelPlaneRegularFactor.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/PointPlaneFactor.cpp" ) diff --git a/src/frontend/CMakeLists.txt b/src/frontend/CMakeLists.txt index 70dd5c4ce..f9b8bb373 100644 --- a/src/frontend/CMakeLists.txt +++ b/src/frontend/CMakeLists.txt @@ -1,15 +1,15 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/CameraParams.cpp" - "${CMAKE_CURRENT_LIST_DIR}/StereoFrame.cpp" - "${CMAKE_CURRENT_LIST_DIR}/StereoMatchingParams.cpp" - "${CMAKE_CURRENT_LIST_DIR}/StereoImuSyncPacket.cpp" - "${CMAKE_CURRENT_LIST_DIR}/StereoVisionFrontEnd.cpp" - "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndFactory.cpp" - "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndParams.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Tracker.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/CameraParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoFrame.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoMatchingParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoImuSyncPacket.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/StereoVisionFrontEnd.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndFactory.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Tracker.cpp" ) add_subdirectory(feature-detector) diff --git a/src/frontend/feature-detector/CMakeLists.txt b/src/frontend/feature-detector/CMakeLists.txt index 441f32479..6516e643d 100644 --- a/src/frontend/feature-detector/CMakeLists.txt +++ b/src/frontend/feature-detector/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/FeatureDetectorParams.cpp" - "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector.cpp" - "${CMAKE_CURRENT_LIST_DIR}/NonMaximumSuppression.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetectorParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetector.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/NonMaximumSuppression.cpp" ) add_subdirectory(anms) diff --git a/src/frontend/feature-detector/anms/CMakeLists.txt b/src/frontend/feature-detector/anms/CMakeLists.txt index 5792cd35e..ce03dcc53 100644 --- a/src/frontend/feature-detector/anms/CMakeLists.txt +++ b/src/frontend/feature-detector/anms/CMakeLists.txt @@ -2,5 +2,5 @@ target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/anms.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/anms.cpp" ) diff --git a/src/imu-frontend/CMakeLists.txt b/src/imu-frontend/CMakeLists.txt index 338206f40..bea5b0aef 100644 --- a/src/imu-frontend/CMakeLists.txt +++ b/src/imu-frontend/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for kimera_vio target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.cpp" - "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEnd.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEndParams.cpp" ) diff --git a/src/initial/CMakeLists.txt b/src/initial/CMakeLists.txt index 8abbe3909..7660762c9 100644 --- a/src/initial/CMakeLists.txt +++ b/src/initial/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/InitializationFromImu.cpp" - "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd.cpp" - "${CMAKE_CURRENT_LIST_DIR}/OnlineGravityAlignment.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/InitializationFromImu.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/InitializationBackEnd.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/OnlineGravityAlignment.cpp" ) diff --git a/src/logging/CMakeLists.txt b/src/logging/CMakeLists.txt index 9345d5827..ce8398ff2 100644 --- a/src/logging/CMakeLists.txt +++ b/src/logging/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Logger.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Logger.cpp" ) diff --git a/src/loopclosure/CMakeLists.txt b/src/loopclosure/CMakeLists.txt index d9f035b75..43de35eb7 100644 --- a/src/loopclosure/CMakeLists.txt +++ b/src/loopclosure/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for LoopClosureDetector target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector.cpp" - "${CMAKE_CURRENT_LIST_DIR}/LcdThirdPartyWrapper.cpp" - "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetectorParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetector.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/LcdThirdPartyWrapper.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetectorParams.cpp" ) diff --git a/src/mesh/CMakeLists.txt b/src/mesh/CMakeLists.txt index ba3ef5029..7f842ad59 100644 --- a/src/mesh/CMakeLists.txt +++ b/src/mesh/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Mesh.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Mesher.cpp" - "${CMAKE_CURRENT_LIST_DIR}/MesherModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/MesherFactory.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Mesh.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Mesher.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/MesherModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/MesherFactory.cpp" ) diff --git a/src/pipeline/CMakeLists.txt b/src/pipeline/CMakeLists.txt index ed4ab6f51..ad26a2ada 100644 --- a/src/pipeline/CMakeLists.txt +++ b/src/pipeline/CMakeLists.txt @@ -1,10 +1,10 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Pipeline.cpp" - "${CMAKE_CURRENT_LIST_DIR}/PipelineModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/PipelinePayload.cpp" - "${CMAKE_CURRENT_LIST_DIR}/PipelineParams.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Pipeline-definitions.cpp" - "${CMAKE_CURRENT_LIST_DIR}/QueueSynchronizer.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/PipelineModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/PipelinePayload.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/PipelineParams.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline-definitions.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/QueueSynchronizer.cpp" ) diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index b6e0707e5..965b2b1f4 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -1,10 +1,10 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Statistics.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Histogram.cpp" - "${CMAKE_CURRENT_LIST_DIR}/UtilsGeometry.cpp" - "${CMAKE_CURRENT_LIST_DIR}/UtilsOpenCV.cpp" - "${CMAKE_CURRENT_LIST_DIR}/UtilsNumerical.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeImuBuffer.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Statistics.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Histogram.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsGeometry.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsOpenCV.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/UtilsNumerical.cpp" ) diff --git a/src/visualizer/CMakeLists.txt b/src/visualizer/CMakeLists.txt index 9f7166d88..e5b3a90e3 100644 --- a/src/visualizer/CMakeLists.txt +++ b/src/visualizer/CMakeLists.txt @@ -1,11 +1,11 @@ ### Add source code target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.cpp" - "${CMAKE_CURRENT_LIST_DIR}/Display.cpp" - "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.cpp" - "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3D.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DFactory.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Display-definitions.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/Display.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/DisplayModule.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/DisplayFactory.cpp" ) From f1445e20e35786fdcba3b779b094f4ab75185e10 Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 15:59:40 -0400 Subject: [PATCH 04/15] WIP: move k-vio to qt in non-ros --- include/kimera-vio/visualizer/Display.h | 30 +++++++++++++++++-- include/kimera-vio/visualizer/DisplayModule.h | 15 +++++++--- .../visualizer/Visualizer3D-definitions.h | 10 ++++++- src/visualizer/Display.cpp | 5 ++-- src/visualizer/DisplayModule.cpp | 14 --------- 5 files changed, 50 insertions(+), 24 deletions(-) diff --git a/include/kimera-vio/visualizer/Display.h b/include/kimera-vio/visualizer/Display.h index dcca09be5..4815c0434 100644 --- a/include/kimera-vio/visualizer/Display.h +++ b/include/kimera-vio/visualizer/Display.h @@ -31,8 +31,12 @@ class DisplayBase { DisplayBase() = default; virtual ~DisplayBase() = default; - // Spins the display once to render the visualizer output. - virtual void spinOnce(VisualizerOutput::UniquePtr&& viz_output) = 0; + /** + * @brief spinOnce + * Spins the display once to render the visualizer output. + * @param viz_output Visualizer output, which is the display input. + */ + virtual void spinOnce(DisplayInputBase::UniquePtr&& viz_output) = 0; }; class OpenCv3dDisplay : public DisplayBase { @@ -47,9 +51,29 @@ class OpenCv3dDisplay : public DisplayBase { // Spins renderers to display data using OpenCV imshow and viz3d // Displaying must be done in the main thread. - void spinOnce(VisualizerOutput::UniquePtr&& viz_output) override; + void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override; private: + VisualizerOutput::UniquePtr safeCast( + DisplayInputBase::UniquePtr display_input_base) { + VisualizerOutput::UniquePtr viz_output; + VisualizerOutput* tmp = nullptr; + try { + tmp = dynamic_cast(display_input_base.get()); + } catch (const std::bad_cast& e) { + LOG(ERROR) << "Seems that you are casting DisplayInputBase to " + "VisualizerOutput, but this object is not " + "a VisualizerOutput!"; + LOG(FATAL) << e.what(); + } catch (...) { + LOG(FATAL) << "Exception caught when casting to VisualizerOutput."; + } + CHECK_NOTNULL(tmp); + display_input_base.release(); + viz_output.reset(tmp); + return viz_output; + } + // Adds 3D widgets to the window, and displays it. void spin3dWindow(VisualizerOutput::UniquePtr&& viz_output); diff --git a/include/kimera-vio/visualizer/DisplayModule.h b/include/kimera-vio/visualizer/DisplayModule.h index ae1451419..3d3a507c8 100644 --- a/include/kimera-vio/visualizer/DisplayModule.h +++ b/include/kimera-vio/visualizer/DisplayModule.h @@ -31,20 +31,27 @@ namespace VIO { * DisplayModule should spin in the main thread to avoid errors. */ class DisplayModule - : public SISOPipelineModule { + : public SISOPipelineModule { public: KIMERA_POINTER_TYPEDEFS(DisplayModule); KIMERA_DELETE_COPY_CONSTRUCTORS(DisplayModule); - using SISO = SISOPipelineModule; + using SISO = SISOPipelineModule; DisplayModule(DisplayQueue* input_queue, OutputQueue* output_queue, bool parallel_run, - DisplayBase::UniquePtr&& display); + DisplayBase::UniquePtr&& display) + : SISO(input_queue, output_queue, "Display", parallel_run), + display_(std::move(display)) {} + virtual ~DisplayModule() = default; - virtual OutputUniquePtr spinOnce(VisualizerOutput::UniquePtr input); + virtual OutputUniquePtr spinOnce(InputUniquePtr input) { + CHECK(input); + display_->spinOnce(std::move(input)); + return VIO::make_unique(); + } private: // The renderer used to display the visualizer output. diff --git a/include/kimera-vio/visualizer/Visualizer3D-definitions.h b/include/kimera-vio/visualizer/Visualizer3D-definitions.h index 18956d8db..bf710c8e9 100644 --- a/include/kimera-vio/visualizer/Visualizer3D-definitions.h +++ b/include/kimera-vio/visualizer/Visualizer3D-definitions.h @@ -82,7 +82,15 @@ struct VisualizerInput : public PipelinePayload { const FrontendOutput::ConstPtr frontend_output_; }; -struct VisualizerOutput { +struct DisplayInputBase { + KIMERA_POINTER_TYPEDEFS(DisplayInputBase); + KIMERA_DELETE_COPY_CONSTRUCTORS(DisplayInputBase); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DisplayInputBase() = default; + virtual ~DisplayInputBase() = default; +}; + +struct VisualizerOutput : public DisplayInputBase { KIMERA_POINTER_TYPEDEFS(VisualizerOutput); KIMERA_DELETE_COPY_CONSTRUCTORS(VisualizerOutput); EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/src/visualizer/Display.cpp b/src/visualizer/Display.cpp index e21c90536..e333a167d 100644 --- a/src/visualizer/Display.cpp +++ b/src/visualizer/Display.cpp @@ -52,8 +52,9 @@ OpenCv3dDisplay::OpenCv3dDisplay( // cv::setMouseCallback( "main2",on_mouse,NULL ); } -void OpenCv3dDisplay::spinOnce(VisualizerOutput::UniquePtr&& viz_output) { - CHECK(viz_output); +void OpenCv3dDisplay::spinOnce(DisplayInputBase::UniquePtr&& display_input) { + CHECK(display_input); + VisualizerOutput::UniquePtr viz_output = safeCast(std::move(display_input)); // Display 2D images. spin2dWindow(*viz_output); // Display 3D window. diff --git a/src/visualizer/DisplayModule.cpp b/src/visualizer/DisplayModule.cpp index 5cb4fc3f0..b8d38f462 100644 --- a/src/visualizer/DisplayModule.cpp +++ b/src/visualizer/DisplayModule.cpp @@ -18,18 +18,4 @@ namespace VIO { -DisplayModule::DisplayModule(DisplayQueue* input_queue, - OutputQueue* output_queue, - bool parallel_run, - DisplayBase::UniquePtr&& display) - : SISO(input_queue, output_queue, "Display", parallel_run), - display_(std::move(display)) {} - -DisplayModule::OutputUniquePtr DisplayModule::spinOnce( - VisualizerOutput::UniquePtr input) { - CHECK(input); - display_->spinOnce(std::move(input)); - return VIO::make_unique(); -} - } // namespace VIO From c0f6abe2db1abf6a34a12449205e3cf40a7eb111 Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 17:23:43 -0400 Subject: [PATCH 05/15] Create template DisplayInputBase for msg passing of images --- include/kimera-vio/frontend/Tracker.h | 4 ++-- .../kimera-vio/visualizer/Display-definitions.h | 5 ++--- include/kimera-vio/visualizer/Display.h | 16 ++++++++++++---- .../visualizer/Visualizer3D-definitions.h | 17 +++++++++-------- include/kimera-vio/visualizer/Visualizer3D.h | 2 +- .../kimera-vio/visualizer/Visualizer3DModule.h | 4 ++-- src/visualizer/Display.cpp | 11 ++++++----- src/visualizer/Visualizer3DModule.cpp | 2 +- 8 files changed, 35 insertions(+), 26 deletions(-) diff --git a/include/kimera-vio/frontend/Tracker.h b/include/kimera-vio/frontend/Tracker.h index dc1bfbdb2..5f2096b66 100644 --- a/include/kimera-vio/frontend/Tracker.h +++ b/include/kimera-vio/frontend/Tracker.h @@ -37,8 +37,8 @@ namespace VIO { // TODO(Toni): Fast-forwarding bcs of an issue wiht includes: // if you include here the display-definitions.h, a million errors appear, this // should go away after properly cleaning what each file includes. -class VisualizerOutput; -typedef ThreadsafeQueue> DisplayQueue; +class DisplayInputBase; +using DisplayQueue = ThreadsafeQueue>; class Tracker { public: diff --git a/include/kimera-vio/visualizer/Display-definitions.h b/include/kimera-vio/visualizer/Display-definitions.h index 610977365..198671553 100644 --- a/include/kimera-vio/visualizer/Display-definitions.h +++ b/include/kimera-vio/visualizer/Display-definitions.h @@ -60,11 +60,10 @@ struct WindowData { inline void displayImage(const std::string& title, const cv::Mat& img, DisplayQueue* display_queue) { - VisualizerOutput::UniquePtr visualizer_output = - VIO::make_unique(); + DisplayInputBase::UniquePtr visualizer_output = + VIO::make_unique(); ImageToDisplay img_to_display(title, img); visualizer_output->images_to_display_.push_back(img_to_display); - visualizer_output->visualization_type_ = VisualizationType::kNone; if (display_queue) { display_queue->push(std::move(visualizer_output)); } else { diff --git a/include/kimera-vio/visualizer/Display.h b/include/kimera-vio/visualizer/Display.h index 4815c0434..9a35e83ac 100644 --- a/include/kimera-vio/visualizer/Display.h +++ b/include/kimera-vio/visualizer/Display.h @@ -54,8 +54,15 @@ class OpenCv3dDisplay : public DisplayBase { void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override; private: + /** + * @brief safeCast Try to cast display input base to the derived visualizer + * output, if unsuccessful, it will return a nullptr. + * @param display_input_base + * @return + */ VisualizerOutput::UniquePtr safeCast( DisplayInputBase::UniquePtr display_input_base) { + if (!display_input_base) return nullptr; VisualizerOutput::UniquePtr viz_output; VisualizerOutput* tmp = nullptr; try { @@ -63,12 +70,13 @@ class OpenCv3dDisplay : public DisplayBase { } catch (const std::bad_cast& e) { LOG(ERROR) << "Seems that you are casting DisplayInputBase to " "VisualizerOutput, but this object is not " - "a VisualizerOutput!"; - LOG(FATAL) << e.what(); + "a VisualizerOutput!\n" + "Error: " << e.what(); + return nullptr; } catch (...) { LOG(FATAL) << "Exception caught when casting to VisualizerOutput."; } - CHECK_NOTNULL(tmp); + if(!tmp) return nullptr; display_input_base.release(); viz_output.reset(tmp); return viz_output; @@ -77,7 +85,7 @@ class OpenCv3dDisplay : public DisplayBase { // Adds 3D widgets to the window, and displays it. void spin3dWindow(VisualizerOutput::UniquePtr&& viz_output); - void spin2dWindow(const VisualizerOutput& viz_output); + void spin2dWindow(const DisplayInputBase& viz_output); //! Sets the visualization properties of the 3D mesh. void setMeshProperties(WidgetsMap* widgets); diff --git a/include/kimera-vio/visualizer/Visualizer3D-definitions.h b/include/kimera-vio/visualizer/Visualizer3D-definitions.h index bf710c8e9..51440ca75 100644 --- a/include/kimera-vio/visualizer/Visualizer3D-definitions.h +++ b/include/kimera-vio/visualizer/Visualizer3D-definitions.h @@ -16,19 +16,19 @@ #include #include +#include #include #include -#include #include -#include "kimera-vio/utils/ThreadsafeQueue.h" +#include "kimera-vio/backend/VioBackEnd-definitions.h" #include "kimera-vio/common/vio_types.h" -#include "kimera-vio/pipeline/PipelinePayload.h" #include "kimera-vio/frontend/StereoVisionFrontEnd-definitions.h" -#include "kimera-vio/backend/VioBackEnd-definitions.h" #include "kimera-vio/mesh/Mesher-definitions.h" +#include "kimera-vio/pipeline/PipelinePayload.h" #include "kimera-vio/utils/Macros.h" +#include "kimera-vio/utils/ThreadsafeQueue.h" namespace VIO { @@ -88,24 +88,25 @@ struct DisplayInputBase { EIGEN_MAKE_ALIGNED_OPERATOR_NEW DisplayInputBase() = default; virtual ~DisplayInputBase() = default; + + std::vector images_to_display_; }; +typedef ThreadsafeQueue DisplayQueue; struct VisualizerOutput : public DisplayInputBase { KIMERA_POINTER_TYPEDEFS(VisualizerOutput); KIMERA_DELETE_COPY_CONSTRUCTORS(VisualizerOutput); EIGEN_MAKE_ALIGNED_OPERATOR_NEW VisualizerOutput() - : visualization_type_(VisualizationType::kNone), - images_to_display_(), + : DisplayInputBase(), + visualization_type_(VisualizationType::kNone), widgets_(), frustum_pose_(cv::Affine3d::Identity()) {} ~VisualizerOutput() = default; VisualizationType visualization_type_; - std::vector images_to_display_; WidgetsMap widgets_; cv::Affine3d frustum_pose_; }; -typedef ThreadsafeQueue DisplayQueue; } // namespace VIO diff --git a/include/kimera-vio/visualizer/Visualizer3D.h b/include/kimera-vio/visualizer/Visualizer3D.h index 893abc6ad..cc839e287 100644 --- a/include/kimera-vio/visualizer/Visualizer3D.h +++ b/include/kimera-vio/visualizer/Visualizer3D.h @@ -54,7 +54,7 @@ class Visualizer3D { */ Visualizer3D(const VisualizationType& viz_type, const BackendType& backend_type); - virtual ~Visualizer3D() { LOG(INFO) << "Visualizer3D destructor"; }; + virtual ~Visualizer3D() { LOG(INFO) << "Visualizer3D destructor"; } /* ------------------------------------------------------------------------ */ inline void registerMesh3dVizProperties( diff --git a/include/kimera-vio/visualizer/Visualizer3DModule.h b/include/kimera-vio/visualizer/Visualizer3DModule.h index 3cbb0a26f..7c13eafa8 100644 --- a/include/kimera-vio/visualizer/Visualizer3DModule.h +++ b/include/kimera-vio/visualizer/Visualizer3DModule.h @@ -30,11 +30,11 @@ namespace VIO { class VisualizerModule - : public MISOPipelineModule { + : public MISOPipelineModule { public: KIMERA_POINTER_TYPEDEFS(VisualizerModule); KIMERA_DELETE_COPY_CONSTRUCTORS(VisualizerModule); - using MISO = MISOPipelineModule; + using MISO = MISOPipelineModule; using VizFrontendInput = FrontendOutput::Ptr; using VizBackendInput = BackendOutput::Ptr; diff --git a/src/visualizer/Display.cpp b/src/visualizer/Display.cpp index e333a167d..ee276a8f3 100644 --- a/src/visualizer/Display.cpp +++ b/src/visualizer/Display.cpp @@ -54,16 +54,17 @@ OpenCv3dDisplay::OpenCv3dDisplay( void OpenCv3dDisplay::spinOnce(DisplayInputBase::UniquePtr&& display_input) { CHECK(display_input); - VisualizerOutput::UniquePtr viz_output = safeCast(std::move(display_input)); // Display 2D images. - spin2dWindow(*viz_output); + spin2dWindow(*display_input); // Display 3D window. - spin3dWindow(std::move(viz_output)); + spin3dWindow(safeCast(std::move(display_input))); } // Adds 3D widgets to the window, and displays it. void OpenCv3dDisplay::spin3dWindow(VisualizerOutput::UniquePtr&& viz_output) { - CHECK(viz_output); + // Only display if we have a valid pointer. + if (!viz_output) return; + if (viz_output->visualization_type_ != VisualizationType::kNone) { if (window_data_.window_.wasStopped()) { // Shutdown the pipeline! This works because display is running in the @@ -85,7 +86,7 @@ void OpenCv3dDisplay::spin3dWindow(VisualizerOutput::UniquePtr&& viz_output) { } } -void OpenCv3dDisplay::spin2dWindow(const VisualizerOutput& viz_output) { +void OpenCv3dDisplay::spin2dWindow(const DisplayInputBase& viz_output) { for (const ImageToDisplay& img_to_display : viz_output.images_to_display_) { cv::namedWindow(img_to_display.name_); cv::imshow(img_to_display.name_, img_to_display.image_); diff --git a/src/visualizer/Visualizer3DModule.cpp b/src/visualizer/Visualizer3DModule.cpp index 53760f722..21a256fda 100644 --- a/src/visualizer/Visualizer3DModule.cpp +++ b/src/visualizer/Visualizer3DModule.cpp @@ -24,7 +24,7 @@ namespace VIO { VisualizerModule::VisualizerModule(OutputQueue* output_queue, bool parallel_run, Visualizer3D::UniquePtr visualizer) - : MISOPipelineModule(output_queue, + : MISOPipelineModule(output_queue, "Visualizer", parallel_run), frontend_queue_("visualizer_frontend_queue"), From 9fe6085c65ec16073ab506d16939be60a9fd993a Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 23:07:37 -0400 Subject: [PATCH 06/15] Add timestamp to DisplayInputBase --- include/kimera-vio/visualizer/Display-definitions.h | 4 +++- include/kimera-vio/visualizer/Visualizer3D-definitions.h | 1 + src/frontend/StereoVisionFrontEnd.cpp | 3 ++- src/frontend/Tracker.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 6 deletions(-) diff --git a/include/kimera-vio/visualizer/Display-definitions.h b/include/kimera-vio/visualizer/Display-definitions.h index 198671553..1a7895a56 100644 --- a/include/kimera-vio/visualizer/Display-definitions.h +++ b/include/kimera-vio/visualizer/Display-definitions.h @@ -57,11 +57,13 @@ struct WindowData { * @param display_queue Threadsafe queue were images will be pushed and that * needs to be consumed by a display thread. */ -inline void displayImage(const std::string& title, +inline void displayImage(const Timestamp& timestamp, + const std::string& title, const cv::Mat& img, DisplayQueue* display_queue) { DisplayInputBase::UniquePtr visualizer_output = VIO::make_unique(); + visualizer_output->timestamp_ = timestamp; ImageToDisplay img_to_display(title, img); visualizer_output->images_to_display_.push_back(img_to_display); if (display_queue) { diff --git a/include/kimera-vio/visualizer/Visualizer3D-definitions.h b/include/kimera-vio/visualizer/Visualizer3D-definitions.h index 51440ca75..b48df34d8 100644 --- a/include/kimera-vio/visualizer/Visualizer3D-definitions.h +++ b/include/kimera-vio/visualizer/Visualizer3D-definitions.h @@ -89,6 +89,7 @@ struct DisplayInputBase { DisplayInputBase() = default; virtual ~DisplayInputBase() = default; + Timestamp timestamp_; std::vector images_to_display_; }; typedef ThreadsafeQueue DisplayQueue; diff --git a/src/frontend/StereoVisionFrontEnd.cpp b/src/frontend/StereoVisionFrontEnd.cpp index 2f6c402da..343d9bdde 100644 --- a/src/frontend/StereoVisionFrontEnd.cpp +++ b/src/frontend/StereoVisionFrontEnd.cpp @@ -362,7 +362,8 @@ StatusStereoMeasurementsPtr StereoVisionFrontEnd::processStereoFrame( if (FLAGS_log_stereo_matching_images) sendMonoTrackingToLogger(); } if (display_queue_ && FLAGS_visualize_feature_tracks) { - displayImage("feature_tracks", + displayImage(stereoFrame_k_->getTimestamp(), + "feature_tracks", tracker_.getTrackerImage(stereoFrame_lkf_->getLeftFrame(), stereoFrame_k_->getLeftFrame()), display_queue_); diff --git a/src/frontend/Tracker.cpp b/src/frontend/Tracker.cpp index c4fdf892d..34cf88a3c 100644 --- a/src/frontend/Tracker.cpp +++ b/src/frontend/Tracker.cpp @@ -146,9 +146,8 @@ void Tracker::featureTracking(Frame* ref_frame, const size_t& lmk_age = ref_frame->landmarks_age_[idx_valid_lmk]; const LandmarkId& lmk_id = ref_frame->landmarks_[idx_valid_lmk]; - // if we tracked keypoint and feature track is not too long - if (!status[i] || - lmk_age > tracker_params_.maxFeatureAge_) { + // if we tracked keypoint and feature track is not too long + if (!status[i] || lmk_age > tracker_params_.maxFeatureAge_) { // we are marking this bad in the ref_frame since features // in the ref frame guide feature detection later on ref_frame->landmarks_[idx_valid_lmk] = -1; @@ -174,7 +173,8 @@ void Tracker::featureTracking(Frame* ref_frame, << " vs. maxFeatureAge_: " << tracker_params_.maxFeatureAge_ << ")"; // Display feature tracks together with predicted points. if (display_queue_ && FLAGS_visualize_feature_predictions) { - displayImage("Feature Tracks With Predicted Keypoints", + displayImage(cur_frame->timestamp_, + "feature_tracks_with_predicted_keypoints", getTrackerImage(*ref_frame, *cur_frame, px_predicted, px_ref), display_queue_); } From d7d193e19da0315bf79b3f849700afcdc4bb8d11 Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 10 May 2020 23:59:30 -0400 Subject: [PATCH 07/15] Add Visualizer3D base class --- include/kimera-vio/visualizer/Visualizer3D.h | 35 +++++++++--- src/imu-frontend/ImuFrontEnd.cpp | 7 ++- src/visualizer/Visualizer3D.cpp | 57 ++++++++++---------- src/visualizer/Visualizer3DFactory.cpp | 2 +- 4 files changed, 60 insertions(+), 41 deletions(-) diff --git a/include/kimera-vio/visualizer/Visualizer3D.h b/include/kimera-vio/visualizer/Visualizer3D.h index cc839e287..728ebeefc 100644 --- a/include/kimera-vio/visualizer/Visualizer3D.h +++ b/include/kimera-vio/visualizer/Visualizer3D.h @@ -41,6 +41,28 @@ class Visualizer3D { KIMERA_DELETE_COPY_CONSTRUCTORS(Visualizer3D); KIMERA_POINTER_TYPEDEFS(Visualizer3D); EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief Visualizer3D base constructor + * @param viz_type: type of 3D visualization + */ + Visualizer3D(const VisualizationType& viz_type) + : visualization_type_(viz_type) {} + virtual ~Visualizer3D() = default; + + public: + virtual VisualizerOutput::UniquePtr spinOnce( + const VisualizerInput& input) = 0; + + public: + VisualizationType visualization_type_; +}; + +class OpenCvVisualizer3D : public Visualizer3D { + public: + KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCvVisualizer3D); + KIMERA_POINTER_TYPEDEFS(OpenCvVisualizer3D); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef size_t LineNr; typedef std::uint64_t PlaneId; typedef std::map LmkIdToLineIdMap; @@ -52,9 +74,11 @@ class Visualizer3D { * @param viz_type: type of 3D visualization * @param backend_type backend used so that we display the right info */ - Visualizer3D(const VisualizationType& viz_type, - const BackendType& backend_type); - virtual ~Visualizer3D() { LOG(INFO) << "Visualizer3D destructor"; } + OpenCvVisualizer3D(const VisualizationType& viz_type, + const BackendType& backend_type); + virtual ~OpenCvVisualizer3D() { + LOG(INFO) << "OpenCvVisualizer3D destructor"; + } /* ------------------------------------------------------------------------ */ inline void registerMesh3dVizProperties( @@ -67,7 +91,7 @@ class Visualizer3D { * The actual visualization must be done in the main thread, and as such, * it is not done here to separate visualization preparation from display. */ - virtual VisualizerOutput::UniquePtr spinOnce(const VisualizerInput& input); + VisualizerOutput::UniquePtr spinOnce(const VisualizerInput& input) override; // TODO(marcus): Is there any reason the following two methods must be // private? @@ -213,9 +237,6 @@ class Visualizer3D { const Mesh2D& mesh_2d, const Mesh3D& mesh_3d); - public: - VisualizationType visualization_type_; - private: // Flags for visualization behaviour. const BackendType backend_type_; diff --git a/src/imu-frontend/ImuFrontEnd.cpp b/src/imu-frontend/ImuFrontEnd.cpp index 2560ae40b..46be3de48 100644 --- a/src/imu-frontend/ImuFrontEnd.cpp +++ b/src/imu-frontend/ImuFrontEnd.cpp @@ -95,7 +95,8 @@ ImuFrontEnd::PimPtr ImuFrontEnd::preintegrateImuMeasurements( // measurement. Nevertheless the imu_stamps, should be shifted one step back // I would say. for (int i = 0; i < imu_stamps.cols() - 1; ++i) { - const gtsam::Vector3& measured_acc = imu_accgyr.block<3, 1>(0, i); + gtsam::Vector3 measured_acc = imu_accgyr.block<3, 1>(0, i); + const gtsam::Vector3& measured_omega = imu_accgyr.block<3, 1>(3, i); const double& delta_t = UtilsNumerical::NsecToSec(imu_stamps(i + 1) - imu_stamps(i)); @@ -123,9 +124,7 @@ ImuFrontEnd::PimPtr ImuFrontEnd::preintegrateImuMeasurements( return VIO::make_unique( safeCastToPreintegratedImuMeasurements(*pim_)); } - default: { - LOG(FATAL) << "Unknown IMU Preintegration Type."; - } + default: { LOG(FATAL) << "Unknown IMU Preintegration Type."; } } } diff --git a/src/visualizer/Visualizer3D.cpp b/src/visualizer/Visualizer3D.cpp index 4cb5b93ae..1b714e83a 100644 --- a/src/visualizer/Visualizer3D.cpp +++ b/src/visualizer/Visualizer3D.cpp @@ -34,7 +34,6 @@ #include "kimera-vio/factors/PointPlaneFactor.h" // For visualization of constraints. - // TODO(Toni): remove visualizer gflags! There are far too many, use a // yaml params class (aka inherit from PipelineParams. DEFINE_bool(visualize_mesh, false, "Enable 3D mesh visualization."); @@ -82,9 +81,9 @@ DEFINE_int32(displayed_trajectory_length, namespace VIO { /* -------------------------------------------------------------------------- */ -Visualizer3D::Visualizer3D(const VisualizationType& viz_type, +OpenCvVisualizer3D::OpenCvVisualizer3D(const VisualizationType& viz_type, const BackendType& backend_type) - : visualization_type_(viz_type), + : Visualizer3D(viz_type), backend_type_(backend_type), logger_(nullptr) { if (FLAGS_log_mesh) { @@ -95,7 +94,7 @@ Visualizer3D::Visualizer3D(const VisualizationType& viz_type, /* -------------------------------------------------------------------------- */ // Returns true if visualization is ready, false otherwise. // TODO(Toni): Put all flags inside spinOnce into Visualizer3DParams! -VisualizerOutput::UniquePtr Visualizer3D::spinOnce( +VisualizerOutput::UniquePtr OpenCvVisualizer3D::spinOnce( const VisualizerInput& input) { DCHECK(input.frontend_output_); DCHECK(input.backend_output_); @@ -327,7 +326,7 @@ VisualizerOutput::UniquePtr Visualizer3D::spinOnce( left_stereo_keyframe.img_, input.mesher_output_->mesh_2d_, input.mesher_output_->mesh_3d_) - : (FLAGS_texturize_3d_mesh ? Visualizer3D::texturizeMesh3D( + : (FLAGS_texturize_3d_mesh ? OpenCvVisualizer3D::texturizeMesh3D( left_stereo_keyframe.timestamp_, left_stereo_keyframe.img_, input.mesher_output_->mesh_2d_, @@ -367,7 +366,7 @@ VisualizerOutput::UniquePtr Visualizer3D::spinOnce( /* -------------------------------------------------------------------------- */ // Create a 2D mesh from 2D corners in an image -cv::Mat Visualizer3D::visualizeMesh2D( +cv::Mat OpenCvVisualizer3D::visualizeMesh2D( const std::vector& triangulation2D, const cv::Mat& img, const KeypointsCV& extra_keypoints) { @@ -404,7 +403,7 @@ cv::Mat Visualizer3D::visualizeMesh2D( /* -------------------------------------------------------------------------- */ // Visualize 2d mesh. -cv::Mat Visualizer3D::visualizeMesh2DStereo( +cv::Mat OpenCvVisualizer3D::visualizeMesh2DStereo( const std::vector& triangulation_2D, const Frame& ref_frame) { static const cv::Scalar kDelaunayColor(0, 255, 0); // Green @@ -456,7 +455,7 @@ cv::Mat Visualizer3D::visualizeMesh2DStereo( /* -------------------------------------------------------------------------- */ // Visualize a 3D point cloud of unique 3D landmarks. -void Visualizer3D::visualizePoints3D( +void OpenCvVisualizer3D::visualizePoints3D( const PointsWithIdMap& points_with_id, const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map, WidgetsMap* widgets_map) { @@ -520,7 +519,7 @@ void Visualizer3D::visualizePoints3D( /* -------------------------------------------------------------------------- */ // Visualize a 3D point cloud of unique 3D landmarks with its connectivity. -void Visualizer3D::visualizePlane(const PlaneId& plane_index, +void OpenCvVisualizer3D::visualizePlane(const PlaneId& plane_index, const double& n_x, const double& n_y, const double& n_z, @@ -556,7 +555,7 @@ void Visualizer3D::visualizePlane(const PlaneId& plane_index, /* -------------------------------------------------------------------------- */ // Draw a line in opencv. -void Visualizer3D::drawLine(const std::string& line_id, +void OpenCvVisualizer3D::drawLine(const std::string& line_id, const double& from_x, const double& from_y, const double& from_z, @@ -570,7 +569,7 @@ void Visualizer3D::drawLine(const std::string& line_id, } /* -------------------------------------------------------------------------- */ -void Visualizer3D::drawLine(const std::string& line_id, +void OpenCvVisualizer3D::drawLine(const std::string& line_id, const cv::Point3d& pt1, const cv::Point3d& pt2, WidgetsMap* widgets) { @@ -580,7 +579,7 @@ void Visualizer3D::drawLine(const std::string& line_id, /* -------------------------------------------------------------------------- */ // Visualize a 3D point cloud of unique 3D landmarks with its connectivity. -void Visualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, +void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, const cv::Mat& polygons_mesh, WidgetsMap* widgets) { cv::Mat colors(0, 1, CV_8UC3, cv::viz::Color::gray()); // Do not color mesh. @@ -590,7 +589,7 @@ void Visualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, /* -------------------------------------------------------------------------- */ // Visualize a 3D point cloud of unique 3D landmarks with its connectivity, // and provide color for each polygon. -void Visualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, +void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, const cv::Mat& colors, const cv::Mat& polygons_mesh, WidgetsMap* widgets, @@ -632,7 +631,7 @@ void Visualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, /* -------------------------------------------------------------------------- */ // Visualize a PLY from filename (absolute path). -void Visualizer3D::visualizePlyMesh(const std::string& filename, +void OpenCvVisualizer3D::visualizePlyMesh(const std::string& filename, WidgetsMap* widgets) { CHECK_NOTNULL(widgets); LOG(INFO) << "Showing ground truth mesh: " << filename; @@ -672,7 +671,7 @@ void Visualizer3D::visualizePlyMesh(const std::string& filename, /// n=3 for triangles. /// [in] color_mesh whether to color the mesh or not /// [in] timestamp to store the timestamp of the mesh when logging the mesh. -void Visualizer3D::visualizeMesh3DWithColoredClusters( +void OpenCvVisualizer3D::visualizeMesh3DWithColoredClusters( const std::vector& planes, const cv::Mat& map_points_3d, const cv::Mat& polygons_mesh, @@ -702,7 +701,7 @@ void Visualizer3D::visualizeMesh3DWithColoredClusters( /* -------------------------------------------------------------------------- */ // Visualize convex hull in 2D for set of points in triangle cluster, // projected along the normal of the cluster. -void Visualizer3D::visualizeConvexHull(const TriangleCluster& cluster, +void OpenCvVisualizer3D::visualizeConvexHull(const TriangleCluster& cluster, const cv::Mat& map_points_3d, const cv::Mat& polygons_mesh, WidgetsMap* widgets_map) { @@ -818,7 +817,7 @@ void Visualizer3D::visualizeConvexHull(const TriangleCluster& cluster, /* -------------------------------------------------------------------------- */ // Visualize trajectory. Adds an image to the frustum if cv::Mat is not empty. -void Visualizer3D::visualizeTrajectory3D(const cv::Mat& frustum_image, +void OpenCvVisualizer3D::visualizeTrajectory3D(const cv::Mat& frustum_image, cv::Affine3d* frustum_pose, WidgetsMap* widgets_map) { CHECK_NOTNULL(frustum_pose); @@ -887,7 +886,7 @@ void Visualizer3D::visualizeTrajectory3D(const cv::Mat& frustum_image, /* -------------------------------------------------------------------------- */ // Remove widget. True if successful, false if not. -bool Visualizer3D::removeWidget(const std::string& widget_id) { +bool OpenCvVisualizer3D::removeWidget(const std::string& widget_id) { try { // window_data_.window_.removeWidget(widget_id); return true; @@ -906,7 +905,7 @@ bool Visualizer3D::removeWidget(const std::string& widget_id) { /* -------------------------------------------------------------------------- */ // Visualize line widgets from plane to lmks. // Point key is required to avoid duplicated lines! -void Visualizer3D::visualizePlaneConstraints(const PlaneId& plane_id, +void OpenCvVisualizer3D::visualizePlaneConstraints(const PlaneId& plane_id, const gtsam::Point3& normal, const double& distance, const LandmarkId& lmk_id, @@ -981,7 +980,7 @@ void Visualizer3D::visualizePlaneConstraints(const PlaneId& plane_id, /* -------------------------------------------------------------------------- */ // Remove line widgets from plane to lmks, for lines that are not pointing // to any lmk_id in lmk_ids. -void Visualizer3D::removeOldLines(const LandmarkIds& lmk_ids) { +void OpenCvVisualizer3D::removeOldLines(const LandmarkIds& lmk_ids) { for (PlaneIdMap::value_type& plane_id_pair : plane_id_map_) { LmkIdToLineIdMap& lmk_id_to_line_id_map = plane_id_pair.second; for (LmkIdToLineIdMap::iterator lmk_id_to_line_id_it = @@ -1009,7 +1008,7 @@ void Visualizer3D::removeOldLines(const LandmarkIds& lmk_ids) { /* -------------------------------------------------------------------------- */ // Remove line widgets from plane to lmks. -void Visualizer3D::removePlaneConstraintsViz(const PlaneId& plane_id) { +void OpenCvVisualizer3D::removePlaneConstraintsViz(const PlaneId& plane_id) { PlaneIdMap::iterator plane_id_it = plane_id_map_.find(plane_id); if (plane_id_it != plane_id_map_.end()) { VLOG(0) << "Removing line constraints for plane with id: " << plane_id; @@ -1035,7 +1034,7 @@ void Visualizer3D::removePlaneConstraintsViz(const PlaneId& plane_id) { /* -------------------------------------------------------------------------- */ // Remove plane widget. -void Visualizer3D::removePlane(const PlaneId& plane_index, +void OpenCvVisualizer3D::removePlane(const PlaneId& plane_index, const bool& remove_plane_label) { const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); if (is_plane_id_in_window_.find(plane_index) != @@ -1058,7 +1057,7 @@ void Visualizer3D::removePlane(const PlaneId& plane_index, /* -------------------------------------------------------------------------- */ // Add pose to the previous trajectory. -void Visualizer3D::addPoseToTrajectory(const gtsam::Pose3& current_pose_gtsam) { +void OpenCvVisualizer3D::addPoseToTrajectory(const gtsam::Pose3& current_pose_gtsam) { trajectory_poses_3d_.push_back( UtilsOpenCV::gtsamPose3ToCvAffine3d(current_pose_gtsam)); if (FLAGS_displayed_trajectory_length > 0) { @@ -1069,7 +1068,7 @@ void Visualizer3D::addPoseToTrajectory(const gtsam::Pose3& current_pose_gtsam) { } /* ------------------------------------------------------------------------ */ -Mesh3DVizProperties Visualizer3D::texturizeMesh3D( +Mesh3DVizProperties OpenCvVisualizer3D::texturizeMesh3D( const Timestamp& image_timestamp, const cv::Mat& texture_image, const Mesh2D& mesh_2d, @@ -1169,7 +1168,7 @@ Mesh3DVizProperties Visualizer3D::texturizeMesh3D( /* -------------------------------------------------------------------------- */ // Log mesh to ply file. -void Visualizer3D::logMesh(const cv::Mat& map_points_3d, +void OpenCvVisualizer3D::logMesh(const cv::Mat& map_points_3d, const cv::Mat& colors, const cv::Mat& polygons_mesh, const Timestamp& timestamp, @@ -1192,7 +1191,7 @@ void Visualizer3D::logMesh(const cv::Mat& map_points_3d, // Input the mesh points and triangle clusters, and // output colors matrix for mesh visualizer. // This will color the point with the color of the last plane having it. -void Visualizer3D::colorMeshByClusters(const std::vector& planes, +void OpenCvVisualizer3D::colorMeshByClusters(const std::vector& planes, const cv::Mat& map_points_3d, const cv::Mat& polygons_mesh, cv::Mat* colors) const { @@ -1224,7 +1223,7 @@ void Visualizer3D::colorMeshByClusters(const std::vector& planes, /* -------------------------------------------------------------------------- */ // Decide color of the cluster depending on its id. -void Visualizer3D::getColorById(const size_t& id, cv::viz::Color* color) const { +void OpenCvVisualizer3D::getColorById(const size_t& id, cv::viz::Color* color) const { CHECK_NOTNULL(color); switch (id) { case 0: { @@ -1248,7 +1247,7 @@ void Visualizer3D::getColorById(const size_t& id, cv::viz::Color* color) const { /* -------------------------------------------------------------------------- */ // Draw a line from lmk to plane center. -void Visualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, +void OpenCvVisualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, const double& plane_n_x, const double& plane_n_y, const double& plane_n_z, @@ -1265,7 +1264,7 @@ void Visualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, /* -------------------------------------------------------------------------- */ // Update line from lmk to plane center. -void Visualizer3D::updateLineFromPlaneToPoint(const std::string& line_id, +void OpenCvVisualizer3D::updateLineFromPlaneToPoint(const std::string& line_id, const double& plane_n_x, const double& plane_n_y, const double& plane_n_z, diff --git a/src/visualizer/Visualizer3DFactory.cpp b/src/visualizer/Visualizer3DFactory.cpp index 3fd69b850..d1798d9e0 100644 --- a/src/visualizer/Visualizer3DFactory.cpp +++ b/src/visualizer/Visualizer3DFactory.cpp @@ -22,7 +22,7 @@ Visualizer3D::UniquePtr VisualizerFactory::createVisualizer( const BackendType& backend_type) { switch (visualizer_type) { case VisualizerType::OpenCV: { - return VIO::make_unique(viz_type, backend_type); + return VIO::make_unique(viz_type, backend_type); } default: { LOG(FATAL) << "Requested visualizer type is not supported.\n" From 0389187e0fb187374cd03507ed0dcd3af2c75db0 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 00:36:44 -0400 Subject: [PATCH 08/15] Accept external visualizer: ros --- include/kimera-vio/pipeline/Pipeline.h | 6 +++++- include/kimera-vio/visualizer/Visualizer3D.h | 6 +++--- .../visualizer/Visualizer3DFactory.h | 2 +- .../kimera-vio/visualizer/Visualizer3DModule.h | 4 ++-- src/pipeline/Pipeline.cpp | 18 ++++++++++++------ src/visualizer/Visualizer3DFactory.cpp | 2 +- src/visualizer/Visualizer3DModule.cpp | 2 +- tests/testVisualizer3D.cpp | 4 ++-- 8 files changed, 27 insertions(+), 17 deletions(-) diff --git a/include/kimera-vio/pipeline/Pipeline.h b/include/kimera-vio/pipeline/Pipeline.h index e50c94762..c5d0d7d03 100644 --- a/include/kimera-vio/pipeline/Pipeline.h +++ b/include/kimera-vio/pipeline/Pipeline.h @@ -33,7 +33,9 @@ #include "kimera-vio/mesh/MesherModule.h" #include "kimera-vio/pipeline/Pipeline-definitions.h" #include "kimera-vio/utils/ThreadsafeQueue.h" +#include "kimera-vio/visualizer/Display.h" // TODO(Toni): separate ocv display #include "kimera-vio/visualizer/DisplayModule.h" +#include "kimera-vio/visualizer/Visualizer3D.h" // TODO(Toni): separate ocv viz #include "kimera-vio/visualizer/Visualizer3DModule.h" namespace VIO { @@ -48,9 +50,11 @@ class Pipeline { /** * @brief Pipeline * @param params Vio parameters - * @param displayer Optional displayer for visualizing results + * @param visualizer Optional visualizer for visualizing 3D results + * @param displayer Optional displayer for visualizing 2D results */ Pipeline(const VioParams& params, + Visualizer3D::UniquePtr&& visualizer = nullptr, DisplayBase::UniquePtr&& displayer = nullptr); virtual ~Pipeline(); diff --git a/include/kimera-vio/visualizer/Visualizer3D.h b/include/kimera-vio/visualizer/Visualizer3D.h index 728ebeefc..2544701fc 100644 --- a/include/kimera-vio/visualizer/Visualizer3D.h +++ b/include/kimera-vio/visualizer/Visualizer3D.h @@ -36,10 +36,10 @@ namespace VIO { -class Visualizer3D { +class OpenCvVisualizer3D { public: - KIMERA_DELETE_COPY_CONSTRUCTORS(Visualizer3D); - KIMERA_POINTER_TYPEDEFS(Visualizer3D); + KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCvVisualizer3D); + KIMERA_POINTER_TYPEDEFS(OpenCvVisualizer3D); EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** diff --git a/include/kimera-vio/visualizer/Visualizer3DFactory.h b/include/kimera-vio/visualizer/Visualizer3DFactory.h index 276b68871..5b422149e 100644 --- a/include/kimera-vio/visualizer/Visualizer3DFactory.h +++ b/include/kimera-vio/visualizer/Visualizer3DFactory.h @@ -30,7 +30,7 @@ class VisualizerFactory { VisualizerFactory() = delete; virtual ~VisualizerFactory() = default; - static Visualizer3D::UniquePtr createVisualizer( + static OpenCvVisualizer3D::UniquePtr createVisualizer( const VisualizerType visualizer_type, const VisualizationType& viz_type, const BackendType& backend_type); diff --git a/include/kimera-vio/visualizer/Visualizer3DModule.h b/include/kimera-vio/visualizer/Visualizer3DModule.h index 7c13eafa8..99e23381d 100644 --- a/include/kimera-vio/visualizer/Visualizer3DModule.h +++ b/include/kimera-vio/visualizer/Visualizer3DModule.h @@ -42,7 +42,7 @@ class VisualizerModule VisualizerModule(OutputQueue* output_queue, bool parallel_run, - Visualizer3D::UniquePtr visualizer); + OpenCvVisualizer3D::UniquePtr visualizer); virtual ~VisualizerModule() = default; //! Callbacks to fill queues: they should be all lighting fast. @@ -78,7 +78,7 @@ class VisualizerModule ThreadsafeQueue::UniquePtr mesher_queue_; //! Visualizer implementation - Visualizer3D::UniquePtr visualizer_; + OpenCvVisualizer3D::UniquePtr visualizer_; }; } // namespace VIO diff --git a/src/pipeline/Pipeline.cpp b/src/pipeline/Pipeline.cpp index 84d96f076..5d0aa380e 100644 --- a/src/pipeline/Pipeline.cpp +++ b/src/pipeline/Pipeline.cpp @@ -34,6 +34,7 @@ #include "kimera-vio/utils/Statistics.h" #include "kimera-vio/utils/Timer.h" #include "kimera-vio/visualizer/DisplayFactory.h" +#include "kimera-vio/visualizer/Visualizer3D.h" #include "kimera-vio/visualizer/Visualizer3DFactory.h" DEFINE_bool(log_output, false, "Log output to CSV files."); @@ -93,7 +94,9 @@ DEFINE_bool(use_lcd, namespace VIO { -Pipeline::Pipeline(const VioParams& params, DisplayBase::UniquePtr&& displayer) +Pipeline::Pipeline(const VioParams& params, + Visualizer3D::UniquePtr&& visualizer, + DisplayBase::UniquePtr&& displayer) : backend_type_(static_cast(params.backend_type_)), stereo_camera_(nullptr), data_provider_module_(nullptr), @@ -217,11 +220,14 @@ Pipeline::Pipeline(const VioParams& params, DisplayBase::UniquePtr&& displayer) //! Send ouput of visualizer to the display_input_queue_ &display_input_queue_, parallel_run_, - VisualizerFactory::createVisualizer( - VisualizerType::OpenCV, - // TODO(Toni): bundle these three params in VisualizerParams... - static_cast(FLAGS_viz_type), - backend_type_)); + // Use given visualizer if any + visualizer ? std::move(visualizer) + : VisualizerFactory::createVisualizer( + VisualizerType::OpenCV, + // TODO(Toni): bundle these three params in + // VisualizerParams... + static_cast(FLAGS_viz_type), + backend_type_)); //! Register input callbacks vio_backend_module_->registerOutputCallback( std::bind(&VisualizerModule::fillBackendQueue, diff --git a/src/visualizer/Visualizer3DFactory.cpp b/src/visualizer/Visualizer3DFactory.cpp index d1798d9e0..566ab49c2 100644 --- a/src/visualizer/Visualizer3DFactory.cpp +++ b/src/visualizer/Visualizer3DFactory.cpp @@ -16,7 +16,7 @@ namespace VIO { -Visualizer3D::UniquePtr VisualizerFactory::createVisualizer( +OpenCvVisualizer3D::UniquePtr VisualizerFactory::createVisualizer( const VisualizerType visualizer_type, const VisualizationType& viz_type, const BackendType& backend_type) { diff --git a/src/visualizer/Visualizer3DModule.cpp b/src/visualizer/Visualizer3DModule.cpp index 21a256fda..262ece5cb 100644 --- a/src/visualizer/Visualizer3DModule.cpp +++ b/src/visualizer/Visualizer3DModule.cpp @@ -23,7 +23,7 @@ namespace VIO { VisualizerModule::VisualizerModule(OutputQueue* output_queue, bool parallel_run, - Visualizer3D::UniquePtr visualizer) + OpenCvVisualizer3D::UniquePtr visualizer) : MISOPipelineModule(output_queue, "Visualizer", parallel_run), diff --git a/tests/testVisualizer3D.cpp b/tests/testVisualizer3D.cpp index 7614604b6..69c506da3 100644 --- a/tests/testVisualizer3D.cpp +++ b/tests/testVisualizer3D.cpp @@ -55,7 +55,7 @@ class VisualizerFixture : public ::testing::Test { img_ = UtilsOpenCV::ReadAndConvertToGrayScale(img_name_); // Construct a frame from image name, and extract keypoints/landmarks. frame_ = constructFrame(true); - visualizer_ = VIO::make_unique(viz_type_, backend_type_); + visualizer_ = VIO::make_unique(viz_type_, backend_type_); } protected: @@ -100,7 +100,7 @@ class VisualizerFixture : public ::testing::Test { std::unique_ptr frame_; VisualizationType viz_type_; BackendType backend_type_; - Visualizer3D::UniquePtr visualizer_; + OpenCvVisualizer3D::UniquePtr visualizer_; }; TEST_F(VisualizerFixture, spinOnce) { From d3af0a6f3b2ef06884157419e9cbfa99e2d874af Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 01:16:33 -0400 Subject: [PATCH 09/15] Send implementation of visualizer to cpp for vtable issue --- include/kimera-vio/visualizer/Visualizer3D.h | 13 +- .../visualizer/Visualizer3DModule.h | 15 +- src/visualizer/Visualizer3D.cpp | 137 +++++++++--------- src/visualizer/Visualizer3DModule.cpp | 4 +- 4 files changed, 84 insertions(+), 85 deletions(-) diff --git a/include/kimera-vio/visualizer/Visualizer3D.h b/include/kimera-vio/visualizer/Visualizer3D.h index 2544701fc..259df3d89 100644 --- a/include/kimera-vio/visualizer/Visualizer3D.h +++ b/include/kimera-vio/visualizer/Visualizer3D.h @@ -8,8 +8,8 @@ /** * @file Visualizer.h - * @brief Build and visualize 2D mesh from Frame - * @author Antoni Rosinol, AJ Haeffner, Luca Carlone + * @brief Build and visualize 3D data: 2D mesh from Frame for example. + * @author Antoni Rosinol */ #pragma once @@ -36,18 +36,17 @@ namespace VIO { -class OpenCvVisualizer3D { +class Visualizer3D { public: - KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCvVisualizer3D); - KIMERA_POINTER_TYPEDEFS(OpenCvVisualizer3D); + KIMERA_DELETE_COPY_CONSTRUCTORS(Visualizer3D); + KIMERA_POINTER_TYPEDEFS(Visualizer3D); EIGEN_MAKE_ALIGNED_OPERATOR_NEW /** * @brief Visualizer3D base constructor * @param viz_type: type of 3D visualization */ - Visualizer3D(const VisualizationType& viz_type) - : visualization_type_(viz_type) {} + Visualizer3D(const VisualizationType& viz_type); virtual ~Visualizer3D() = default; public: diff --git a/include/kimera-vio/visualizer/Visualizer3DModule.h b/include/kimera-vio/visualizer/Visualizer3DModule.h index 99e23381d..2241c2601 100644 --- a/include/kimera-vio/visualizer/Visualizer3DModule.h +++ b/include/kimera-vio/visualizer/Visualizer3DModule.h @@ -22,11 +22,10 @@ #include "kimera-vio/mesh/Mesher-definitions.h" #include "kimera-vio/pipeline/PipelineModule.h" #include "kimera-vio/utils/Macros.h" +#include "kimera-vio/utils/ThreadsafeQueue.h" #include "kimera-vio/visualizer/Visualizer3D-definitions.h" #include "kimera-vio/visualizer/Visualizer3D.h" -#include - namespace VIO { class VisualizerModule @@ -42,7 +41,7 @@ class VisualizerModule VisualizerModule(OutputQueue* output_queue, bool parallel_run, - OpenCvVisualizer3D::UniquePtr visualizer); + Visualizer3D::UniquePtr visualizer); virtual ~VisualizerModule() = default; //! Callbacks to fill queues: they should be all lighting fast. @@ -60,15 +59,15 @@ class VisualizerModule //! then loop over the other queues until you get a payload that has exactly //! the same timestamp. Guaranteed to sync messages unless the assumption //! on the order of msg generation is broken. - virtual inline InputUniquePtr getInputPacket() override; + inline InputUniquePtr getInputPacket() override; - virtual OutputUniquePtr spinOnce(VisualizerInput::UniquePtr input) override; + OutputUniquePtr spinOnce(VisualizerInput::UniquePtr input) override; //! Called when general shutdown of PipelineModule is triggered. - virtual void shutdownQueues() override; + void shutdownQueues() override; //! Checks if the module has work to do (should check input queues are empty) - virtual bool hasWork() const override; + bool hasWork() const override; private: //! Input Queues @@ -78,7 +77,7 @@ class VisualizerModule ThreadsafeQueue::UniquePtr mesher_queue_; //! Visualizer implementation - OpenCvVisualizer3D::UniquePtr visualizer_; + Visualizer3D::UniquePtr visualizer_; }; } // namespace VIO diff --git a/src/visualizer/Visualizer3D.cpp b/src/visualizer/Visualizer3D.cpp index 1b714e83a..a216cb223 100644 --- a/src/visualizer/Visualizer3D.cpp +++ b/src/visualizer/Visualizer3D.cpp @@ -8,8 +8,8 @@ /** * @file Visualizer.cpp - * @brief Build and visualize 2D mesh from Frame - * @author Antoni Rosinol, AJ Haeffner, Luca Carlone + * @brief Build and visualize 3D data: 2D mesh from Frame for example. + * @author Antoni Rosinol */ #include "kimera-vio/visualizer/Visualizer3D.h" @@ -80,12 +80,13 @@ DEFINE_int32(displayed_trajectory_length, namespace VIO { +Visualizer3D::Visualizer3D(const VisualizationType& viz_type) + : visualization_type_(viz_type) {} + /* -------------------------------------------------------------------------- */ OpenCvVisualizer3D::OpenCvVisualizer3D(const VisualizationType& viz_type, - const BackendType& backend_type) - : Visualizer3D(viz_type), - backend_type_(backend_type), - logger_(nullptr) { + const BackendType& backend_type) + : Visualizer3D(viz_type), backend_type_(backend_type), logger_(nullptr) { if (FLAGS_log_mesh) { logger_ = VIO::make_unique(); } @@ -520,13 +521,13 @@ void OpenCvVisualizer3D::visualizePoints3D( /* -------------------------------------------------------------------------- */ // Visualize a 3D point cloud of unique 3D landmarks with its connectivity. void OpenCvVisualizer3D::visualizePlane(const PlaneId& plane_index, - const double& n_x, - const double& n_y, - const double& n_z, - const double& d, - WidgetsMap* widgets, - const bool& visualize_plane_label, - const int& cluster_id) { + const double& n_x, + const double& n_y, + const double& n_z, + const double& d, + WidgetsMap* widgets, + const bool& visualize_plane_label, + const int& cluster_id) { CHECK_NOTNULL(widgets); const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); // Create a plane widget. @@ -556,13 +557,13 @@ void OpenCvVisualizer3D::visualizePlane(const PlaneId& plane_index, /* -------------------------------------------------------------------------- */ // Draw a line in opencv. void OpenCvVisualizer3D::drawLine(const std::string& line_id, - const double& from_x, - const double& from_y, - const double& from_z, - const double& to_x, - const double& to_y, - const double& to_z, - WidgetsMap* widgets) { + const double& from_x, + const double& from_y, + const double& from_z, + const double& to_x, + const double& to_y, + const double& to_z, + WidgetsMap* widgets) { cv::Point3d pt1(from_x, from_y, from_z); cv::Point3d pt2(to_x, to_y, to_z); drawLine(line_id, pt1, pt2, widgets); @@ -570,9 +571,9 @@ void OpenCvVisualizer3D::drawLine(const std::string& line_id, /* -------------------------------------------------------------------------- */ void OpenCvVisualizer3D::drawLine(const std::string& line_id, - const cv::Point3d& pt1, - const cv::Point3d& pt2, - WidgetsMap* widgets) { + const cv::Point3d& pt1, + const cv::Point3d& pt2, + WidgetsMap* widgets) { CHECK_NOTNULL(widgets); (*widgets)[line_id] = VIO::make_unique(pt1, pt2); } @@ -580,8 +581,8 @@ void OpenCvVisualizer3D::drawLine(const std::string& line_id, /* -------------------------------------------------------------------------- */ // Visualize a 3D point cloud of unique 3D landmarks with its connectivity. void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets) { + const cv::Mat& polygons_mesh, + WidgetsMap* widgets) { cv::Mat colors(0, 1, CV_8UC3, cv::viz::Color::gray()); // Do not color mesh. visualizeMesh3D(map_points_3d, colors, polygons_mesh, widgets); } @@ -590,11 +591,11 @@ void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, // Visualize a 3D point cloud of unique 3D landmarks with its connectivity, // and provide color for each polygon. void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, - const cv::Mat& colors, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets, - const cv::Mat& tcoords, - const cv::Mat& texture) { + const cv::Mat& colors, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets, + const cv::Mat& tcoords, + const cv::Mat& texture) { CHECK_NOTNULL(widgets); // Check data bool color_mesh = false; @@ -632,7 +633,7 @@ void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, /* -------------------------------------------------------------------------- */ // Visualize a PLY from filename (absolute path). void OpenCvVisualizer3D::visualizePlyMesh(const std::string& filename, - WidgetsMap* widgets) { + WidgetsMap* widgets) { CHECK_NOTNULL(widgets); LOG(INFO) << "Showing ground truth mesh: " << filename; // The ply file must have in the header a "element vertex" and @@ -702,9 +703,9 @@ void OpenCvVisualizer3D::visualizeMesh3DWithColoredClusters( // Visualize convex hull in 2D for set of points in triangle cluster, // projected along the normal of the cluster. void OpenCvVisualizer3D::visualizeConvexHull(const TriangleCluster& cluster, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets_map) { + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets_map) { CHECK_NOTNULL(widgets_map); // Create a new coord system, which has as z the normal. @@ -818,8 +819,8 @@ void OpenCvVisualizer3D::visualizeConvexHull(const TriangleCluster& cluster, /* -------------------------------------------------------------------------- */ // Visualize trajectory. Adds an image to the frustum if cv::Mat is not empty. void OpenCvVisualizer3D::visualizeTrajectory3D(const cv::Mat& frustum_image, - cv::Affine3d* frustum_pose, - WidgetsMap* widgets_map) { + cv::Affine3d* frustum_pose, + WidgetsMap* widgets_map) { CHECK_NOTNULL(frustum_pose); CHECK_NOTNULL(widgets_map); @@ -906,11 +907,11 @@ bool OpenCvVisualizer3D::removeWidget(const std::string& widget_id) { // Visualize line widgets from plane to lmks. // Point key is required to avoid duplicated lines! void OpenCvVisualizer3D::visualizePlaneConstraints(const PlaneId& plane_id, - const gtsam::Point3& normal, - const double& distance, - const LandmarkId& lmk_id, - const gtsam::Point3& point, - WidgetsMap* widgets) { + const gtsam::Point3& normal, + const double& distance, + const LandmarkId& lmk_id, + const gtsam::Point3& point, + WidgetsMap* widgets) { PlaneIdMap::iterator plane_id_it = plane_id_map_.find(plane_id); LmkIdToLineIdMap* lmk_id_to_line_id_map_ptr = nullptr; LineNr* line_nr_ptr = nullptr; @@ -1035,7 +1036,7 @@ void OpenCvVisualizer3D::removePlaneConstraintsViz(const PlaneId& plane_id) { /* -------------------------------------------------------------------------- */ // Remove plane widget. void OpenCvVisualizer3D::removePlane(const PlaneId& plane_index, - const bool& remove_plane_label) { + const bool& remove_plane_label) { const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); if (is_plane_id_in_window_.find(plane_index) != is_plane_id_in_window_.end() && @@ -1057,7 +1058,8 @@ void OpenCvVisualizer3D::removePlane(const PlaneId& plane_index, /* -------------------------------------------------------------------------- */ // Add pose to the previous trajectory. -void OpenCvVisualizer3D::addPoseToTrajectory(const gtsam::Pose3& current_pose_gtsam) { +void OpenCvVisualizer3D::addPoseToTrajectory( + const gtsam::Pose3& current_pose_gtsam) { trajectory_poses_3d_.push_back( UtilsOpenCV::gtsamPose3ToCvAffine3d(current_pose_gtsam)); if (FLAGS_displayed_trajectory_length > 0) { @@ -1169,10 +1171,10 @@ Mesh3DVizProperties OpenCvVisualizer3D::texturizeMesh3D( /* -------------------------------------------------------------------------- */ // Log mesh to ply file. void OpenCvVisualizer3D::logMesh(const cv::Mat& map_points_3d, - const cv::Mat& colors, - const cv::Mat& polygons_mesh, - const Timestamp& timestamp, - bool log_accumulated_mesh) { + const cv::Mat& colors, + const cv::Mat& polygons_mesh, + const Timestamp& timestamp, + bool log_accumulated_mesh) { /// Log the mesh in a ply file. static Timestamp last_timestamp = timestamp; static const Timestamp first_timestamp = timestamp; @@ -1192,9 +1194,9 @@ void OpenCvVisualizer3D::logMesh(const cv::Mat& map_points_3d, // output colors matrix for mesh visualizer. // This will color the point with the color of the last plane having it. void OpenCvVisualizer3D::colorMeshByClusters(const std::vector& planes, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - cv::Mat* colors) const { + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + cv::Mat* colors) const { CHECK_NOTNULL(colors); *colors = cv::Mat(map_points_3d.rows, 1, CV_8UC3, cv::viz::Color::gray()); @@ -1223,7 +1225,8 @@ void OpenCvVisualizer3D::colorMeshByClusters(const std::vector& planes, /* -------------------------------------------------------------------------- */ // Decide color of the cluster depending on its id. -void OpenCvVisualizer3D::getColorById(const size_t& id, cv::viz::Color* color) const { +void OpenCvVisualizer3D::getColorById(const size_t& id, + cv::viz::Color* color) const { CHECK_NOTNULL(color); switch (id) { case 0: { @@ -1248,14 +1251,14 @@ void OpenCvVisualizer3D::getColorById(const size_t& id, cv::viz::Color* color) c /* -------------------------------------------------------------------------- */ // Draw a line from lmk to plane center. void OpenCvVisualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, - const double& plane_n_x, - const double& plane_n_y, - const double& plane_n_z, - const double& plane_d, - const double& point_x, - const double& point_y, - const double& point_z, - WidgetsMap* widgets) { + const double& plane_n_x, + const double& plane_n_y, + const double& plane_n_z, + const double& plane_d, + const double& point_x, + const double& point_y, + const double& point_z, + WidgetsMap* widgets) { const cv::Point3d center( plane_d * plane_n_x, plane_d * plane_n_y, plane_d * plane_n_z); const cv::Point3d point(point_x, point_y, point_z); @@ -1265,14 +1268,14 @@ void OpenCvVisualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, /* -------------------------------------------------------------------------- */ // Update line from lmk to plane center. void OpenCvVisualizer3D::updateLineFromPlaneToPoint(const std::string& line_id, - const double& plane_n_x, - const double& plane_n_y, - const double& plane_n_z, - const double& plane_d, - const double& point_x, - const double& point_y, - const double& point_z, - WidgetsMap* widgets) { + const double& plane_n_x, + const double& plane_n_y, + const double& plane_n_z, + const double& plane_d, + const double& point_x, + const double& point_y, + const double& point_z, + WidgetsMap* widgets) { removeWidget(line_id); drawLineFromPlaneToPoint(line_id, plane_n_x, diff --git a/src/visualizer/Visualizer3DModule.cpp b/src/visualizer/Visualizer3DModule.cpp index 262ece5cb..370987960 100644 --- a/src/visualizer/Visualizer3DModule.cpp +++ b/src/visualizer/Visualizer3DModule.cpp @@ -17,13 +17,11 @@ #include #include -#include - namespace VIO { VisualizerModule::VisualizerModule(OutputQueue* output_queue, bool parallel_run, - OpenCvVisualizer3D::UniquePtr visualizer) + Visualizer3D::UniquePtr visualizer) : MISOPipelineModule(output_queue, "Visualizer", parallel_run), From 48f318b85239f439ca73135fb11f88c81238c31f Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 01:37:45 -0400 Subject: [PATCH 10/15] Make debug tracker info getter const.. --- .../kimera-vio/frontend/StereoVisionFrontEnd-definitions.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/kimera-vio/frontend/StereoVisionFrontEnd-definitions.h b/include/kimera-vio/frontend/StereoVisionFrontEnd-definitions.h index c0b2bdd39..dc15c6c59 100644 --- a/include/kimera-vio/frontend/StereoVisionFrontEnd-definitions.h +++ b/include/kimera-vio/frontend/StereoVisionFrontEnd-definitions.h @@ -8,7 +8,7 @@ /** * @file StereoVisionFrontEnd-definitions.h - * @brief Definitions for VioBackEnd + * @brief Definitions for StereoVisionFrontEnd * @author Antoni Rosinol */ @@ -64,7 +64,7 @@ struct FrontendOutput : public PipelinePayload { const DebugTrackerInfo debug_tracker_info_; const cv::Mat feature_tracks_; - inline DebugTrackerInfo getTrackerInfo() { return debug_tracker_info_; } + inline DebugTrackerInfo getTrackerInfo() const { return debug_tracker_info_; } }; } // namespace VIO From 5ed25eb8230e38f11a2e433f15adf1ada269022d Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 01:41:03 -0400 Subject: [PATCH 11/15] Revert "QtCreator was sending me to devel space" This reverts commit 2ea2d50fa07756c2e4e0a00c643ca9a92399fc5e. --- CMakeLists.txt | 10 +++--- include/kimera-vio/backend/CMakeLists.txt | 18 +++++----- include/kimera-vio/common/CMakeLists.txt | 4 +-- .../kimera-vio/dataprovider/CMakeLists.txt | 10 +++--- include/kimera-vio/factors/CMakeLists.txt | 4 +-- include/kimera-vio/frontend/CMakeLists.txt | 34 +++++++++---------- .../frontend/feature-detector/CMakeLists.txt | 8 ++--- .../feature-detector/anms/CMakeLists.txt | 8 ++--- .../kimera-vio/imu-frontend/CMakeLists.txt | 6 ++-- include/kimera-vio/initial/CMakeLists.txt | 10 +++--- include/kimera-vio/logging/CMakeLists.txt | 2 +- include/kimera-vio/loopclosure/CMakeLists.txt | 8 ++--- include/kimera-vio/mesh/CMakeLists.txt | 12 +++---- include/kimera-vio/pipeline/CMakeLists.txt | 12 +++---- include/kimera-vio/utils/CMakeLists.txt | 34 +++++++++---------- include/kimera-vio/visualizer/CMakeLists.txt | 16 ++++----- src/backend/CMakeLists.txt | 10 +++--- src/common/CMakeLists.txt | 2 +- src/dataprovider/CMakeLists.txt | 10 +++--- src/factors/CMakeLists.txt | 4 +-- src/frontend/CMakeLists.txt | 18 +++++----- src/frontend/feature-detector/CMakeLists.txt | 6 ++-- .../feature-detector/anms/CMakeLists.txt | 2 +- src/imu-frontend/CMakeLists.txt | 4 +-- src/initial/CMakeLists.txt | 6 ++-- src/logging/CMakeLists.txt | 2 +- src/loopclosure/CMakeLists.txt | 6 ++-- src/mesh/CMakeLists.txt | 8 ++--- src/pipeline/CMakeLists.txt | 12 +++---- src/utils/CMakeLists.txt | 12 +++---- src/visualizer/CMakeLists.txt | 14 ++++---- 31 files changed, 156 insertions(+), 156 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 303d70cea..ee21df4b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,7 +134,7 @@ target_include_directories(${PROJECT_NAME} ${GFLAGS_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GTSAM_INCLUDE_DIR} - $ + $ $ ) @@ -260,7 +260,7 @@ write_basic_package_version_file( # This file is necessary to find_package the library kimera_vio. set(INSTALL_CONFIGDIR lib/cmake/kimera_vio) configure_package_config_file( - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/kimera_vioConfig.cmake.in + ${CMAKE_CURRENT_LIST_DIR}/cmake/kimera_vioConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/kimera_vioConfig.cmake INSTALL_DESTINATION ${INSTALL_CONFIGDIR} ) @@ -289,7 +289,7 @@ else(EXPORT_KIMERA) ${INSTALL_CONFIGDIR} ) # Install header files - install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/ + install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/include/ DESTINATION include FILES_MATCHING PATTERN "*.h") @@ -297,8 +297,8 @@ else(EXPORT_KIMERA) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/kimera_vioConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/kimera_vioConfigVersion.cmake - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/FindGflags.cmake - ${CMAKE_CURRENT_SOURCE_DIR}/cmake/FindGlog.cmake + ${CMAKE_CURRENT_LIST_DIR}/cmake/FindGflags.cmake + ${CMAKE_CURRENT_LIST_DIR}/cmake/FindGlog.cmake DESTINATION ${INSTALL_CONFIGDIR} ) endif(EXPORT_KIMERA) diff --git a/include/kimera-vio/backend/CMakeLists.txt b/include/kimera-vio/backend/CMakeLists.txt index cff83e4dd..0dd33f2a7 100644 --- a/include/kimera-vio/backend/CMakeLists.txt +++ b/include/kimera-vio/backend/CMakeLists.txt @@ -1,12 +1,12 @@ ### Add source code just for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/FactorGraphManagement.h" - "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEnd-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEnd.h" - "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEndParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEnd-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEnd.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/FactorGraphManagement.h" + "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd.h" + "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEndParams.h" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd.h" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEndParams.h" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEndModule.h" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEndFactory.h" ) diff --git a/include/kimera-vio/common/CMakeLists.txt b/include/kimera-vio/common/CMakeLists.txt index 940990f7f..37cdfbe0d 100644 --- a/include/kimera-vio/common/CMakeLists.txt +++ b/include/kimera-vio/common/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code just for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/vio_types.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VioNavState.h" + "${CMAKE_CURRENT_LIST_DIR}/vio_types.h" + "${CMAKE_CURRENT_LIST_DIR}/VioNavState.h" ) diff --git a/include/kimera-vio/dataprovider/CMakeLists.txt b/include/kimera-vio/dataprovider/CMakeLists.txt index bc2c800bb..c39e7087c 100644 --- a/include/kimera-vio/dataprovider/CMakeLists.txt +++ b/include/kimera-vio/dataprovider/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface.h" - "${CMAKE_CURRENT_SOURCE_DIR}/EurocDataProvider.h" - "${CMAKE_CURRENT_SOURCE_DIR}/KittiDataProvider.h" + "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/DataProviderModule.h" + "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface.h" + "${CMAKE_CURRENT_LIST_DIR}/EurocDataProvider.h" + "${CMAKE_CURRENT_LIST_DIR}/KittiDataProvider.h" ) diff --git a/include/kimera-vio/factors/CMakeLists.txt b/include/kimera-vio/factors/CMakeLists.txt index 5f07f7c86..e758bc3d9 100644 --- a/include/kimera-vio/factors/CMakeLists.txt +++ b/include/kimera-vio/factors/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/ParallelPlaneRegularFactor.h" - "${CMAKE_CURRENT_SOURCE_DIR}/PointPlaneFactor.h" + "${CMAKE_CURRENT_LIST_DIR}/ParallelPlaneRegularFactor.h" + "${CMAKE_CURRENT_LIST_DIR}/PointPlaneFactor.h" ) diff --git a/include/kimera-vio/frontend/CMakeLists.txt b/include/kimera-vio/frontend/CMakeLists.txt index 65da979c5..9ef19825c 100644 --- a/include/kimera-vio/frontend/CMakeLists.txt +++ b/include/kimera-vio/frontend/CMakeLists.txt @@ -1,22 +1,22 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Camera.h" - "${CMAKE_CURRENT_SOURCE_DIR}/CameraParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoMatchingParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Frame.h" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoFrame-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoFrame.h" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoImuSyncPacket.h" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoVisionFrontEnd-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoVisionFrontEnd.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndFactory.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Tracker-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Tracker.h" - "${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlowPredictor-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlowPredictor.h" - "${CMAKE_CURRENT_SOURCE_DIR}/OpticalFlowPredictorFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Camera.h" + "${CMAKE_CURRENT_LIST_DIR}/CameraParams.h" + "${CMAKE_CURRENT_LIST_DIR}/StereoMatchingParams.h" + "${CMAKE_CURRENT_LIST_DIR}/Frame.h" + "${CMAKE_CURRENT_LIST_DIR}/StereoFrame-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/StereoFrame.h" + "${CMAKE_CURRENT_LIST_DIR}/StereoImuSyncPacket.h" + "${CMAKE_CURRENT_LIST_DIR}/StereoVisionFrontEnd-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/StereoVisionFrontEnd.h" + "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndParams.h" + "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndModule.h" + "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Tracker-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/Tracker.h" + "${CMAKE_CURRENT_LIST_DIR}/OpticalFlowPredictor-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/OpticalFlowPredictor.h" + "${CMAKE_CURRENT_LIST_DIR}/OpticalFlowPredictorFactory.h" ) add_subdirectory(feature-detector) diff --git a/include/kimera-vio/frontend/feature-detector/CMakeLists.txt b/include/kimera-vio/frontend/feature-detector/CMakeLists.txt index ff73f2ef7..207166d2c 100644 --- a/include/kimera-vio/frontend/feature-detector/CMakeLists.txt +++ b/include/kimera-vio/frontend/feature-detector/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for IDEs target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetector.h" - "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetectorParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetector-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/NonMaximumSuppression.h" + "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector.h" + "${CMAKE_CURRENT_LIST_DIR}/FeatureDetectorParams.h" + "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/NonMaximumSuppression.h" ) add_subdirectory(anms) diff --git a/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt b/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt index 14246f790..cc08258db 100644 --- a/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt +++ b/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt @@ -2,8 +2,8 @@ target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/nanoflann.hpp" - "${CMAKE_CURRENT_SOURCE_DIR}/range-tree/lrtypes.h" - "${CMAKE_CURRENT_SOURCE_DIR}/range-tree/ranget.h" - "${CMAKE_CURRENT_SOURCE_DIR}/anms.h" + "${CMAKE_CURRENT_LIST_DIR}/nanoflann.hpp" + "${CMAKE_CURRENT_LIST_DIR}/range-tree/lrtypes.h" + "${CMAKE_CURRENT_LIST_DIR}/range-tree/ranget.h" + "${CMAKE_CURRENT_LIST_DIR}/anms.h" ) diff --git a/include/kimera-vio/imu-frontend/CMakeLists.txt b/include/kimera-vio/imu-frontend/CMakeLists.txt index 395dc2137..42ce70bf3 100644 --- a/include/kimera-vio/imu-frontend/CMakeLists.txt +++ b/include/kimera-vio/imu-frontend/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code for kimera_vio target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEnd-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEnd.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEndParams.h" + "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.h" + "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.h" ) diff --git a/include/kimera-vio/initial/CMakeLists.txt b/include/kimera-vio/initial/CMakeLists.txt index 5bca8ee95..edefa6a3a 100644 --- a/include/kimera-vio/initial/CMakeLists.txt +++ b/include/kimera-vio/initial/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/InitializationBackEnd-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/InitializationBackEnd.h" - "${CMAKE_CURRENT_SOURCE_DIR}/InitializationFromImu.h" - "${CMAKE_CURRENT_SOURCE_DIR}/OnlineGravityAlignment-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/OnlineGravityAlignment.h" + "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd.h" + "${CMAKE_CURRENT_LIST_DIR}/InitializationFromImu.h" + "${CMAKE_CURRENT_LIST_DIR}/OnlineGravityAlignment-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/OnlineGravityAlignment.h" ) diff --git a/include/kimera-vio/logging/CMakeLists.txt b/include/kimera-vio/logging/CMakeLists.txt index 81bc21843..5d5bd3392 100644 --- a/include/kimera-vio/logging/CMakeLists.txt +++ b/include/kimera-vio/logging/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Logger.h" + "${CMAKE_CURRENT_LIST_DIR}/Logger.h" ) diff --git a/include/kimera-vio/loopclosure/CMakeLists.txt b/include/kimera-vio/loopclosure/CMakeLists.txt index df19de525..e76bf18fe 100644 --- a/include/kimera-vio/loopclosure/CMakeLists.txt +++ b/include/kimera-vio/loopclosure/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for LoopClosureDetector target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetector-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetector.h" - "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetectorParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/LcdThirdPartyWrapper.h" + "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector.h" + "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetectorParams.h" + "${CMAKE_CURRENT_LIST_DIR}/LcdThirdPartyWrapper.h" ) diff --git a/include/kimera-vio/mesh/CMakeLists.txt b/include/kimera-vio/mesh/CMakeLists.txt index d882bbad3..efb824c1f 100644 --- a/include/kimera-vio/mesh/CMakeLists.txt +++ b/include/kimera-vio/mesh/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Mesh.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Mesher.h" - "${CMAKE_CURRENT_SOURCE_DIR}/MesherModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/MesherFactory.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Mesher-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Mesher_cgal.h" + "${CMAKE_CURRENT_LIST_DIR}/Mesh.h" + "${CMAKE_CURRENT_LIST_DIR}/Mesher.h" + "${CMAKE_CURRENT_LIST_DIR}/MesherModule.h" + "${CMAKE_CURRENT_LIST_DIR}/MesherFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Mesher-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/Mesher_cgal.h" ) diff --git a/include/kimera-vio/pipeline/CMakeLists.txt b/include/kimera-vio/pipeline/CMakeLists.txt index 3d35ddf3b..fbf57dcdc 100644 --- a/include/kimera-vio/pipeline/CMakeLists.txt +++ b/include/kimera-vio/pipeline/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/PipelinePayload.h" - "${CMAKE_CURRENT_SOURCE_DIR}/PipelineModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/PipelineParams.h" - "${CMAKE_CURRENT_SOURCE_DIR}/QueueSynchronizer.h" + "${CMAKE_CURRENT_LIST_DIR}/Pipeline.h" + "${CMAKE_CURRENT_LIST_DIR}/Pipeline-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/PipelinePayload.h" + "${CMAKE_CURRENT_LIST_DIR}/PipelineModule.h" + "${CMAKE_CURRENT_LIST_DIR}/PipelineParams.h" + "${CMAKE_CURRENT_LIST_DIR}/QueueSynchronizer.h" ) diff --git a/include/kimera-vio/utils/CMakeLists.txt b/include/kimera-vio/utils/CMakeLists.txt index 8a88eb2ab..9187fbbb4 100644 --- a/include/kimera-vio/utils/CMakeLists.txt +++ b/include/kimera-vio/utils/CMakeLists.txt @@ -1,21 +1,21 @@ ### Add includes for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Accumulator.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Histogram.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Macros.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Statistics.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeImuBuffer.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeImuBuffer-inl.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeQueue.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeTemporalBuffer.h" - "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeTemporalBuffer-inl.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Timer.h" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsGeometry.h" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsGTSAM.h" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsOpenCV.h" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsNumerical.h" - "${CMAKE_CURRENT_SOURCE_DIR}/SerializationOpenCv.h" - "${CMAKE_CURRENT_SOURCE_DIR}/YamlParser.h" - "${CMAKE_CURRENT_SOURCE_DIR}/FilesystemUtils.h" + "${CMAKE_CURRENT_LIST_DIR}/Accumulator.h" + "${CMAKE_CURRENT_LIST_DIR}/Histogram.h" + "${CMAKE_CURRENT_LIST_DIR}/Macros.h" + "${CMAKE_CURRENT_LIST_DIR}/Statistics.h" + "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer.h" + "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer-inl.h" + "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeQueue.h" + "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeTemporalBuffer.h" + "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeTemporalBuffer-inl.h" + "${CMAKE_CURRENT_LIST_DIR}/Timer.h" + "${CMAKE_CURRENT_LIST_DIR}/UtilsGeometry.h" + "${CMAKE_CURRENT_LIST_DIR}/UtilsGTSAM.h" + "${CMAKE_CURRENT_LIST_DIR}/UtilsOpenCV.h" + "${CMAKE_CURRENT_LIST_DIR}/UtilsNumerical.h" + "${CMAKE_CURRENT_LIST_DIR}/SerializationOpenCv.h" + "${CMAKE_CURRENT_LIST_DIR}/YamlParser.h" + "${CMAKE_CURRENT_LIST_DIR}/FilesystemUtils.h" ) diff --git a/include/kimera-vio/visualizer/CMakeLists.txt b/include/kimera-vio/visualizer/CMakeLists.txt index cb19cb322..3bfc60415 100644 --- a/include/kimera-vio/visualizer/CMakeLists.txt +++ b/include/kimera-vio/visualizer/CMakeLists.txt @@ -1,11 +1,11 @@ ### Add includes target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3D.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3D-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DFactory.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Display-definitions.h" - "${CMAKE_CURRENT_SOURCE_DIR}/Display.h" - "${CMAKE_CURRENT_SOURCE_DIR}/DisplayModule.h" - "${CMAKE_CURRENT_SOURCE_DIR}/DisplayFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.h" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.h" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.h" + "${CMAKE_CURRENT_LIST_DIR}/Display.h" + "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.h" + "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.h" ) diff --git a/src/backend/CMakeLists.txt b/src/backend/CMakeLists.txt index 0d924e1c6..2cd79690f 100644 --- a/src/backend/CMakeLists.txt +++ b/src/backend/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEnd.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/VioBackEndParams.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEnd.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/RegularVioBackEndParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEndModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VioBackEndParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd.cpp" + "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEndParams.cpp" ) diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index 2ec1d47c3..b851589c5 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/VioNavState.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VioNavState.cpp" ) diff --git a/src/dataprovider/CMakeLists.txt b/src/dataprovider/CMakeLists.txt index 7bcaeb9f4..4a03df329 100644 --- a/src/dataprovider/CMakeLists.txt +++ b/src/dataprovider/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface-definitions.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderInterface.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/DataProviderModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/EurocDataProvider.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/KittiDataProvider.cpp" + "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface-definitions.cpp" + "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface.cpp" + "${CMAKE_CURRENT_LIST_DIR}/DataProviderModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/EurocDataProvider.cpp" + "${CMAKE_CURRENT_LIST_DIR}/KittiDataProvider.cpp" ) diff --git a/src/factors/CMakeLists.txt b/src/factors/CMakeLists.txt index 24c8edebe..f106a8b45 100644 --- a/src/factors/CMakeLists.txt +++ b/src/factors/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/ParallelPlaneRegularFactor.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/PointPlaneFactor.cpp" + "${CMAKE_CURRENT_LIST_DIR}/ParallelPlaneRegularFactor.cpp" + "${CMAKE_CURRENT_LIST_DIR}/PointPlaneFactor.cpp" ) diff --git a/src/frontend/CMakeLists.txt b/src/frontend/CMakeLists.txt index f9b8bb373..70dd5c4ce 100644 --- a/src/frontend/CMakeLists.txt +++ b/src/frontend/CMakeLists.txt @@ -1,15 +1,15 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/CameraParams.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoFrame.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoMatchingParams.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoImuSyncPacket.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/StereoVisionFrontEnd.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndFactory.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/VisionFrontEndParams.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Tracker.cpp" + "${CMAKE_CURRENT_LIST_DIR}/CameraParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/StereoFrame.cpp" + "${CMAKE_CURRENT_LIST_DIR}/StereoMatchingParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/StereoImuSyncPacket.cpp" + "${CMAKE_CURRENT_LIST_DIR}/StereoVisionFrontEnd.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndFactory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/VisionFrontEndParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Tracker.cpp" ) add_subdirectory(feature-detector) diff --git a/src/frontend/feature-detector/CMakeLists.txt b/src/frontend/feature-detector/CMakeLists.txt index 6516e643d..441f32479 100644 --- a/src/frontend/feature-detector/CMakeLists.txt +++ b/src/frontend/feature-detector/CMakeLists.txt @@ -1,9 +1,9 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetectorParams.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/FeatureDetector.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/NonMaximumSuppression.cpp" + "${CMAKE_CURRENT_LIST_DIR}/FeatureDetectorParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector.cpp" + "${CMAKE_CURRENT_LIST_DIR}/NonMaximumSuppression.cpp" ) add_subdirectory(anms) diff --git a/src/frontend/feature-detector/anms/CMakeLists.txt b/src/frontend/feature-detector/anms/CMakeLists.txt index ce03dcc53..5792cd35e 100644 --- a/src/frontend/feature-detector/anms/CMakeLists.txt +++ b/src/frontend/feature-detector/anms/CMakeLists.txt @@ -2,5 +2,5 @@ target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/anms.cpp" + "${CMAKE_CURRENT_LIST_DIR}/anms.cpp" ) diff --git a/src/imu-frontend/CMakeLists.txt b/src/imu-frontend/CMakeLists.txt index bea5b0aef..338206f40 100644 --- a/src/imu-frontend/CMakeLists.txt +++ b/src/imu-frontend/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for kimera_vio target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEnd.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/ImuFrontEndParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.cpp" + "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.cpp" ) diff --git a/src/initial/CMakeLists.txt b/src/initial/CMakeLists.txt index 7660762c9..8abbe3909 100644 --- a/src/initial/CMakeLists.txt +++ b/src/initial/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/InitializationFromImu.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/InitializationBackEnd.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/OnlineGravityAlignment.cpp" + "${CMAKE_CURRENT_LIST_DIR}/InitializationFromImu.cpp" + "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd.cpp" + "${CMAKE_CURRENT_LIST_DIR}/OnlineGravityAlignment.cpp" ) diff --git a/src/logging/CMakeLists.txt b/src/logging/CMakeLists.txt index ce8398ff2..9345d5827 100644 --- a/src/logging/CMakeLists.txt +++ b/src/logging/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Logger.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Logger.cpp" ) diff --git a/src/loopclosure/CMakeLists.txt b/src/loopclosure/CMakeLists.txt index 43de35eb7..d9f035b75 100644 --- a/src/loopclosure/CMakeLists.txt +++ b/src/loopclosure/CMakeLists.txt @@ -1,7 +1,7 @@ ### Add source code for LoopClosureDetector target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetector.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/LcdThirdPartyWrapper.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/LoopClosureDetectorParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector.cpp" + "${CMAKE_CURRENT_LIST_DIR}/LcdThirdPartyWrapper.cpp" + "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetectorParams.cpp" ) diff --git a/src/mesh/CMakeLists.txt b/src/mesh/CMakeLists.txt index 7f842ad59..ba3ef5029 100644 --- a/src/mesh/CMakeLists.txt +++ b/src/mesh/CMakeLists.txt @@ -1,8 +1,8 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Mesh.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Mesher.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/MesherModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/MesherFactory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Mesh.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Mesher.cpp" + "${CMAKE_CURRENT_LIST_DIR}/MesherModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/MesherFactory.cpp" ) diff --git a/src/pipeline/CMakeLists.txt b/src/pipeline/CMakeLists.txt index ad26a2ada..ed4ab6f51 100644 --- a/src/pipeline/CMakeLists.txt +++ b/src/pipeline/CMakeLists.txt @@ -1,10 +1,10 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/PipelineModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/PipelinePayload.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/PipelineParams.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Pipeline-definitions.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/QueueSynchronizer.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Pipeline.cpp" + "${CMAKE_CURRENT_LIST_DIR}/PipelineModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/PipelinePayload.cpp" + "${CMAKE_CURRENT_LIST_DIR}/PipelineParams.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Pipeline-definitions.cpp" + "${CMAKE_CURRENT_LIST_DIR}/QueueSynchronizer.cpp" ) diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index 965b2b1f4..b6e0707e5 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -1,10 +1,10 @@ ### Add source code for stereoVIO target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/ThreadsafeImuBuffer.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Statistics.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Histogram.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsGeometry.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsOpenCV.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/UtilsNumerical.cpp" + "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Statistics.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Histogram.cpp" + "${CMAKE_CURRENT_LIST_DIR}/UtilsGeometry.cpp" + "${CMAKE_CURRENT_LIST_DIR}/UtilsOpenCV.cpp" + "${CMAKE_CURRENT_LIST_DIR}/UtilsNumerical.cpp" ) diff --git a/src/visualizer/CMakeLists.txt b/src/visualizer/CMakeLists.txt index e5b3a90e3..9f7166d88 100644 --- a/src/visualizer/CMakeLists.txt +++ b/src/visualizer/CMakeLists.txt @@ -1,11 +1,11 @@ ### Add source code target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3D.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Visualizer3DFactory.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Display-definitions.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/Display.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/DisplayModule.cpp" - "${CMAKE_CURRENT_SOURCE_DIR}/DisplayFactory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.cpp" + "${CMAKE_CURRENT_LIST_DIR}/Display.cpp" + "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.cpp" + "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.cpp" ) From 374e0cd435d653e1ae7a68d906a5d0f875daa9fd Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 21:28:36 -0400 Subject: [PATCH 12/15] Use const& in pim, and minor refactor --- src/imu-frontend/ImuFrontEnd.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/imu-frontend/ImuFrontEnd.cpp b/src/imu-frontend/ImuFrontEnd.cpp index 46be3de48..e4180006c 100644 --- a/src/imu-frontend/ImuFrontEnd.cpp +++ b/src/imu-frontend/ImuFrontEnd.cpp @@ -95,8 +95,7 @@ ImuFrontEnd::PimPtr ImuFrontEnd::preintegrateImuMeasurements( // measurement. Nevertheless the imu_stamps, should be shifted one step back // I would say. for (int i = 0; i < imu_stamps.cols() - 1; ++i) { - gtsam::Vector3 measured_acc = imu_accgyr.block<3, 1>(0, i); - + const gtsam::Vector3& measured_acc = imu_accgyr.block<3, 1>(0, i); const gtsam::Vector3& measured_omega = imu_accgyr.block<3, 1>(3, i); const double& delta_t = UtilsNumerical::NsecToSec(imu_stamps(i + 1) - imu_stamps(i)); @@ -135,20 +134,20 @@ gtsam::Rot3 ImuFrontEnd::preintegrateGyroMeasurements( CHECK(imu_stamps.cols() >= 2) << "No Imu data found."; CHECK(imu_accgyr.cols() >= 2) << "No Imu data found."; std::lock_guard lock(imu_bias_mutex_); - gtsam::PreintegratedAhrsMeasurements pimRot(latest_imu_bias_.gyroscope(), + gtsam::PreintegratedAhrsMeasurements pim_rot(latest_imu_bias_.gyroscope(), gtsam::Matrix3::Identity()); for (int i = 0; i < imu_stamps.cols() - 1; ++i) { const gtsam::Vector3& measured_omega = imu_accgyr.block<3, 1>(3, i); const double& delta_t = UtilsNumerical::NsecToSec(imu_stamps(i + 1) - imu_stamps(i)); CHECK_GT(delta_t, 0.0) << "Imu delta is 0!"; - pimRot.integrateMeasurement(measured_omega, delta_t); + pim_rot.integrateMeasurement(measured_omega, delta_t); } if (VLOG_IS_ON(10)) { LOG(INFO) << "Finished preintegration for gyro aided: "; - pimRot.print(); + pim_rot.print(); } - return pimRot.deltaRij(); + return pim_rot.deltaRij(); } /* -------------------------------------------------------------------------- */ From f0466d99d6cc0e01d28b60291c94d30b1736fd38 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 21:41:21 -0400 Subject: [PATCH 13/15] Separate OpenCV display from DisplayBase --- include/kimera-vio/visualizer/CMakeLists.txt | 1 + include/kimera-vio/visualizer/Display.h | 101 ------ .../kimera-vio/visualizer/DisplayFactory.h | 1 + include/kimera-vio/visualizer/OpenCvDisplay.h | 117 ++++++ src/visualizer/CMakeLists.txt | 1 + src/visualizer/Display.cpp | 304 ---------------- src/visualizer/OpenCvDisplay.cpp | 341 ++++++++++++++++++ 7 files changed, 461 insertions(+), 405 deletions(-) create mode 100644 include/kimera-vio/visualizer/OpenCvDisplay.h create mode 100644 src/visualizer/OpenCvDisplay.cpp diff --git a/include/kimera-vio/visualizer/CMakeLists.txt b/include/kimera-vio/visualizer/CMakeLists.txt index 3bfc60415..b5e0276d2 100644 --- a/include/kimera-vio/visualizer/CMakeLists.txt +++ b/include/kimera-vio/visualizer/CMakeLists.txt @@ -6,6 +6,7 @@ target_sources(kimera_vio PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.h" "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/Display.h" + "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.h" "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.h" "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.h" ) diff --git a/include/kimera-vio/visualizer/Display.h b/include/kimera-vio/visualizer/Display.h index 9a35e83ac..971f43030 100644 --- a/include/kimera-vio/visualizer/Display.h +++ b/include/kimera-vio/visualizer/Display.h @@ -39,105 +39,4 @@ class DisplayBase { virtual void spinOnce(DisplayInputBase::UniquePtr&& viz_output) = 0; }; -class OpenCv3dDisplay : public DisplayBase { - public: - KIMERA_POINTER_TYPEDEFS(OpenCv3dDisplay); - KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCv3dDisplay); - - OpenCv3dDisplay(const ShutdownPipelineCallback& shutdown_pipeline_cb); - - // TODO(Toni): consider using `unregisterAllWindows` - ~OpenCv3dDisplay() override = default; - - // Spins renderers to display data using OpenCV imshow and viz3d - // Displaying must be done in the main thread. - void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override; - - private: - /** - * @brief safeCast Try to cast display input base to the derived visualizer - * output, if unsuccessful, it will return a nullptr. - * @param display_input_base - * @return - */ - VisualizerOutput::UniquePtr safeCast( - DisplayInputBase::UniquePtr display_input_base) { - if (!display_input_base) return nullptr; - VisualizerOutput::UniquePtr viz_output; - VisualizerOutput* tmp = nullptr; - try { - tmp = dynamic_cast(display_input_base.get()); - } catch (const std::bad_cast& e) { - LOG(ERROR) << "Seems that you are casting DisplayInputBase to " - "VisualizerOutput, but this object is not " - "a VisualizerOutput!\n" - "Error: " << e.what(); - return nullptr; - } catch (...) { - LOG(FATAL) << "Exception caught when casting to VisualizerOutput."; - } - if(!tmp) return nullptr; - display_input_base.release(); - viz_output.reset(tmp); - return viz_output; - } - - // Adds 3D widgets to the window, and displays it. - void spin3dWindow(VisualizerOutput::UniquePtr&& viz_output); - - void spin2dWindow(const DisplayInputBase& viz_output); - - //! Sets the visualization properties of the 3D mesh. - void setMeshProperties(WidgetsMap* widgets); - - //! Sets a 3D Widget Pose, because Widget3D::setPose() doesn't work; - void setFrustumPose(const cv::Affine3d& frustum_pose); - - // Keyboard callback. - static void keyboardCallback(const cv::viz::KeyboardEvent& event, void* t); - - // Keyboard callback to toggle freezing screen. - static void toggleFreezeScreenKeyboardCallback(const uchar& code, - WindowData* window_data); - - // Keyboard callback to set mesh representation. - static void setMeshRepresentation(const uchar& code, WindowData* window_data); - - // Keyboard callback to set mesh shading. - static void setMeshShadingCallback(const uchar& code, - WindowData* window_data); - - // Keyboard callback to set mesh ambient. - static void setMeshAmbientCallback(const uchar& code, - WindowData* window_data); - - // Keyboard callback to set mesh lighting. - static void setMeshLightingCallback(const uchar& code, - WindowData* window_data); - - // Keyboard callback to get current viewer pose. - static void getViewerPoseKeyboardCallback(const uchar& code, - WindowData* window_data); - - // Keyboard callback to get current screen size. - static void getCurrentWindowSizeKeyboardCallback(const uchar& code, - WindowData* window_data); - - // Keyboard callback to get screenshot of current windodw. - static void getScreenshotCallback(const uchar& code, WindowData* window_data); - - // Record video sequence at a hardcoded directory relative to executable. - void recordVideo(); - - // Useful for when testing on servers without display screen. - void setOffScreenRendering(); - - private: - WindowData window_data_; - - //! We use this callback to shutdown the pipeline gracefully if - //! the visualization window is closed. - ShutdownPipelineCallback shutdown_pipeline_cb_; -}; - } // namespace VIO diff --git a/include/kimera-vio/visualizer/DisplayFactory.h b/include/kimera-vio/visualizer/DisplayFactory.h index e395bec2c..7780e5ccf 100644 --- a/include/kimera-vio/visualizer/DisplayFactory.h +++ b/include/kimera-vio/visualizer/DisplayFactory.h @@ -18,6 +18,7 @@ #include "kimera-vio/utils/Macros.h" #include "kimera-vio/visualizer/Display.h" +#include "kimera-vio/visualizer/OpenCvDisplay.h" namespace VIO { diff --git a/include/kimera-vio/visualizer/OpenCvDisplay.h b/include/kimera-vio/visualizer/OpenCvDisplay.h new file mode 100644 index 000000000..32c054320 --- /dev/null +++ b/include/kimera-vio/visualizer/OpenCvDisplay.h @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + * Copyright 2017, Massachusetts Institute of Technology, + * Cambridge, MA 02139 + * All Rights Reserved + * Authors: Luca Carlone, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file OpenCvDisplay.h + * @brief Class to display visualizer output using OpenCV + * @author Antoni Rosinol + */ + +#pragma once + +#include + +#include "kimera-vio/pipeline/Pipeline-definitions.h" // Needed for shutdown cb +#include "kimera-vio/utils/Macros.h" +#include "kimera-vio/visualizer/Display-definitions.h" +#include "kimera-vio/visualizer/Display.h" +#include "kimera-vio/visualizer/Visualizer3D-definitions.h" + +namespace VIO { + +class OpenCv3dDisplay : public DisplayBase { + public: + KIMERA_POINTER_TYPEDEFS(OpenCv3dDisplay); + KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCv3dDisplay); + + OpenCv3dDisplay(const ShutdownPipelineCallback& shutdown_pipeline_cb); + + // TODO(Toni): consider using `unregisterAllWindows` + ~OpenCv3dDisplay() override = default; + + /** + * @brief spinOnce + * Spins renderers to display data using OpenCV imshow and viz3d + * Displaying must be done in the main thread, that it is why it is separated + * from Visualizer3D (plus it makes everything faster as displaying and + * building 3D graphics is decoupled). + * @param viz_output + */ + void spinOnce(DisplayInputBase::UniquePtr&& viz_output) override; + + private: + //! Adds 3D widgets to the window, and displays it. + void spin3dWindow(VisualizerOutput::UniquePtr&& viz_output); + + //! Visualizes 2D data. + void spin2dWindow(const DisplayInputBase& viz_output); + + //! Sets the visualization properties of the 3D mesh. + void setMeshProperties(WidgetsMap* widgets); + + //! Sets a 3D Widget Pose, because Widget3D::setPose() doesn't work; + void setFrustumPose(const cv::Affine3d& frustum_pose); + + //! Keyboard callback. + static void keyboardCallback(const cv::viz::KeyboardEvent& event, void* t); + + //! Keyboard callback to toggle freezing screen. + static void toggleFreezeScreenKeyboardCallback(const uchar& code, + WindowData* window_data); + + //! Keyboard callback to set mesh representation. + static void setMeshRepresentation(const uchar& code, WindowData* window_data); + + //! Keyboard callback to set mesh shading. + static void setMeshShadingCallback(const uchar& code, + WindowData* window_data); + + //! Keyboard callback to set mesh ambient. + static void setMeshAmbientCallback(const uchar& code, + WindowData* window_data); + + //! Keyboard callback to set mesh lighting. + static void setMeshLightingCallback(const uchar& code, + WindowData* window_data); + + //! Keyboard callback to get current viewer pose. + static void getViewerPoseKeyboardCallback(const uchar& code, + WindowData* window_data); + + //! Keyboard callback to get current screen size. + static void getCurrentWindowSizeKeyboardCallback(const uchar& code, + WindowData* window_data); + + //! Keyboard callback to get screenshot of current windodw. + static void getScreenshotCallback(const uchar& code, WindowData* window_data); + + //! Record video sequence at a hardcoded directory relative to executable. + void recordVideo(); + + //! Useful for when testing on servers without display screen. + void setOffScreenRendering(); + + /** + * @brief safeCast Try to cast display input base to the derived visualizer + * output, if unsuccessful, it will return a nullptr. + * @param display_input_base + * @return + */ + VisualizerOutput::UniquePtr safeCast( + DisplayInputBase::UniquePtr display_input_base); + + private: + //! Data to visualize (in 3D) + WindowData window_data_; + + //! We use this callback to shutdown the pipeline gracefully if + //! the visualization window is closed. + ShutdownPipelineCallback shutdown_pipeline_cb_; +}; + +} // namespace VIO diff --git a/src/visualizer/CMakeLists.txt b/src/visualizer/CMakeLists.txt index 9f7166d88..09dac2198 100644 --- a/src/visualizer/CMakeLists.txt +++ b/src/visualizer/CMakeLists.txt @@ -6,6 +6,7 @@ target_sources(kimera_vio "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.cpp" "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.cpp" "${CMAKE_CURRENT_LIST_DIR}/Display.cpp" + "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.cpp" "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.cpp" "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.cpp" ) diff --git a/src/visualizer/Display.cpp b/src/visualizer/Display.cpp index ee276a8f3..05a7a48bd 100644 --- a/src/visualizer/Display.cpp +++ b/src/visualizer/Display.cpp @@ -14,310 +14,6 @@ #include "kimera-vio/visualizer/Display.h" -#include - -#include - -#include - -#include "kimera-vio/utils/FilesystemUtils.h" - namespace VIO { -OpenCv3dDisplay::OpenCv3dDisplay( - const ShutdownPipelineCallback& shutdown_pipeline_cb) - : DisplayBase(), - window_data_(), - shutdown_pipeline_cb_(shutdown_pipeline_cb) { - if (VLOG_IS_ON(2)) { - window_data_.window_.setGlobalWarnings(true); - } else { - window_data_.window_.setGlobalWarnings(false); - } - window_data_.window_.registerKeyboardCallback(keyboardCallback, - &window_data_); - window_data_.window_.setBackgroundColor(window_data_.background_color_); - window_data_.window_.showWidget("Coordinate Widget", - cv::viz::WCoordinateSystem()); - - // TODO(Toni): not sure if we need this... - // See page 211: - // Learning OpenCV 3: Computer Vision in C++ with the OpenCV Library - //LOG_IF(WARNING, cv::startWindowThread() == 0) - // << "Could not start OpenCV window thread."; - - // TODO(Toni): perhaps we want to use these in the future - // cv::createButton(nameb2, callbackButton,NULL,QT_CHECKBOX,0); - // cv::createTrackbar( "track2", NULL, &value2, 255, NULL); - // cv::setMouseCallback( "main2",on_mouse,NULL ); -} - -void OpenCv3dDisplay::spinOnce(DisplayInputBase::UniquePtr&& display_input) { - CHECK(display_input); - // Display 2D images. - spin2dWindow(*display_input); - // Display 3D window. - spin3dWindow(safeCast(std::move(display_input))); -} - -// Adds 3D widgets to the window, and displays it. -void OpenCv3dDisplay::spin3dWindow(VisualizerOutput::UniquePtr&& viz_output) { - // Only display if we have a valid pointer. - if (!viz_output) return; - - if (viz_output->visualization_type_ != VisualizationType::kNone) { - if (window_data_.window_.wasStopped()) { - // Shutdown the pipeline! This works because display is running in the - // main thread, otherwise you would get a nasty: - // ``` - // terminate called after throwing an instance of 'std::system_error' - // what(): Resource deadlock avoided - // ``` - shutdown_pipeline_cb_(); - } - // viz_output.window_->spinOnce(1, true); - setMeshProperties(&viz_output->widgets_); - for (const auto& widget : viz_output->widgets_) { - CHECK(widget.second); - window_data_.window_.showWidget(widget.first, *(widget.second)); - } - setFrustumPose(viz_output->frustum_pose_); - window_data_.window_.spinOnce(1, true); - } -} - -void OpenCv3dDisplay::spin2dWindow(const DisplayInputBase& viz_output) { - for (const ImageToDisplay& img_to_display : viz_output.images_to_display_) { - cv::namedWindow(img_to_display.name_); - cv::imshow(img_to_display.name_, img_to_display.image_); - } - VLOG(10) << "Spin Visualize 2D output."; - cv::waitKey(1); // Not needed because we are using startWindowThread() -} - -void OpenCv3dDisplay::setFrustumPose(const cv::Affine3d& frustum_pose) { - static const std::string kFrustum = "Camera Pose with Frustum"; - try { - window_data_.window_.setWidgetPose(kFrustum, frustum_pose); - } catch (...) { - LOG(ERROR) << "Setting widget pose for " << kFrustum << " failed."; - } -} - -void OpenCv3dDisplay::setMeshProperties(WidgetsMap* widgets) { - CHECK_NOTNULL(widgets); - static const std::string kMesh = "Mesh"; - auto mesh_iterator = widgets->find(kMesh); - if (mesh_iterator == widgets->end()) { - LOG_EVERY_N(WARNING, 100) << "Missing Mesh in visualization's 3D widgets."; - return; - } - WidgetPtr& mesh_widget = mesh_iterator->second; - // Decide mesh shading style. - switch (window_data_.mesh_shading_) { - case 0: { - mesh_widget->setRenderingProperty(cv::viz::SHADING, - cv::viz::SHADING_FLAT); - break; - } - case 1: { - mesh_widget->setRenderingProperty(cv::viz::SHADING, - cv::viz::SHADING_GOURAUD); - break; - } - case 2: { - mesh_widget->setRenderingProperty(cv::viz::SHADING, - cv::viz::SHADING_PHONG); - break; - } - default: { break; } - } - - // Decide mesh representation style. - switch (window_data_.mesh_representation_) { - case 0: { - mesh_widget->setRenderingProperty(cv::viz::REPRESENTATION, - cv::viz::REPRESENTATION_POINTS); - mesh_widget->setRenderingProperty(cv::viz::POINT_SIZE, 8); - break; - } - case 1: { - mesh_widget->setRenderingProperty(cv::viz::REPRESENTATION, - cv::viz::REPRESENTATION_SURFACE); - break; - } - case 2: { - mesh_widget->setRenderingProperty(cv::viz::REPRESENTATION, - cv::viz::REPRESENTATION_WIREFRAME); - break; - } - default: { break; } - } - mesh_widget->setRenderingProperty(cv::viz::AMBIENT, - window_data_.mesh_ambient_); - mesh_widget->setRenderingProperty(cv::viz::LIGHTING, - window_data_.mesh_lighting_); -} - -void OpenCv3dDisplay::keyboardCallback(const cv::viz::KeyboardEvent& event, - void* t) { - WindowData* window_data = static_cast(t); - if (event.action == cv::viz::KeyboardEvent::Action::KEY_DOWN) { - toggleFreezeScreenKeyboardCallback(event.code, window_data); - setMeshRepresentation(event.code, window_data); - setMeshShadingCallback(event.code, window_data); - setMeshAmbientCallback(event.code, window_data); - setMeshLightingCallback(event.code, window_data); - getViewerPoseKeyboardCallback(event.code, window_data); - getCurrentWindowSizeKeyboardCallback(event.code, window_data); - getScreenshotCallback(event.code, window_data); - } -} - -// Keyboard callback to toggle freezing screen. -void OpenCv3dDisplay::toggleFreezeScreenKeyboardCallback( - const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == 't') { - LOG(WARNING) << "Pressing " << code << " toggles freezing screen."; - static bool freeze = false; - freeze = !freeze; // Toggle. - window_data->window_.spinOnce(1, true); - while (!window_data->window_.wasStopped()) { - if (freeze) { - window_data->window_.spinOnce(1, true); - } else { - break; - } - } - } -} - -// Keyboard callback to set mesh representation. -void OpenCv3dDisplay::setMeshRepresentation(const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == '0') { - LOG(WARNING) << "Pressing " << code - << " sets mesh representation to " - "a point cloud."; - window_data->mesh_representation_ = 0u; - } else if (code == '1') { - LOG(WARNING) << "Pressing " << code - << " sets mesh representation to " - "a mesh."; - window_data->mesh_representation_ = 1u; - } else if (code == '2') { - LOG(WARNING) << "Pressing " << code - << " sets mesh representation to " - "a wireframe."; - window_data->mesh_representation_ = 2u; - } -} - -// Keyboard callback to set mesh shading. -void OpenCv3dDisplay::setMeshShadingCallback(const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == '4') { - LOG(WARNING) << "Pressing " << code - << " sets mesh shading to " - "flat."; - window_data->mesh_shading_ = 0u; - } else if (code == '5') { - LOG(WARNING) << "Pressing " << code - << " sets mesh shading to " - "Gouraud."; - window_data->mesh_shading_ = 1u; - } else if (code == '6') { - LOG(WARNING) << "Pressing " << code - << " sets mesh shading to " - "Phong."; - window_data->mesh_shading_ = 2u; - } -} - -// Keyboard callback to set mesh ambient. -void OpenCv3dDisplay::setMeshAmbientCallback(const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == 'a') { - window_data->mesh_ambient_ = !window_data->mesh_ambient_; - LOG(WARNING) << "Pressing " << code << " toggles mesh ambient." - << " Now set to " << window_data->mesh_ambient_; - } -} - -// Keyboard callback to set mesh lighting. -void OpenCv3dDisplay::setMeshLightingCallback(const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == 'l') { - window_data->mesh_lighting_ = !window_data->mesh_lighting_; - LOG(WARNING) << "Pressing " << code << " toggles mesh lighting." - << " Now set to " << window_data->mesh_lighting_; - } -} - -// Keyboard callback to get current viewer pose. -void OpenCv3dDisplay::getViewerPoseKeyboardCallback(const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == 'v') { - LOG(INFO) << "Current viewer pose:\n" - << "\tRodriguez vector: " - << window_data->window_.getViewerPose().rvec() - << "\n\tAffine matrix: " - << window_data->window_.getViewerPose().matrix; - } -} - -// Keyboard callback to get current screen size. -void OpenCv3dDisplay::getCurrentWindowSizeKeyboardCallback( - const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == 'w') { - LOG(WARNING) << "Pressing " << code << " displays current window size:\n" - << "\theight: " << window_data->window_.getWindowSize().height - << "\twidth: " << window_data->window_.getWindowSize().width; - } -} - -// Keyboard callback to get screenshot of current windodw. -void OpenCv3dDisplay::getScreenshotCallback(const uchar& code, - WindowData* window_data) { - CHECK_NOTNULL(window_data); - if (code == 's') { - int i = 0; - std::string filename = "screenshot_3d_window" + std::to_string(i); - LOG(WARNING) << "Pressing " << code - << " takes a screenshot of the " - "window, saved in: " + - filename; - window_data->window_.saveScreenshot(filename); - } -} - -// Record video sequence at a hardcoded directory relative to executable. -void OpenCv3dDisplay::recordVideo() { - int i = 0u; - const std::string dir_path = "."; - const std::string dir_name = "3d_viz_video"; - const std::string dir_full_path = common::pathAppend(dir_path, dir_name); - if (i == 0u) CHECK(common::createDirectory(dir_path, dir_name)); - std::string screenshot_path = - common::pathAppend(dir_full_path, std::to_string(i)); - i++; - LOG(WARNING) << "Recording video sequence for 3d Viz, " - << "current frame saved in: " + screenshot_path; - // window_data_.window_.saveScreenshot(screenshot_path); - LOG(ERROR) << "WTF"; -} - -void OpenCv3dDisplay::setOffScreenRendering() { - window_data_.window_.setOffScreenRendering(); -} - } // namespace VIO diff --git a/src/visualizer/OpenCvDisplay.cpp b/src/visualizer/OpenCvDisplay.cpp new file mode 100644 index 000000000..d72398a2c --- /dev/null +++ b/src/visualizer/OpenCvDisplay.cpp @@ -0,0 +1,341 @@ +/* ---------------------------------------------------------------------------- + * Copyright 2017, Massachusetts Institute of Technology, + * Cambridge, MA 02139 + * All Rights Reserved + * Authors: Luca Carlone, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file OpenCvDisplay.cpp + * @brief Class to display visualizer output using OpenCV + * @author Antoni Rosinol + */ + +#include "kimera-vio/visualizer/OpenCvDisplay.h" + +#include + +#include + +#include + +#include "kimera-vio/utils/FilesystemUtils.h" + +namespace VIO { + +OpenCv3dDisplay::OpenCv3dDisplay( + const ShutdownPipelineCallback& shutdown_pipeline_cb) + : DisplayBase(), + window_data_(), + shutdown_pipeline_cb_(shutdown_pipeline_cb) { + if (VLOG_IS_ON(2)) { + window_data_.window_.setGlobalWarnings(true); + } else { + window_data_.window_.setGlobalWarnings(false); + } + window_data_.window_.registerKeyboardCallback(keyboardCallback, + &window_data_); + window_data_.window_.setBackgroundColor(window_data_.background_color_); + window_data_.window_.showWidget("Coordinate Widget", + cv::viz::WCoordinateSystem()); + + // TODO(Toni): not sure if we need this... + // See page 211: + // Learning OpenCV 3: Computer Vision in C++ with the OpenCV Library + // LOG_IF(WARNING, cv::startWindowThread() == 0) + // << "Could not start OpenCV window thread."; + + // TODO(Toni): perhaps we want to use these in the future + // cv::createButton(nameb2, callbackButton,NULL,QT_CHECKBOX,0); + // cv::createTrackbar( "track2", NULL, &value2, 255, NULL); + // cv::setMouseCallback( "main2",on_mouse,NULL ); +} + +void OpenCv3dDisplay::spinOnce(DisplayInputBase::UniquePtr&& display_input) { + CHECK(display_input); + // Display 2D images. + spin2dWindow(*display_input); + // Display 3D window. + spin3dWindow(safeCast(std::move(display_input))); +} + + +// Adds 3D widgets to the window, and displays it. +void OpenCv3dDisplay::spin3dWindow(VisualizerOutput::UniquePtr&& viz_output) { + // Only display if we have a valid pointer. + if (!viz_output) return; + + if (viz_output->visualization_type_ != VisualizationType::kNone) { + if (window_data_.window_.wasStopped()) { + // Shutdown the pipeline! This works because display is running in the + // main thread, otherwise you would get a nasty: + // ``` + // terminate called after throwing an instance of 'std::system_error' + // what(): Resource deadlock avoided + // ``` + shutdown_pipeline_cb_(); + } + // viz_output.window_->spinOnce(1, true); + setMeshProperties(&viz_output->widgets_); + for (const auto& widget : viz_output->widgets_) { + CHECK(widget.second); + window_data_.window_.showWidget(widget.first, *(widget.second)); + } + setFrustumPose(viz_output->frustum_pose_); + window_data_.window_.spinOnce(1, true); + } +} + +void OpenCv3dDisplay::spin2dWindow(const DisplayInputBase& viz_output) { + for (const ImageToDisplay& img_to_display : viz_output.images_to_display_) { + cv::namedWindow(img_to_display.name_); + cv::imshow(img_to_display.name_, img_to_display.image_); + } + VLOG(10) << "Spin Visualize 2D output."; + cv::waitKey(1); // Not needed because we are using startWindowThread() +} + +void OpenCv3dDisplay::setFrustumPose(const cv::Affine3d& frustum_pose) { + static const std::string kFrustum = "Camera Pose with Frustum"; + try { + window_data_.window_.setWidgetPose(kFrustum, frustum_pose); + } catch (...) { + LOG(ERROR) << "Setting widget pose for " << kFrustum << " failed."; + } +} + +void OpenCv3dDisplay::setMeshProperties(WidgetsMap* widgets) { + CHECK_NOTNULL(widgets); + static const std::string kMesh = "Mesh"; + auto mesh_iterator = widgets->find(kMesh); + if (mesh_iterator == widgets->end()) { + LOG_EVERY_N(WARNING, 100) << "Missing Mesh in visualization's 3D widgets."; + return; + } + WidgetPtr& mesh_widget = mesh_iterator->second; + // Decide mesh shading style. + switch (window_data_.mesh_shading_) { + case 0: { + mesh_widget->setRenderingProperty(cv::viz::SHADING, + cv::viz::SHADING_FLAT); + break; + } + case 1: { + mesh_widget->setRenderingProperty(cv::viz::SHADING, + cv::viz::SHADING_GOURAUD); + break; + } + case 2: { + mesh_widget->setRenderingProperty(cv::viz::SHADING, + cv::viz::SHADING_PHONG); + break; + } + default: { break; } + } + + // Decide mesh representation style. + switch (window_data_.mesh_representation_) { + case 0: { + mesh_widget->setRenderingProperty(cv::viz::REPRESENTATION, + cv::viz::REPRESENTATION_POINTS); + mesh_widget->setRenderingProperty(cv::viz::POINT_SIZE, 8); + break; + } + case 1: { + mesh_widget->setRenderingProperty(cv::viz::REPRESENTATION, + cv::viz::REPRESENTATION_SURFACE); + break; + } + case 2: { + mesh_widget->setRenderingProperty(cv::viz::REPRESENTATION, + cv::viz::REPRESENTATION_WIREFRAME); + break; + } + default: { break; } + } + mesh_widget->setRenderingProperty(cv::viz::AMBIENT, + window_data_.mesh_ambient_); + mesh_widget->setRenderingProperty(cv::viz::LIGHTING, + window_data_.mesh_lighting_); +} + +void OpenCv3dDisplay::keyboardCallback(const cv::viz::KeyboardEvent& event, + void* t) { + WindowData* window_data = static_cast(t); + if (event.action == cv::viz::KeyboardEvent::Action::KEY_DOWN) { + toggleFreezeScreenKeyboardCallback(event.code, window_data); + setMeshRepresentation(event.code, window_data); + setMeshShadingCallback(event.code, window_data); + setMeshAmbientCallback(event.code, window_data); + setMeshLightingCallback(event.code, window_data); + getViewerPoseKeyboardCallback(event.code, window_data); + getCurrentWindowSizeKeyboardCallback(event.code, window_data); + getScreenshotCallback(event.code, window_data); + } +} + +// Keyboard callback to toggle freezing screen. +void OpenCv3dDisplay::toggleFreezeScreenKeyboardCallback( + const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == 't') { + LOG(WARNING) << "Pressing " << code << " toggles freezing screen."; + static bool freeze = false; + freeze = !freeze; // Toggle. + window_data->window_.spinOnce(1, true); + while (!window_data->window_.wasStopped()) { + if (freeze) { + window_data->window_.spinOnce(1, true); + } else { + break; + } + } + } +} + +// Keyboard callback to set mesh representation. +void OpenCv3dDisplay::setMeshRepresentation(const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == '0') { + LOG(WARNING) << "Pressing " << code << " sets mesh representation to " + "a point cloud."; + window_data->mesh_representation_ = 0u; + } else if (code == '1') { + LOG(WARNING) << "Pressing " << code << " sets mesh representation to " + "a mesh."; + window_data->mesh_representation_ = 1u; + } else if (code == '2') { + LOG(WARNING) << "Pressing " << code << " sets mesh representation to " + "a wireframe."; + window_data->mesh_representation_ = 2u; + } +} + +// Keyboard callback to set mesh shading. +void OpenCv3dDisplay::setMeshShadingCallback(const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == '4') { + LOG(WARNING) << "Pressing " << code << " sets mesh shading to " + "flat."; + window_data->mesh_shading_ = 0u; + } else if (code == '5') { + LOG(WARNING) << "Pressing " << code << " sets mesh shading to " + "Gouraud."; + window_data->mesh_shading_ = 1u; + } else if (code == '6') { + LOG(WARNING) << "Pressing " << code << " sets mesh shading to " + "Phong."; + window_data->mesh_shading_ = 2u; + } +} + +// Keyboard callback to set mesh ambient. +void OpenCv3dDisplay::setMeshAmbientCallback(const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == 'a') { + window_data->mesh_ambient_ = !window_data->mesh_ambient_; + LOG(WARNING) << "Pressing " << code << " toggles mesh ambient." + << " Now set to " << window_data->mesh_ambient_; + } +} + +// Keyboard callback to set mesh lighting. +void OpenCv3dDisplay::setMeshLightingCallback(const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == 'l') { + window_data->mesh_lighting_ = !window_data->mesh_lighting_; + LOG(WARNING) << "Pressing " << code << " toggles mesh lighting." + << " Now set to " << window_data->mesh_lighting_; + } +} + +// Keyboard callback to get current viewer pose. +void OpenCv3dDisplay::getViewerPoseKeyboardCallback(const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == 'v') { + LOG(INFO) << "Current viewer pose:\n" + << "\tRodriguez vector: " + << window_data->window_.getViewerPose().rvec() + << "\n\tAffine matrix: " + << window_data->window_.getViewerPose().matrix; + } +} + +// Keyboard callback to get current screen size. +void OpenCv3dDisplay::getCurrentWindowSizeKeyboardCallback( + const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == 'w') { + LOG(WARNING) << "Pressing " << code << " displays current window size:\n" + << "\theight: " << window_data->window_.getWindowSize().height + << "\twidth: " << window_data->window_.getWindowSize().width; + } +} + +// Keyboard callback to get screenshot of current windodw. +void OpenCv3dDisplay::getScreenshotCallback(const uchar& code, + WindowData* window_data) { + CHECK_NOTNULL(window_data); + if (code == 's') { + int i = 0; + std::string filename = "screenshot_3d_window" + std::to_string(i); + LOG(WARNING) << "Pressing " << code + << " takes a screenshot of the " + "window, saved in: " + + filename; + window_data->window_.saveScreenshot(filename); + } +} + +// Record video sequence at a hardcoded directory relative to executable. +void OpenCv3dDisplay::recordVideo() { + int i = 0u; + const std::string dir_path = "."; + const std::string dir_name = "3d_viz_video"; + const std::string dir_full_path = common::pathAppend(dir_path, dir_name); + if (i == 0u) CHECK(common::createDirectory(dir_path, dir_name)); + std::string screenshot_path = + common::pathAppend(dir_full_path, std::to_string(i)); + i++; + LOG(WARNING) << "Recording video sequence for 3d Viz, " + << "current frame saved in: " + screenshot_path; + // window_data_.window_.saveScreenshot(screenshot_path); + LOG(ERROR) << "WTF"; +} + +void OpenCv3dDisplay::setOffScreenRendering() { + window_data_.window_.setOffScreenRendering(); +} + +VisualizerOutput::UniquePtr OpenCv3dDisplay::safeCast( + DisplayInputBase::UniquePtr display_input_base) { + if (!display_input_base) return nullptr; + VisualizerOutput::UniquePtr viz_output; + VisualizerOutput* tmp = nullptr; + try { + tmp = dynamic_cast(display_input_base.get()); + } catch (const std::bad_cast& e) { + LOG(ERROR) << "Seems that you are casting DisplayInputBase to " + "VisualizerOutput, but this object is not " + "a VisualizerOutput!\n" + "Error: " + << e.what(); + return nullptr; + } catch (...) { + LOG(FATAL) << "Exception caught when casting to VisualizerOutput."; + } + if (!tmp) return nullptr; + display_input_base.release(); + viz_output.reset(tmp); + return viz_output; +} + +} // namespace VIO From cb9bff65a74b786534b77ebceae3315d67b4b50e Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 21:56:21 -0400 Subject: [PATCH 14/15] Separate OpenCV Visualizer from VisualizerBase --- include/kimera-vio/visualizer/CMakeLists.txt | 7 +- .../visualizer/OpenCvVisualizer3D.h | 264 ++++ .../visualizer/Visualizer3D-definitions.h | 24 +- include/kimera-vio/visualizer/Visualizer3D.h | 266 +--- .../visualizer/Visualizer3DFactory.h | 2 +- src/visualizer/CMakeLists.txt | 3 +- src/visualizer/OpenCvVisualizer3D.cpp | 1266 ++++++++++++++++ src/visualizer/Visualizer3D.cpp | 1269 ----------------- src/visualizer/Visualizer3DFactory.cpp | 3 +- 9 files changed, 1553 insertions(+), 1551 deletions(-) create mode 100644 include/kimera-vio/visualizer/OpenCvVisualizer3D.h create mode 100644 src/visualizer/OpenCvVisualizer3D.cpp diff --git a/include/kimera-vio/visualizer/CMakeLists.txt b/include/kimera-vio/visualizer/CMakeLists.txt index b5e0276d2..dcbb5e1ea 100644 --- a/include/kimera-vio/visualizer/CMakeLists.txt +++ b/include/kimera-vio/visualizer/CMakeLists.txt @@ -1,12 +1,13 @@ ### Add includes target_sources(kimera_vio PRIVATE - "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.h" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.h" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.h" + "${CMAKE_CURRENT_LIST_DIR}/OpenCvVisualizer3D.h" "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.h" - "${CMAKE_CURRENT_LIST_DIR}/Display.h" - "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.h" "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.h" "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.h" + "${CMAKE_CURRENT_LIST_DIR}/Display.h" + "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.h" ) diff --git a/include/kimera-vio/visualizer/OpenCvVisualizer3D.h b/include/kimera-vio/visualizer/OpenCvVisualizer3D.h new file mode 100644 index 000000000..efcc41b17 --- /dev/null +++ b/include/kimera-vio/visualizer/OpenCvVisualizer3D.h @@ -0,0 +1,264 @@ +/* ---------------------------------------------------------------------------- + * Copyright 2017, Massachusetts Institute of Technology, + * Cambridge, MA 02139 + * All Rights Reserved + * Authors: Luca Carlone, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file OpenCvVisualizer3D.h + * @brief Build and visualize 3D data: 2D mesh from Frame for example. + * @author Antoni Rosinol + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include "kimera-vio/backend/VioBackEnd-definitions.h" +#include "kimera-vio/logging/Logger.h" +#include "kimera-vio/mesh/Mesher-definitions.h" +#include "kimera-vio/utils/Macros.h" +#include "kimera-vio/visualizer/Visualizer3D-definitions.h" +#include "kimera-vio/visualizer/Visualizer3D.h" + +namespace VIO { + +class OpenCvVisualizer3D : public Visualizer3D { + public: + KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCvVisualizer3D); + KIMERA_POINTER_TYPEDEFS(OpenCvVisualizer3D); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef size_t LineNr; + typedef std::uint64_t PlaneId; + typedef std::map LmkIdToLineIdMap; + typedef std::map PlaneIdMap; + typedef std::function DisplayCallback; + + /** + * @brief Visualizer3D constructor + * @param viz_type: type of 3D visualization + * @param backend_type backend used so that we display the right info + */ + OpenCvVisualizer3D(const VisualizationType& viz_type, + const BackendType& backend_type); + virtual ~OpenCvVisualizer3D(); + + /** + * @brief registerMesh3dVizProperties (Legacy) this was used to paint the + * mesh with semantic labels if sent by third-party. + * @param cb callback + */ + inline void registerMesh3dVizProperties( + Mesh3dVizPropertiesSetterCallback cb) { + mesh3d_viz_properties_callback_ = cb; + } + + /** + * \brief Returns true if visualization is ready, false otherwise. + * The actual visualization must be done in the main thread, and as such, + * it is not done here to separate visualization preparation from display. + */ + VisualizerOutput::UniquePtr spinOnce(const VisualizerInput& input) override; + + // TODO(marcus): Is there any reason the following two methods must be + // private? + + //! Visualize 2d mesh. + static cv::Mat visualizeMesh2DStereo( + const std::vector& triangulation_2D, + const Frame& ref_frame); + + //! Create a 2D mesh from 2D corners in an image, coded as a Frame class + static cv::Mat visualizeMesh2D( + const std::vector& triangulation2D, + const cv::Mat& img, + const KeypointsCV& extra_keypoints = KeypointsCV()); + + private: + //! Visualize a 3D point cloud of unique 3D landmarks. + void visualizePoints3D(const PointsWithIdMap& points_with_id, + const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map, + WidgetsMap* widgets_map); + + //! Visualize a 3D point cloud of unique 3D landmarks with its connectivity. + void visualizePlane(const PlaneId& plane_index, + const double& n_x, + const double& n_y, + const double& n_z, + const double& d, + WidgetsMap* widgets_map, + const bool& visualize_plane_label = true, + const int& cluster_id = 1); + + //! Draw a line in opencv. + void drawLine(const std::string& line_id, + const double& from_x, + const double& from_y, + const double& from_z, + const double& to_x, + const double& to_y, + const double& to_z, + WidgetsMap* widgets); + + //! Same as above but with different interface + void drawLine(const std::string& line_id, + const cv::Point3d& pt1, + const cv::Point3d& pt2, + WidgetsMap* widgets); + + //! Visualize a 3D point cloud of unique 3D landmarks with its connectivity. + void visualizeMesh3D(const cv::Mat& mapPoints3d, + const cv::Mat& polygonsMesh, + WidgetsMap* widgets); + + //! Visualize a 3D point cloud of unique 3D landmarks with its connectivity, + //! and provide color for each polygon. + void visualizeMesh3D(const cv::Mat& map_points_3d, + const cv::Mat& colors, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets, + const cv::Mat& tcoords = cv::Mat(), + const cv::Mat& texture = cv::Mat()); + + /// Visualize a PLY from filename (absolute path). + void visualizePlyMesh(const std::string& filename, WidgetsMap* widgets); + + /// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. + /// Each triangle is colored depending on the cluster it is in, or gray if it + /// is in no cluster. + /// [in] clusters: a set of triangle clusters. The ids of the triangles must + /// match the order in polygons_mesh. + /// [in] map_points_3d: set of 3d points in the mesh, format is n rows, with + /// three columns (x, y, z). + /// [in] polygons_mesh: mesh faces, format is n rows, 1 column, + /// with [n id_a id_b id_c, ..., n /id_x id_y id_z], where n = polygon size + /// n=3 for triangles. + /// [in] color_mesh whether to color the mesh or not + /// [in] timestamp to store the timestamp of the mesh when logging the mesh. + void visualizeMesh3DWithColoredClusters( + const std::vector& planes, + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets, + const bool visualize_mesh_with_colored_polygon_clusters = false, + const Timestamp& timestamp = 0.0); + + //! Visualize convex hull in 2D for set of points in triangle cluster, + //! projected along the normal of the cluster. + void visualizeConvexHull(const TriangleCluster& cluster, + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets); + + //! Visualize trajectory. Adds an image to the frustum if cv::Mat is not empty. + void visualizeTrajectory3D(const cv::Mat& frustum_image, + cv::Affine3d* frustum_pose, + WidgetsMap* widgets_map); + + //!!!!!!!! Remove widget. True if successful, false if not. + bool removeWidget(const std::string& widget_id); + + //! Visualize line widgets from plane to lmks. + //! Point key is required to avoid duplicated lines! + void visualizePlaneConstraints(const PlaneId& plane_id, + const gtsam::Point3& normal, + const double& distance, + const LandmarkId& lmk_id, + const gtsam::Point3& point, + WidgetsMap* widgets); + + //! Remove line widgets from plane to lmks, for lines that are not pointing + //! to any lmk_id in lmk_ids. + void removeOldLines(const LandmarkIds& lmk_ids); + + //! Remove line widgets from plane to lmks. + void removePlaneConstraintsViz(const PlaneId& plane_id); + + //! Remove plane widget. + void removePlane(const PlaneId& plane_index, + const bool& remove_plane_label = true); + + //! Add pose to the previous trajectory. + void addPoseToTrajectory(const gtsam::Pose3& current_pose_gtsam); + + static Mesh3DVizProperties texturizeMesh3D(const Timestamp& image_timestamp, + const cv::Mat& texture_image, + const Mesh2D& mesh_2d, + const Mesh3D& mesh_3d); + + private: + //! Flags for visualization behaviour. + const BackendType backend_type_; + + //! Callbacks. + //! Mesh 3d visualization properties setter callback. + Mesh3dVizPropertiesSetterCallback mesh3d_viz_properties_callback_; + + std::deque trajectory_poses_3d_; + + std::map plane_to_line_nr_map_; + PlaneIdMap plane_id_map_; + std::map is_plane_id_in_window_; + + //! Colors + cv::viz::Color cloud_color_ = cv::viz::Color::white(); + + //! Logging instance. + std::unique_ptr logger_; + + //! Log mesh to ply file. + void logMesh(const cv::Mat& map_points_3d, + const cv::Mat& colors, + const cv::Mat& polygons_mesh, + const Timestamp& timestamp, + bool log_accumulated_mesh = false); + + //! Input the mesh points and triangle clusters, and + //! output colors matrix for mesh visualizer. + //! This will color the point with the color of the last plane having it. + void colorMeshByClusters(const std::vector& planes, + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + cv::Mat* colors) const; + + //! Decide color of the cluster depending on its id. + void getColorById(const size_t& id, cv::viz::Color* color) const; + + //! Draw a line from lmk to plane center. + void drawLineFromPlaneToPoint(const std::string& line_id, + const double& plane_n_x, + const double& plane_n_y, + const double& plane_n_z, + const double& plane_d, + const double& point_x, + const double& point_y, + const double& point_z, + WidgetsMap* widgets); + + //! Update line from lmk to plane center. + void updateLineFromPlaneToPoint(const std::string& line_id, + const double& plane_n_x, + const double& plane_n_y, + const double& plane_n_z, + const double& plane_d, + const double& point_x, + const double& point_y, + const double& point_z, + WidgetsMap* widgets); +}; + +} // namespace VIO diff --git a/include/kimera-vio/visualizer/Visualizer3D-definitions.h b/include/kimera-vio/visualizer/Visualizer3D-definitions.h index b48df34d8..64715bf35 100644 --- a/include/kimera-vio/visualizer/Visualizer3D-definitions.h +++ b/include/kimera-vio/visualizer/Visualizer3D-definitions.h @@ -56,6 +56,18 @@ struct ImageToDisplay { cv::Mat image_; }; +struct DisplayInputBase { + KIMERA_POINTER_TYPEDEFS(DisplayInputBase); + KIMERA_DELETE_COPY_CONSTRUCTORS(DisplayInputBase); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DisplayInputBase() = default; + virtual ~DisplayInputBase() = default; + + Timestamp timestamp_; + std::vector images_to_display_; +}; +typedef ThreadsafeQueue DisplayQueue; + struct VisualizerInput : public PipelinePayload { KIMERA_POINTER_TYPEDEFS(VisualizerInput); KIMERA_DELETE_COPY_CONSTRUCTORS(VisualizerInput); @@ -82,18 +94,6 @@ struct VisualizerInput : public PipelinePayload { const FrontendOutput::ConstPtr frontend_output_; }; -struct DisplayInputBase { - KIMERA_POINTER_TYPEDEFS(DisplayInputBase); - KIMERA_DELETE_COPY_CONSTRUCTORS(DisplayInputBase); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - DisplayInputBase() = default; - virtual ~DisplayInputBase() = default; - - Timestamp timestamp_; - std::vector images_to_display_; -}; -typedef ThreadsafeQueue DisplayQueue; - struct VisualizerOutput : public DisplayInputBase { KIMERA_POINTER_TYPEDEFS(VisualizerOutput); KIMERA_DELETE_COPY_CONSTRUCTORS(VisualizerOutput); diff --git a/include/kimera-vio/visualizer/Visualizer3D.h b/include/kimera-vio/visualizer/Visualizer3D.h index 259df3d89..2e4beeb80 100644 --- a/include/kimera-vio/visualizer/Visualizer3D.h +++ b/include/kimera-vio/visualizer/Visualizer3D.h @@ -7,30 +7,13 @@ * -------------------------------------------------------------------------- */ /** - * @file Visualizer.h - * @brief Build and visualize 3D data: 2D mesh from Frame for example. + * @file Visualizer3D.h + * @brief Build and visualize 3D data: 2D mesh from frame for example. * @author Antoni Rosinol */ #pragma once -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include - -#include "kimera-vio/backend/VioBackEnd-definitions.h" -#include "kimera-vio/logging/Logger.h" -#include "kimera-vio/mesh/Mesher-definitions.h" #include "kimera-vio/utils/Macros.h" #include "kimera-vio/visualizer/Visualizer3D-definitions.h" @@ -57,249 +40,4 @@ class Visualizer3D { VisualizationType visualization_type_; }; -class OpenCvVisualizer3D : public Visualizer3D { - public: - KIMERA_DELETE_COPY_CONSTRUCTORS(OpenCvVisualizer3D); - KIMERA_POINTER_TYPEDEFS(OpenCvVisualizer3D); - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef size_t LineNr; - typedef std::uint64_t PlaneId; - typedef std::map LmkIdToLineIdMap; - typedef std::map PlaneIdMap; - typedef std::function DisplayCallback; - - /** - * @brief Visualizer3D constructor - * @param viz_type: type of 3D visualization - * @param backend_type backend used so that we display the right info - */ - OpenCvVisualizer3D(const VisualizationType& viz_type, - const BackendType& backend_type); - virtual ~OpenCvVisualizer3D() { - LOG(INFO) << "OpenCvVisualizer3D destructor"; - } - - /* ------------------------------------------------------------------------ */ - inline void registerMesh3dVizProperties( - Mesh3dVizPropertiesSetterCallback cb) { - mesh3d_viz_properties_callback_ = cb; - } - - /** - * \brief Returns true if visualization is ready, false otherwise. - * The actual visualization must be done in the main thread, and as such, - * it is not done here to separate visualization preparation from display. - */ - VisualizerOutput::UniquePtr spinOnce(const VisualizerInput& input) override; - - // TODO(marcus): Is there any reason the following two methods must be - // private? - - /* ------------------------------------------------------------------------ */ - // Visualize 2d mesh. - static cv::Mat visualizeMesh2DStereo( - const std::vector& triangulation_2D, - const Frame& ref_frame); - - /* ------------------------------------------------------------------------ */ - // Create a 2D mesh from 2D corners in an image, coded as a Frame class - static cv::Mat visualizeMesh2D( - const std::vector& triangulation2D, - const cv::Mat& img, - const KeypointsCV& extra_keypoints = KeypointsCV()); - - private: - /* ------------------------------------------------------------------------ */ - // Visualize a 3D point cloud of unique 3D landmarks. - void visualizePoints3D(const PointsWithIdMap& points_with_id, - const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map, - WidgetsMap* widgets_map); - - /* ------------------------------------------------------------------------ */ - // Visualize a 3D point cloud of unique 3D landmarks with its connectivity. - void visualizePlane(const PlaneId& plane_index, - const double& n_x, - const double& n_y, - const double& n_z, - const double& d, - WidgetsMap* widgets_map, - const bool& visualize_plane_label = true, - const int& cluster_id = 1); - - /* ------------------------------------------------------------------------ */ - // Draw a line in opencv. - void drawLine(const std::string& line_id, - const double& from_x, - const double& from_y, - const double& from_z, - const double& to_x, - const double& to_y, - const double& to_z, - WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - void drawLine(const std::string& line_id, - const cv::Point3d& pt1, - const cv::Point3d& pt2, - WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - // Visualize a 3D point cloud of unique 3D landmarks with its connectivity. - void visualizeMesh3D(const cv::Mat& mapPoints3d, - const cv::Mat& polygonsMesh, - WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - // Visualize a 3D point cloud of unique 3D landmarks with its connectivity, - // and provide color for each polygon. - void visualizeMesh3D(const cv::Mat& map_points_3d, - const cv::Mat& colors, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets, - const cv::Mat& tcoords = cv::Mat(), - const cv::Mat& texture = cv::Mat()); - - /* ------------------------------------------------------------------------ */ - /// Visualize a PLY from filename (absolute path). - void visualizePlyMesh(const std::string& filename, WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - /// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. - /// Each triangle is colored depending on the cluster it is in, or gray if it - /// is in no cluster. - /// [in] clusters: a set of triangle clusters. The ids of the triangles must - /// match the order in polygons_mesh. - /// [in] map_points_3d: set of 3d points in the mesh, format is n rows, with - /// three columns (x, y, z). - /// [in] polygons_mesh: mesh faces, format is n rows, 1 column, - /// with [n id_a id_b id_c, ..., n /id_x id_y id_z], where n = polygon size - /// n=3 for triangles. - /// [in] color_mesh whether to color the mesh or not - /// [in] timestamp to store the timestamp of the mesh when logging the mesh. - void visualizeMesh3DWithColoredClusters( - const std::vector& planes, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets, - const bool visualize_mesh_with_colored_polygon_clusters = false, - const Timestamp& timestamp = 0.0); - - /* ------------------------------------------------------------------------ */ - // Visualize convex hull in 2D for set of points in triangle cluster, - // projected along the normal of the cluster. - void visualizeConvexHull(const TriangleCluster& cluster, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - // Visualize trajectory. Adds an image to the frustum if cv::Mat is not empty. - void visualizeTrajectory3D(const cv::Mat& frustum_image, - cv::Affine3d* frustum_pose, - WidgetsMap* widgets_map); - - /* ------------------------------------------------------------------------ */ - // Remove widget. True if successful, false if not. - bool removeWidget(const std::string& widget_id); - - /* ------------------------------------------------------------------------ */ - // Visualize line widgets from plane to lmks. - // Point key is required to avoid duplicated lines! - void visualizePlaneConstraints(const PlaneId& plane_id, - const gtsam::Point3& normal, - const double& distance, - const LandmarkId& lmk_id, - const gtsam::Point3& point, - WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - // Remove line widgets from plane to lmks, for lines that are not pointing - // to any lmk_id in lmk_ids. - void removeOldLines(const LandmarkIds& lmk_ids); - - /* ------------------------------------------------------------------------ */ - // Remove line widgets from plane to lmks. - void removePlaneConstraintsViz(const PlaneId& plane_id); - - /* ------------------------------------------------------------------------ */ - // Remove plane widget. - void removePlane(const PlaneId& plane_index, - const bool& remove_plane_label = true); - - /* ------------------------------------------------------------------------ */ - // Add pose to the previous trajectory. - void addPoseToTrajectory(const gtsam::Pose3& current_pose_gtsam); - - /* ------------------------------------------------------------------------ */ - static Mesh3DVizProperties texturizeMesh3D(const Timestamp& image_timestamp, - const cv::Mat& texture_image, - const Mesh2D& mesh_2d, - const Mesh3D& mesh_3d); - - private: - // Flags for visualization behaviour. - const BackendType backend_type_; - - // Callbacks. - // Mesh 3d visualization properties setter callback. - Mesh3dVizPropertiesSetterCallback mesh3d_viz_properties_callback_; - - std::deque trajectory_poses_3d_; - - std::map plane_to_line_nr_map_; - PlaneIdMap plane_id_map_; - std::map is_plane_id_in_window_; - - //! Colors - cv::viz::Color cloud_color_ = cv::viz::Color::white(); - - //! Logging instance. - std::unique_ptr logger_; - - /* ------------------------------------------------------------------------ */ - // Log mesh to ply file. - void logMesh(const cv::Mat& map_points_3d, - const cv::Mat& colors, - const cv::Mat& polygons_mesh, - const Timestamp& timestamp, - bool log_accumulated_mesh = false); - - /* ------------------------------------------------------------------------ */ - // Input the mesh points and triangle clusters, and - // output colors matrix for mesh visualizer. - // This will color the point with the color of the last plane having it. - void colorMeshByClusters(const std::vector& planes, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - cv::Mat* colors) const; - - /* ------------------------------------------------------------------------ */ - // Decide color of the cluster depending on its id. - void getColorById(const size_t& id, cv::viz::Color* color) const; - - /* ------------------------------------------------------------------------ */ - // Draw a line from lmk to plane center. - void drawLineFromPlaneToPoint(const std::string& line_id, - const double& plane_n_x, - const double& plane_n_y, - const double& plane_n_z, - const double& plane_d, - const double& point_x, - const double& point_y, - const double& point_z, - WidgetsMap* widgets); - - /* ------------------------------------------------------------------------ */ - // Update line from lmk to plane center. - void updateLineFromPlaneToPoint(const std::string& line_id, - const double& plane_n_x, - const double& plane_n_y, - const double& plane_n_z, - const double& plane_d, - const double& point_x, - const double& point_y, - const double& point_z, - WidgetsMap* widgets); -}; - } // namespace VIO diff --git a/include/kimera-vio/visualizer/Visualizer3DFactory.h b/include/kimera-vio/visualizer/Visualizer3DFactory.h index 5b422149e..276b68871 100644 --- a/include/kimera-vio/visualizer/Visualizer3DFactory.h +++ b/include/kimera-vio/visualizer/Visualizer3DFactory.h @@ -30,7 +30,7 @@ class VisualizerFactory { VisualizerFactory() = delete; virtual ~VisualizerFactory() = default; - static OpenCvVisualizer3D::UniquePtr createVisualizer( + static Visualizer3D::UniquePtr createVisualizer( const VisualizerType visualizer_type, const VisualizationType& viz_type, const BackendType& backend_type); diff --git a/src/visualizer/CMakeLists.txt b/src/visualizer/CMakeLists.txt index 09dac2198..8fcfced4f 100644 --- a/src/visualizer/CMakeLists.txt +++ b/src/visualizer/CMakeLists.txt @@ -4,9 +4,10 @@ target_sources(kimera_vio "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.cpp" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.cpp" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/OpenCvVisualizer3D.cpp" "${CMAKE_CURRENT_LIST_DIR}/Display-definitions.cpp" "${CMAKE_CURRENT_LIST_DIR}/Display.cpp" - "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.cpp" "${CMAKE_CURRENT_LIST_DIR}/DisplayModule.cpp" "${CMAKE_CURRENT_LIST_DIR}/DisplayFactory.cpp" + "${CMAKE_CURRENT_LIST_DIR}/OpenCvDisplay.cpp" ) diff --git a/src/visualizer/OpenCvVisualizer3D.cpp b/src/visualizer/OpenCvVisualizer3D.cpp new file mode 100644 index 000000000..17756ad74 --- /dev/null +++ b/src/visualizer/OpenCvVisualizer3D.cpp @@ -0,0 +1,1266 @@ +/* ---------------------------------------------------------------------------- + * Copyright 2017, Massachusetts Institute of Technology, + * Cambridge, MA 02139 + * All Rights Reserved + * Authors: Luca Carlone, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file OpenCvVisualizer3D.cpp + * @brief Build and visualize 3D data: 2D mesh from frame for example. + * @author Antoni Rosinol + */ + +#include "kimera-vio/visualizer/Visualizer3D.h" + +#include // for min +#include // for shared_ptr<> +#include // for string +#include // for unordered_map<> +#include // for pair<> +#include // for vector<> + +#include + +#include + +#include "kimera-vio/backend/VioBackEnd-definitions.h" +#include "kimera-vio/utils/FilesystemUtils.h" +#include "kimera-vio/utils/Statistics.h" +#include "kimera-vio/utils/Timer.h" +#include "kimera-vio/utils/UtilsGTSAM.h" +#include "kimera-vio/utils/UtilsOpenCV.h" + +#include "kimera-vio/factors/PointPlaneFactor.h" // For visualization of constraints. + +// TODO(Toni): remove visualizer gflags! There are far too many, use a +// yaml params class (aka inherit from PipelineParams. +DEFINE_bool(visualize_mesh, false, "Enable 3D mesh visualization."); + +DEFINE_bool(visualize_mesh_2d, false, "Visualize mesh 2D."); + +DEFINE_bool(visualize_semantic_mesh, + false, + "Color the 3d mesh according to their semantic labels."); +DEFINE_bool(visualize_mesh_with_colored_polygon_clusters, + false, + "Color the polygon clusters according to their cluster id."); +DEFINE_bool(visualize_point_cloud, true, "Enable point cloud visualization."); +DEFINE_bool(visualize_convex_hull, false, "Enable convex hull visualization."); +DEFINE_bool(visualize_plane_constraints, + false, + "Enable plane constraints" + " visualization."); +DEFINE_bool(visualize_planes, false, "Enable plane visualization."); +DEFINE_bool(visualize_plane_label, false, "Enable plane label visualization."); +DEFINE_bool(visualize_mesh_in_frustum, + false, + "Enable mesh visualization in " + "camera frustum."); +DEFINE_string( + visualize_load_mesh_filename, + "", + "Load a mesh in the visualization, i.e. to visualize ground-truth " + "point cloud from Euroc's Vicon dataset."); + +// 3D Mesh related flags. +DEFINE_bool(texturize_3d_mesh, + false, + "Whether you want to add texture to the 3d" + "mesh. The texture is taken from the image" + " frame."); +DEFINE_bool(log_mesh, false, "Log the mesh at time horizon."); +DEFINE_bool(log_accumulated_mesh, false, "Accumulate the mesh when logging."); + +DEFINE_int32(displayed_trajectory_length, + 50, + "Set length of plotted trajectory." + "If -1 then all the trajectory is plotted."); + +namespace VIO { + +OpenCvVisualizer3D::OpenCvVisualizer3D(const VisualizationType& viz_type, + const BackendType& backend_type) + : Visualizer3D(viz_type), backend_type_(backend_type), logger_(nullptr) { + if (FLAGS_log_mesh) { + logger_ = VIO::make_unique(); + } +} + +OpenCvVisualizer3D::~OpenCvVisualizer3D() { + LOG(INFO) << "OpenCvVisualizer3D destructor"; +} + +// Returns true if visualization is ready, false otherwise. +// TODO(Toni): Put all flags inside spinOnce into Visualizer3DParams! +VisualizerOutput::UniquePtr OpenCvVisualizer3D::spinOnce( + const VisualizerInput& input) { + DCHECK(input.frontend_output_); + DCHECK(input.backend_output_); + + VisualizerOutput::UniquePtr output = VIO::make_unique(); + + // Ensure we have mesher output if the user requested mesh visualization + // otherwise, switch to pointcloud visualization. + if (visualization_type_ == VisualizationType::kMesh2dTo3dSparse && + !input.mesher_output_) { + LOG(ERROR) << "Mesh visualization requested, but no " + "mesher output available. Switching to Pointcloud" + "visualization only."; + visualization_type_ = VisualizationType::kPointcloud; + } + output->visualization_type_ = visualization_type_; + + cv::Mat mesh_2d_img; // Only for visualization. + const Frame& left_stereo_keyframe = + input.frontend_output_->stereo_frame_lkf_.getLeftFrame(); + switch (visualization_type_) { + // Computes and visualizes 3D mesh from 2D triangulation. + // vertices: all leftframe kps with right-VALID (3D), lmkId != -1 and + // inside the image triangles: all the ones with edges inside images as + // produced by cv::subdiv, which have uniform gradient (updateMesh3D also + // filters out geometrically) Sparsity comes from filtering out triangles + // corresponding to non planar obstacles which are assumed to have + // non-uniform gradient. + case VisualizationType::kMesh2dTo3dSparse: { + CHECK(input.mesher_output_); + // Visualize 2d mesh. + if (FLAGS_visualize_mesh_2d || FLAGS_visualize_mesh_in_frustum) { + const ImageToDisplay& mesh_display = ImageToDisplay( + "Mesh 2D", + visualizeMesh2DStereo(input.mesher_output_->mesh_2d_for_viz_, + left_stereo_keyframe)); + if (FLAGS_visualize_mesh_2d) { + output->images_to_display_.push_back(mesh_display); + } + if (FLAGS_visualize_mesh_in_frustum) { + mesh_2d_img = mesh_display.image_; + } + } + + // 3D mesh visualization + VLOG(10) << "Starting 3D mesh visualization..."; + + static std::vector planes_prev; + static PointsWithIdMap points_with_id_VIO_prev; + static LmkIdToLmkTypeMap lmk_id_to_lmk_type_map_prev; + static cv::Mat vertices_mesh_prev; + static cv::Mat polygons_mesh_prev; + static Mesh3DVizProperties mesh_3d_viz_props_prev; + + if (FLAGS_visualize_mesh) { + VLOG(10) << "Visualize mesh."; + if (FLAGS_visualize_semantic_mesh) { + VLOG(10) << "Visualize Semantic mesh."; + LOG_IF(WARNING, FLAGS_visualize_mesh_with_colored_polygon_clusters) + << "Both gflags visualize_semantic_mesh and " + "visualize_mesh_with_colored_polygon_cluster are set to True," + " but visualization of the semantic mesh has priority over " + "visualization of the polygon clusters."; + visualizeMesh3D(vertices_mesh_prev, + mesh_3d_viz_props_prev.colors_, + polygons_mesh_prev, + &output->widgets_, + mesh_3d_viz_props_prev.tcoords_, + mesh_3d_viz_props_prev.texture_); + } else { + VLOG(10) << "Visualize mesh with colored clusters."; + LOG_IF(ERROR, mesh_3d_viz_props_prev.colors_.rows > 0u) + << "The 3D mesh is being colored with semantic information, but" + " gflag visualize_semantic_mesh is set to false..."; + visualizeMesh3DWithColoredClusters( + planes_prev, + vertices_mesh_prev, + polygons_mesh_prev, + &output->widgets_, + FLAGS_visualize_mesh_with_colored_polygon_clusters, + input.timestamp_); + } + } + + if (FLAGS_visualize_point_cloud) { + visualizePoints3D(points_with_id_VIO_prev, + lmk_id_to_lmk_type_map_prev, + &output->widgets_); + } + + if (!FLAGS_visualize_load_mesh_filename.empty()) { + // TODO(Toni): remove static, never use static wo const/constexpr + static bool visualize_ply_mesh_once = true; + if (visualize_ply_mesh_once) { + visualizePlyMesh(FLAGS_visualize_load_mesh_filename.c_str(), + &output->widgets_); + visualize_ply_mesh_once = false; + } + } + + if (FLAGS_visualize_convex_hull) { + if (planes_prev.size() != 0) { + visualizeConvexHull(planes_prev.at(0).triangle_cluster_, + vertices_mesh_prev, + polygons_mesh_prev, + &output->widgets_); + } + } + + if (backend_type_ == BackendType::kStructuralRegularities && + FLAGS_visualize_plane_constraints) { + LandmarkIds lmk_ids_in_current_pp_factors; + for (const auto& g : input.backend_output_->graph_) { + const auto& ppf = + boost::dynamic_pointer_cast(g); + if (ppf) { + // We found a PointPlaneFactor. + // Get point key. + Key point_key = ppf->getPointKey(); + LandmarkId lmk_id = gtsam::Symbol(point_key).index(); + lmk_ids_in_current_pp_factors.push_back(lmk_id); + // Get point estimate. + gtsam::Point3 point; + // This call makes visualizer unable to perform this + // in parallel. But you can just copy the state_ + CHECK(getEstimateOfKey( + input.backend_output_->state_, point_key, &point)); + // Visualize. + const Key& ppf_plane_key = ppf->getPlaneKey(); + // not sure, we are having some w planes_prev + // others with planes... + for (const Plane& plane : input.mesher_output_->planes_) { + if (ppf_plane_key == plane.getPlaneSymbol().key()) { + gtsam::OrientedPlane3 current_plane_estimate; + CHECK(getEstimateOfKey( // This call makes visualizer + // unable to perform this in + // parallel. + input.backend_output_->state_, + ppf_plane_key, + ¤t_plane_estimate)); + // WARNING assumes the backend updates normal and distance + // of plane and that no one modifies it afterwards... + visualizePlaneConstraints( + plane.getPlaneSymbol().key(), + current_plane_estimate.normal().point3(), + current_plane_estimate.distance(), + lmk_id, + point, + &output->widgets_); + // Stop since there are not multiple planes for one + // ppf. + break; + } + } + } + } + + // Remove lines that are not representing a point plane factor + // in the current graph. + removeOldLines(lmk_ids_in_current_pp_factors); + } + + // Must go after visualize plane constraints. + if (FLAGS_visualize_planes || FLAGS_visualize_plane_constraints) { + for (const Plane& plane : input.mesher_output_->planes_) { + const gtsam::Symbol& plane_symbol = plane.getPlaneSymbol(); + const std::uint64_t& plane_index = plane_symbol.index(); + gtsam::OrientedPlane3 current_plane_estimate; + if (getEstimateOfKey( + input.backend_output_->state_, + plane_symbol.key(), + ¤t_plane_estimate)) { + const cv::Point3d& plane_normal_estimate = + UtilsOpenCV::unit3ToPoint3d(current_plane_estimate.normal()); + CHECK(plane.normal_ == plane_normal_estimate); + // We have the plane in the optimization. + // Visualize plane. + visualizePlane(plane_index, + plane_normal_estimate.x, + plane_normal_estimate.y, + plane_normal_estimate.z, + current_plane_estimate.distance(), + &output->widgets_, + FLAGS_visualize_plane_label, + plane.triangle_cluster_.cluster_id_); + } else { + // We could not find the plane in the optimization... + // Careful cause we might enter here because there are new + // segmented planes. + // Delete the plane. + LOG(ERROR) << "Remove plane viz for id:" << plane_index; + if (FLAGS_visualize_plane_constraints) { + removePlaneConstraintsViz(plane_index); + } + removePlane(plane_index); + } + } + + // Also remove planes that were deleted by the backend... + for (const Plane& plane : planes_prev) { + const gtsam::Symbol& plane_symbol = plane.getPlaneSymbol(); + const std::uint64_t& plane_index = plane_symbol.index(); + gtsam::OrientedPlane3 current_plane_estimate; + if (!getEstimateOfKey(input.backend_output_->state_, + plane_symbol.key(), + ¤t_plane_estimate)) { + // We could not find the plane in the optimization... + // Delete the plane. + if (FLAGS_visualize_plane_constraints) { + removePlaneConstraintsViz(plane_index); + } + removePlane(plane_index); + } + } + } + + planes_prev = input.mesher_output_->planes_; + vertices_mesh_prev = input.mesher_output_->vertices_mesh_; + polygons_mesh_prev = input.mesher_output_->polygons_mesh_; + points_with_id_VIO_prev = input.backend_output_->landmarks_with_id_map_; + lmk_id_to_lmk_type_map_prev = + input.backend_output_->lmk_id_to_lmk_type_map_; + LOG_IF(WARNING, mesh3d_viz_properties_callback_) + << "Coloring the mesh using semantic segmentation colors."; + mesh_3d_viz_props_prev = + // Call semantic mesh segmentation if someone registered a callback. + mesh3d_viz_properties_callback_ + ? mesh3d_viz_properties_callback_(left_stereo_keyframe.timestamp_, + left_stereo_keyframe.img_, + input.mesher_output_->mesh_2d_, + input.mesher_output_->mesh_3d_) + : (FLAGS_texturize_3d_mesh ? OpenCvVisualizer3D::texturizeMesh3D( + left_stereo_keyframe.timestamp_, + left_stereo_keyframe.img_, + input.mesher_output_->mesh_2d_, + input.mesher_output_->mesh_3d_) + : Mesh3DVizProperties()), + VLOG(10) << "Finished mesh visualization."; + + break; + } + + // Computes and visualizes a 3D point cloud with VIO points in current time + // horizon of the optimization. + case VisualizationType::kPointcloud: { + // Do not color the cloud, send empty lmk id to lmk type map + visualizePoints3D(input.backend_output_->landmarks_with_id_map_, + input.backend_output_->lmk_id_to_lmk_type_map_, + &output->widgets_); + break; + } + case VisualizationType::kNone: { + break; + } + } + + // Visualize trajectory. + VLOG(10) << "Starting trajectory visualization..."; + addPoseToTrajectory(input.backend_output_->W_State_Blkf_.pose_.compose( + input.frontend_output_->stereo_frame_lkf_.getBPoseCamLRect())); + visualizeTrajectory3D( + FLAGS_visualize_mesh_in_frustum ? mesh_2d_img : left_stereo_keyframe.img_, + &output->frustum_pose_, + &output->widgets_); + VLOG(10) << "Finished trajectory visualization."; + + return output; +} + +// Create a 2D mesh from 2D corners in an image +cv::Mat OpenCvVisualizer3D::visualizeMesh2D( + const std::vector& triangulation2D, + const cv::Mat& img, + const KeypointsCV& extra_keypoints) { + static const cv::Scalar kDelaunayColor(0u, 255u, 0u); + static const cv::Scalar kPointsColor(255u, 0u, 0u); + + // Duplicate image for annotation and visualization. + cv::Mat img_clone = img.clone(); + cv::cvtColor(img_clone, img_clone, cv::COLOR_GRAY2BGR); + const cv::Size& size = img_clone.size(); + cv::Rect rect(0, 0, size.width, size.height); + std::vector pt(3); + for (size_t i = 0; i < triangulation2D.size(); i++) { + const cv::Vec6f& t = triangulation2D[i]; + + // Visualize mesh vertices. + pt[0] = cv::Point(cvRound(t[0]), cvRound(t[1])); + pt[1] = cv::Point(cvRound(t[2]), cvRound(t[3])); + pt[2] = cv::Point(cvRound(t[4]), cvRound(t[5])); + + // Visualize mesh edges. + cv::line(img_clone, pt[0], pt[1], kDelaunayColor, 1, CV_AA, 0); + cv::line(img_clone, pt[1], pt[2], kDelaunayColor, 1, CV_AA, 0); + cv::line(img_clone, pt[2], pt[0], kDelaunayColor, 1, CV_AA, 0); + } + + // Visualize extra vertices. + for (const auto& keypoint : extra_keypoints) { + cv::circle(img_clone, keypoint, 2, kPointsColor, CV_FILLED, CV_AA, 0); + } + + return img_clone; +} + +// Visualize 2d mesh. +cv::Mat OpenCvVisualizer3D::visualizeMesh2DStereo( + const std::vector& triangulation_2D, + const Frame& ref_frame) { + static const cv::Scalar kDelaunayColor(0, 255, 0); // Green + static const cv::Scalar kMeshVertexColor(255, 0, 0); // Blue + static const cv::Scalar kInvalidKeypointsColor(0, 0, 255); // Red + + // Sanity check. + DCHECK(ref_frame.landmarks_.size() == ref_frame.keypoints_.size()) + << "Frame: wrong dimension for the landmarks."; + + // Duplicate image for annotation and visualization. + cv::Mat img_clone = ref_frame.img_.clone(); + cv::cvtColor(img_clone, img_clone, cv::COLOR_GRAY2BGR); + + // Visualize extra vertices. + for (size_t i = 0; i < ref_frame.keypoints_.size(); i++) { + // Only for valid keypoints, but possibly without a right pixel. + // Kpts that are both valid and have a right pixel are currently the ones + // passed to the mesh. + if (ref_frame.landmarks_[i] != -1) { + cv::circle(img_clone, + ref_frame.keypoints_[i], + 2, + kInvalidKeypointsColor, + CV_FILLED, + CV_AA, + 0); + } + } + + std::vector pt(3); + for (const cv::Vec6f& triangle : triangulation_2D) { + // Visualize mesh vertices. + pt[0] = cv::Point(cvRound(triangle[0]), cvRound(triangle[1])); + pt[1] = cv::Point(cvRound(triangle[2]), cvRound(triangle[3])); + pt[2] = cv::Point(cvRound(triangle[4]), cvRound(triangle[5])); + cv::circle(img_clone, pt[0], 2, kMeshVertexColor, CV_FILLED, CV_AA, 0); + cv::circle(img_clone, pt[1], 2, kMeshVertexColor, CV_FILLED, CV_AA, 0); + cv::circle(img_clone, pt[2], 2, kMeshVertexColor, CV_FILLED, CV_AA, 0); + + // Visualize mesh edges. + cv::line(img_clone, pt[0], pt[1], kDelaunayColor, 1, CV_AA, 0); + cv::line(img_clone, pt[1], pt[2], kDelaunayColor, 1, CV_AA, 0); + cv::line(img_clone, pt[2], pt[0], kDelaunayColor, 1, CV_AA, 0); + } + + return img_clone; +} + +// Visualize a 3D point cloud of unique 3D landmarks. +void OpenCvVisualizer3D::visualizePoints3D( + const PointsWithIdMap& points_with_id, + const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map, + WidgetsMap* widgets_map) { + CHECK(widgets_map); + bool color_the_cloud = false; + if (lmk_id_to_lmk_type_map.size() != 0) { + color_the_cloud = true; + CHECK_EQ(points_with_id.size(), lmk_id_to_lmk_type_map.size()); + } + + // Sanity check dimension. + if (points_with_id.size() == 0) { + // No points to visualize. + LOG(WARNING) << "No landmark information for Visualizer. " + "Not displaying 3D points."; + return; + } + + // Populate cloud structure with 3D points. + cv::Mat point_cloud(1, points_with_id.size(), CV_32FC3); + cv::Mat point_cloud_color( + 1, lmk_id_to_lmk_type_map.size(), CV_8UC3, cloud_color_); + cv::Point3f* data = point_cloud.ptr(); + size_t i = 0; + for (const std::pair& id_point : points_with_id) { + const gtsam::Point3& point_3d = id_point.second; + data[i].x = static_cast(point_3d.x()); + data[i].y = static_cast(point_3d.y()); + data[i].z = static_cast(point_3d.z()); + if (color_the_cloud) { + DCHECK(lmk_id_to_lmk_type_map.find(id_point.first) != + lmk_id_to_lmk_type_map.end()); + switch (lmk_id_to_lmk_type_map.at(id_point.first)) { + case LandmarkType::SMART: { + point_cloud_color.col(i) = cv::viz::Color::white(); + break; + } + case LandmarkType::PROJECTION: { + point_cloud_color.col(i) = cv::viz::Color::green(); + break; + } + default: { + point_cloud_color.col(i) = cv::viz::Color::white(); + break; + } + } + } + i++; + } + + // Create a cloud widget. + std::unique_ptr cloud_widget = + VIO::make_unique(point_cloud, cloud_color_); + if (color_the_cloud) { + *cloud_widget = cv::viz::WCloud(point_cloud, point_cloud_color); + } + cloud_widget->setRenderingProperty(cv::viz::POINT_SIZE, 6); + + (*widgets_map)["Point cloud"] = std::move(cloud_widget); +} + +// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. +void OpenCvVisualizer3D::visualizePlane(const PlaneId& plane_index, + const double& n_x, + const double& n_y, + const double& n_z, + const double& d, + WidgetsMap* widgets, + const bool& visualize_plane_label, + const int& cluster_id) { + CHECK_NOTNULL(widgets); + const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); + // Create a plane widget. + const cv::Vec3d normal(n_x, n_y, n_z); + const cv::Point3d center(d * n_x, d * n_y, d * n_z); + static const cv::Vec3d new_yaxis(0, 1, 0); + static const cv::Size2d size(1.0, 1.0); + + cv::viz::Color plane_color; + getColorById(cluster_id, &plane_color); + + if (visualize_plane_label) { + static double increase = 0.0; + const cv::Point3d text_position( + d * n_x, d * n_y, d * n_z + std::fmod(increase, 1)); + increase += 0.1; + (*widgets)[plane_id_for_viz + "_label"] = + VIO::make_unique( + plane_id_for_viz, text_position, 0.07, true); + } + + (*widgets)[plane_id_for_viz] = VIO::make_unique( + center, normal, new_yaxis, size, plane_color); + is_plane_id_in_window_[plane_index] = true; +} + +// Draw a line in opencv. +void OpenCvVisualizer3D::drawLine(const std::string& line_id, + const double& from_x, + const double& from_y, + const double& from_z, + const double& to_x, + const double& to_y, + const double& to_z, + WidgetsMap* widgets) { + cv::Point3d pt1(from_x, from_y, from_z); + cv::Point3d pt2(to_x, to_y, to_z); + drawLine(line_id, pt1, pt2, widgets); +} + +void OpenCvVisualizer3D::drawLine(const std::string& line_id, + const cv::Point3d& pt1, + const cv::Point3d& pt2, + WidgetsMap* widgets) { + CHECK_NOTNULL(widgets); + (*widgets)[line_id] = VIO::make_unique(pt1, pt2); +} + +// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. +void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets) { + cv::Mat colors(0, 1, CV_8UC3, cv::viz::Color::gray()); // Do not color mesh. + visualizeMesh3D(map_points_3d, colors, polygons_mesh, widgets); +} + +// Visualize a 3D point cloud of unique 3D landmarks with its connectivity, +// and provide color for each polygon. +void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, + const cv::Mat& colors, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets, + const cv::Mat& tcoords, + const cv::Mat& texture) { + CHECK_NOTNULL(widgets); + // Check data + bool color_mesh = false; + if (colors.rows != 0) { + CHECK_EQ(map_points_3d.rows, colors.rows) + << "Map points and Colors should have same number of rows. One" + " color per map point."; + LOG(ERROR) << "Coloring mesh!"; + color_mesh = true; + } + + if (tcoords.rows != 0) { + CHECK_EQ(map_points_3d.rows, tcoords.rows) + << "Map points and tcoords should have same number of rows. One" + "tcoord per map point."; + CHECK(!texture.empty()); + } + + // No points/mesh to visualize. + if (map_points_3d.rows == 0 || polygons_mesh.rows == 0) { + return; + } + + cv::viz::Mesh cv_mesh; + cv_mesh.cloud = map_points_3d.t(); + cv_mesh.polygons = polygons_mesh; + cv_mesh.colors = color_mesh ? colors.t() : cv::Mat(); + cv_mesh.tcoords = tcoords.t(); + cv_mesh.texture = texture; + + // Plot mesh. + (*widgets)["Mesh"] = VIO::make_unique(cv_mesh); +} + +// Visualize a PLY from filename (absolute path). +void OpenCvVisualizer3D::visualizePlyMesh(const std::string& filename, + WidgetsMap* widgets) { + CHECK_NOTNULL(widgets); + LOG(INFO) << "Showing ground truth mesh: " << filename; + // The ply file must have in the header a "element vertex" and + // a "element face" primitives, otherwise you'll get a + // "Cannot read geometry" error. + cv::viz::Mesh mesh(cv::viz::Mesh::load(filename)); + if (mesh.polygons.size[1] == 0) { + LOG(WARNING) << "No polygons available for mesh, showing point cloud only."; + // If there are no polygons, convert to point cloud, otw there will be + // nothing displayed... + std::unique_ptr cloud = + VIO::make_unique(mesh.cloud, cv::viz::Color::lime()); + cloud->setRenderingProperty(cv::viz::REPRESENTATION, + cv::viz::REPRESENTATION_POINTS); + cloud->setRenderingProperty(cv::viz::POINT_SIZE, 2); + cloud->setRenderingProperty(cv::viz::OPACITY, 0.1); + + // Plot point cloud. + (*widgets)["Mesh from ply"] = std::move(cloud); + } else { + // Plot mesh. + (*widgets)["Mesh from ply"] = VIO::make_unique(mesh); + } +} + +// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. +/// Each triangle is colored depending on the cluster it is in, or gray if it +/// is in no cluster. +/// [in] clusters: a set of triangle clusters. The ids of the triangles must +/// match the order in polygons_mesh. +/// [in] map_points_3d: set of 3d points in the mesh, format is n rows, with +/// three columns (x, y, z). +/// [in] polygons_mesh: mesh faces, format is n rows, 1 column, +/// with [n id_a id_b id_c, ..., n /id_x id_y id_z], where n = polygon size +/// n=3 for triangles. +/// [in] color_mesh whether to color the mesh or not +/// [in] timestamp to store the timestamp of the mesh when logging the mesh. +void OpenCvVisualizer3D::visualizeMesh3DWithColoredClusters( + const std::vector& planes, + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets, + const bool visualize_mesh_with_colored_polygon_clusters, + const Timestamp& timestamp) { + if (visualize_mesh_with_colored_polygon_clusters) { + // Color the mesh. + cv::Mat colors; + colorMeshByClusters(planes, map_points_3d, polygons_mesh, &colors); + // Visualize the colored mesh. + visualizeMesh3D(map_points_3d, colors, polygons_mesh, widgets); + // Log the mesh. + if (FLAGS_log_mesh) { + logMesh(map_points_3d, + colors, + polygons_mesh, + timestamp, + FLAGS_log_accumulated_mesh); + } + } else { + // Visualize the mesh with same colour. + visualizeMesh3D(map_points_3d, polygons_mesh, widgets); + } +} + +// Visualize convex hull in 2D for set of points in triangle cluster, +// projected along the normal of the cluster. +void OpenCvVisualizer3D::visualizeConvexHull(const TriangleCluster& cluster, + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + WidgetsMap* widgets_map) { + CHECK_NOTNULL(widgets_map); + + // Create a new coord system, which has as z the normal. + const cv::Point3f& normal = cluster.cluster_direction_; + + // Find first axis of the coord system. + // Pick random x and y + static constexpr float random_x = 0.1; + static constexpr float random_y = 0.0; + // Find z, such that dot product with the normal is 0. + float z = -(normal.x * random_x + normal.y * random_y) / normal.z; + // Create new first axis: + cv::Point3f x_axis(random_x, random_y, z); + // Normalize new first axis: + x_axis /= cv::norm(x_axis); + // Create new second axis, by cross product normal to x_axis; + cv::Point3f y_axis = normal.cross(x_axis); + // Normalize just in case? + y_axis /= cv::norm(y_axis); + // Construct new cartesian coord system: + cv::Mat new_coordinates(3, 3, CV_32FC1); + new_coordinates.at(0, 0) = x_axis.x; + new_coordinates.at(0, 1) = x_axis.y; + new_coordinates.at(0, 2) = x_axis.z; + new_coordinates.at(1, 0) = y_axis.x; + new_coordinates.at(1, 1) = y_axis.y; + new_coordinates.at(1, 2) = y_axis.z; + new_coordinates.at(2, 0) = normal.x; + new_coordinates.at(2, 1) = normal.y; + new_coordinates.at(2, 2) = normal.z; + + std::vector points_2d; + std::vector z_s; + for (const size_t& triangle_id : cluster.triangle_ids_) { + const size_t& triangle_idx = std::round(triangle_id * 4); + LOG_IF(FATAL, triangle_idx + 3 >= polygons_mesh.rows) + << "Visualizer3D: an id in triangle_ids_ is too large."; + const int32_t& idx_1 = polygons_mesh.at(triangle_idx + 1); + const int32_t& idx_2 = polygons_mesh.at(triangle_idx + 2); + const int32_t& idx_3 = polygons_mesh.at(triangle_idx + 3); + + // Project points to new coord system + const cv::Point3f& new_map_point_1 = // new_coordinates * + // map_points_3d.row(idx_1).t(); + map_points_3d.at(idx_1); + const cv::Point3f& new_map_point_2 = // new_coordinates * + // map_points_3d.row(idx_2).t(); + map_points_3d.at(idx_2); + const cv::Point3f& new_map_point_3 = // new_coordinates * + // map_points_3d.row(idx_3).t(); + map_points_3d.at(idx_3); + + // Keep only 1st and 2nd component, aka the projection of the point on the + // plane. + points_2d.push_back(cv::Point2f(new_map_point_1.x, new_map_point_1.y)); + z_s.push_back(new_map_point_1.z); + points_2d.push_back(cv::Point2f(new_map_point_2.x, new_map_point_2.y)); + z_s.push_back(new_map_point_2.z); + points_2d.push_back(cv::Point2f(new_map_point_3.x, new_map_point_3.y)); + z_s.push_back(new_map_point_3.z); + } + + // Create convex hull. + if (points_2d.size() != 0) { + std::vector hull_idx; + convexHull(cv::Mat(points_2d), hull_idx, false); + + // Add the z component. + std::vector hull_3d; + for (const int& idx : hull_idx) { + hull_3d.push_back( + cv::Point3f(points_2d.at(idx).x, points_2d.at(idx).y, z_s.at(idx))); + } + + static constexpr bool visualize_hull_as_polyline = false; + if (visualize_hull_as_polyline) { + // Close the hull. + CHECK_NE(hull_idx.size(), 0); + hull_3d.push_back(cv::Point3f(points_2d.at(hull_idx.at(0)).x, + points_2d.at(hull_idx.at(0)).y, + z_s.at(hull_idx.at(0)))); + // Visualize convex hull. + (*widgets_map)["Convex Hull"] = + VIO::make_unique(hull_3d); + } else { + // Visualize convex hull as a mesh of one polygon with multiple points. + if (hull_3d.size() > 2) { + cv::Mat polygon_hull = cv::Mat(4 * (hull_3d.size() - 2), 1, CV_32SC1); + size_t i = 1; + for (size_t k = 0; k + 3 < polygon_hull.rows; k += 4) { + polygon_hull.row(k) = 3; + polygon_hull.row(k + 1) = 0; + polygon_hull.row(k + 2) = static_cast(i); + polygon_hull.row(k + 3) = static_cast(i) + 1; + i++; + } + cv::viz::Color mesh_color; + getColorById(cluster.cluster_id_, &mesh_color); + cv::Mat normals = cv::Mat(0, 1, CV_32FC3); + for (size_t k = 0; k < hull_3d.size(); k++) { + normals.push_back(normal); + } + cv::Mat colors(hull_3d.size(), 1, CV_8UC3, mesh_color); + (*widgets_map)["Convex Hull"] = VIO::make_unique( + hull_3d, polygon_hull, colors.t(), normals.t()); + } + } + } +} + +// Visualize trajectory. Adds an image to the frustum if cv::Mat is not empty. +void OpenCvVisualizer3D::visualizeTrajectory3D(const cv::Mat& frustum_image, + cv::Affine3d* frustum_pose, + WidgetsMap* widgets_map) { + CHECK_NOTNULL(frustum_pose); + CHECK_NOTNULL(widgets_map); + + if (trajectory_poses_3d_.size() == 0) { // no points to visualize + return; + } + + // Show current camera pose. + static const cv::Matx33d K(458, 0.0, 360, 0.0, 458, 240, 0.0, 0.0, 1.0); + std::unique_ptr cam_widget_ptr = nullptr; + if (frustum_image.empty()) { + cam_widget_ptr = VIO::make_unique( + K, 1.0, cv::viz::Color::white()); + } else { + cam_widget_ptr = VIO::make_unique( + K, frustum_image, 1.0, cv::viz::Color::white()); + } + CHECK(cam_widget_ptr); + // Normally you would use Widget3D.setPose(), or updatePose(), but it does not + // seem to work, so we send the pose to the display module, which then calls + // the window_.setWidgetPose()... + *frustum_pose = trajectory_poses_3d_.back(); + (*widgets_map)["Camera Pose with Frustum"] = std::move(cam_widget_ptr); + + // Option A: This does not work very well. + // window_data_.window_.resetCameraViewpoint("Camera Pose with Frustum"); + // Viewer is our viewpoint, camera the pose estimate (frustum). + static constexpr bool follow_camera = false; + if (follow_camera) { + cv::Affine3f camera_in_world_coord = trajectory_poses_3d_.back(); + // Option B: specify viewer wrt camera. Works, but motion is non-smooth. + // cv::Affine3f viewer_in_camera_coord (Vec3f( + // -0.3422019, -0.3422019, 1.5435732), + // Vec3f(3.0, 0.0, -4.5)); + // cv::Affine3f viewer_in_world_coord = + // viewer_in_camera_coord.concatenate(camera_in_world_coord); + + // Option C: use "look-at" camera parametrization. + // Works, but motion is non-smooth as well. + cv::Vec3d cam_pos(-6.0, 0.0, 6.0); + cv::Vec3d cam_focal_point(camera_in_world_coord.translation()); + cv::Vec3d cam_y_dir(0.0, 0.0, -1.0); + cv::Affine3f cam_pose = + cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir); + // window_data_.window_.setViewerPose(cam_pose); + // window_data_.window_.setViewerPose(viewer_in_world_coord); + } + + // Create a Trajectory frustums widget. + std::vector trajectory_frustums( + trajectory_poses_3d_.end() - + std::min(trajectory_poses_3d_.size(), size_t(10u)), + trajectory_poses_3d_.end()); + (*widgets_map)["Trajectory Frustums"] = + VIO::make_unique( + trajectory_frustums, K, 0.2, cv::viz::Color::red()); + + // Create a Trajectory widget. (argument can be PATH, FRAMES, BOTH). + std::vector trajectory(trajectory_poses_3d_.begin(), + trajectory_poses_3d_.end()); + (*widgets_map)["Trajectory"] = VIO::make_unique( + trajectory, cv::viz::WTrajectory::PATH, 1.0, cv::viz::Color::red()); +} + +// Remove widget. True if successful, false if not. +bool OpenCvVisualizer3D::removeWidget(const std::string& widget_id) { + try { + // window_data_.window_.removeWidget(widget_id); + return true; + } catch (const cv::Exception& e) { + VLOG(20) << e.what(); + LOG(ERROR) << "Widget with id: " << widget_id.c_str() + << " is not in window."; + } catch (...) { + LOG(ERROR) << "Unrecognized exception when using " + "window_data_.window_.removeWidget() " + << "with widget with id: " << widget_id.c_str(); + } + return false; +} + +// Visualize line widgets from plane to lmks. +// Point key is required to avoid duplicated lines! +void OpenCvVisualizer3D::visualizePlaneConstraints(const PlaneId& plane_id, + const gtsam::Point3& normal, + const double& distance, + const LandmarkId& lmk_id, + const gtsam::Point3& point, + WidgetsMap* widgets) { + PlaneIdMap::iterator plane_id_it = plane_id_map_.find(plane_id); + LmkIdToLineIdMap* lmk_id_to_line_id_map_ptr = nullptr; + LineNr* line_nr_ptr = nullptr; + if (plane_id_it != plane_id_map_.end()) { + // We already have this plane id stored. + lmk_id_to_line_id_map_ptr = &(plane_id_it->second); + + // Ensure we also have the line nr stored. + const auto& line_nr_it = plane_to_line_nr_map_.find(plane_id); + CHECK(line_nr_it != plane_to_line_nr_map_.end()); + line_nr_ptr = &(line_nr_it->second); + } else { + // We have not this plane id stored. + // Create it by calling default ctor. + lmk_id_to_line_id_map_ptr = &(plane_id_map_[plane_id]); + plane_id_it = plane_id_map_.find(plane_id); + DCHECK(plane_id_it != plane_id_map_.end()); + + // Also start line nr to 0. + plane_to_line_nr_map_[plane_id] = 0; + DCHECK(plane_to_line_nr_map_.find(plane_id) != plane_to_line_nr_map_.end()); + line_nr_ptr = &(plane_to_line_nr_map_[plane_id]); + } + CHECK_NOTNULL(lmk_id_to_line_id_map_ptr); + CHECK_NOTNULL(line_nr_ptr); + + // TODO should use map from line_id_to_lmk_id as well, + // to remove the line_ids which are not having a lmk_id... + const auto& lmk_id_to_line_id = lmk_id_to_line_id_map_ptr->find(lmk_id); + if (lmk_id_to_line_id == lmk_id_to_line_id_map_ptr->end()) { + // We have never drawn this line. + // Store line nr (as line id). + (*lmk_id_to_line_id_map_ptr)[lmk_id] = *line_nr_ptr; + std::string line_id = "Line " + + std::to_string(static_cast(plane_id_it->first)) + + std::to_string(static_cast(*line_nr_ptr)); + // Draw it. + drawLineFromPlaneToPoint(line_id, + normal.x(), + normal.y(), + normal.z(), + distance, + point.x(), + point.y(), + point.z(), + widgets); + // Augment line_nr for next line_id. + (*line_nr_ptr)++; + } else { + // We have drawn this line before. + // Update line. + std::string line_id = + "Line " + std::to_string(static_cast(plane_id_it->first)) + + std::to_string(static_cast(lmk_id_to_line_id->second)); + updateLineFromPlaneToPoint(line_id, + normal.x(), + normal.y(), + normal.z(), + distance, + point.x(), + point.y(), + point.z(), + widgets); + } +} + +// Remove line widgets from plane to lmks, for lines that are not pointing +// to any lmk_id in lmk_ids. +void OpenCvVisualizer3D::removeOldLines(const LandmarkIds& lmk_ids) { + for (PlaneIdMap::value_type& plane_id_pair : plane_id_map_) { + LmkIdToLineIdMap& lmk_id_to_line_id_map = plane_id_pair.second; + for (LmkIdToLineIdMap::iterator lmk_id_to_line_id_it = + lmk_id_to_line_id_map.begin(); + lmk_id_to_line_id_it != lmk_id_to_line_id_map.end();) { + if (std::find(lmk_ids.begin(), + lmk_ids.end(), + lmk_id_to_line_id_it->first) == lmk_ids.end()) { + // We did not find the lmk_id of the current line in the list + // of lmk_ids... + // Delete the corresponding line. + std::string line_id = + "Line " + std::to_string(static_cast(plane_id_pair.first)) + + std::to_string(static_cast(lmk_id_to_line_id_it->second)); + removeWidget(line_id); + // Delete the corresponding entry in the map from lmk id to line id. + lmk_id_to_line_id_it = + lmk_id_to_line_id_map.erase(lmk_id_to_line_id_it); + } else { + lmk_id_to_line_id_it++; + } + } + } +} + +// Remove line widgets from plane to lmks. +void OpenCvVisualizer3D::removePlaneConstraintsViz(const PlaneId& plane_id) { + PlaneIdMap::iterator plane_id_it = plane_id_map_.find(plane_id); + if (plane_id_it != plane_id_map_.end()) { + VLOG(0) << "Removing line constraints for plane with id: " << plane_id; + for (const auto& lmk_id_to_line_id : plane_id_it->second) { + std::string line_id = + "Line " + std::to_string(static_cast(plane_id_it->first)) + + std::to_string(static_cast(lmk_id_to_line_id.second)); + removeWidget(line_id); + } + // Delete the corresponding entry in the map for this plane. + plane_id_map_.erase(plane_id_it); + // Same for the map holding the line nr. + auto line_nr_it = plane_to_line_nr_map_.find(plane_id); + CHECK(line_nr_it != plane_to_line_nr_map_.end()); + plane_to_line_nr_map_.erase(line_nr_it); + } else { + // Careful if we did not find, might be because it is a newly segmented + // plane. + LOG(WARNING) << "Could not find plane with id: " << plane_id + << " from plane_id_map_..."; + } +} + +// Remove plane widget. +void OpenCvVisualizer3D::removePlane(const PlaneId& plane_index, + const bool& remove_plane_label) { + const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); + if (is_plane_id_in_window_.find(plane_index) != + is_plane_id_in_window_.end() && + is_plane_id_in_window_[plane_index]) { + if (removeWidget(plane_id_for_viz)) { + if (remove_plane_label) { + if (!removeWidget(plane_id_for_viz + "_label")) { + LOG(WARNING) << "Did you disable labels of planes?, then also" + "disable label removal. Otherwise, did you change " + "the id of the label? then change it here as well."; + } + } + is_plane_id_in_window_[plane_index] = false; + } else { + is_plane_id_in_window_[plane_index] = true; + } + } +} + +// Add pose to the previous trajectory. +void OpenCvVisualizer3D::addPoseToTrajectory( + const gtsam::Pose3& current_pose_gtsam) { + trajectory_poses_3d_.push_back( + UtilsOpenCV::gtsamPose3ToCvAffine3d(current_pose_gtsam)); + if (FLAGS_displayed_trajectory_length > 0) { + while (trajectory_poses_3d_.size() > FLAGS_displayed_trajectory_length) { + trajectory_poses_3d_.pop_front(); + } + } +} + +Mesh3DVizProperties OpenCvVisualizer3D::texturizeMesh3D( + const Timestamp& image_timestamp, + const cv::Mat& texture_image, + const Mesh2D& mesh_2d, + const Mesh3D& mesh_3d) { + // Dummy checks for valid data. + CHECK(!texture_image.empty()); + CHECK_GE(mesh_2d.getNumberOfUniqueVertices(), 0); + CHECK_GE(mesh_3d.getNumberOfUniqueVertices(), 0); + + // Let us fill the mesh 3d viz properties structure. + Mesh3DVizProperties mesh_3d_viz_props; + + // Color all vertices in red. Each polygon will be colored according + // to a mix of the three vertices colors I think... + mesh_3d_viz_props.colors_ = cv::Mat( + mesh_3d.getNumberOfUniqueVertices(), 1, CV_8UC3, cv::viz::Color::red()); + + // Add texture to the mesh using the given image. + // README: tcoords specify the texture coordinates of the 3d mesh wrt 2d + // image. As a small hack, we not only use the left_image as texture but we + // also horizontally concatenate a white image so we can set a white texture + // to those 3d mesh faces which should not have a texture. Below we init all + // tcoords to 0.99 (1.0) gives a weird texture... Meaning that all faces + // start with a default white texture, and then we change that texture to + // the right texture for each 2d triangle that has a corresponding 3d face. + Mesh2D::Polygon polygon; + std::vector tcoords(mesh_3d.getNumberOfUniqueVertices(), + cv::Vec2d(0.9, 0.9)); + for (size_t i = 0; i < mesh_2d.getNumberOfPolygons(); i++) { + CHECK(mesh_2d.getPolygon(i, &polygon)) << "Could not retrieve 2d polygon."; + + const LandmarkId& lmk0 = polygon.at(0).getLmkId(); + const LandmarkId& lmk1 = polygon.at(1).getLmkId(); + const LandmarkId& lmk2 = polygon.at(2).getLmkId(); + + // Returns indices of points in the 3D mesh corresponding to the vertices + // in the 2D mesh. + int p0_id, p1_id, p2_id; + if (mesh_3d.getVertex(lmk0, nullptr, &p0_id) && + mesh_3d.getVertex(lmk1, nullptr, &p1_id) && + mesh_3d.getVertex(lmk2, nullptr, &p2_id)) { + // Sanity check. + CHECK_LE(p0_id, tcoords.size()); + CHECK_LE(p1_id, tcoords.size()); + CHECK_LE(p2_id, tcoords.size()); + + // Get pixel coordinates of the vertices of the 2D mesh. + const auto& px0 = polygon.at(0).getVertexPosition(); + const auto& px1 = polygon.at(1).getVertexPosition(); + const auto& px2 = polygon.at(2).getVertexPosition(); + + // These pixels correspond to the tcoords in the image for the 3d mesh + // vertices. + VLOG(100) << "Pixel: with id: " << p0_id << ", x: " << px0.x + << ", y: " << px0.y; + // We divide by 2.0 to account for fake default texture padded to the + // right of the texture_image. + tcoords.at(p0_id) = cv::Vec2d(px0.x / texture_image.cols / 2.0, + px0.y / texture_image.rows); + tcoords.at(p1_id) = cv::Vec2d(px1.x / texture_image.cols / 2.0, + px1.y / texture_image.rows); + tcoords.at(p2_id) = cv::Vec2d(px2.x / texture_image.cols / 2.0, + px2.y / texture_image.rows); + mesh_3d_viz_props.colors_.row(p0_id) = cv::viz::Color::white(); + mesh_3d_viz_props.colors_.row(p1_id) = cv::viz::Color::white(); + mesh_3d_viz_props.colors_.row(p2_id) = cv::viz::Color::white(); + } else { + // If we did not find a corresponding 3D triangle for the 2D triangle + // leave tcoords and colors to the default values. + LOG_EVERY_N(ERROR, 1000) << "Polygon in 2d mesh did not have a " + "corresponding polygon in 3d mesh!"; + } + } + + // Add a column with a fixed color at the end so that we can specify an + // "invalid" or "default" texture for those points which we do not want to + // texturize. + static cv::Mat default_texture(texture_image.rows, + texture_image.cols, + texture_image.type(), + cv::viz::Color::white()); + CHECK_EQ(texture_image.dims, default_texture.dims); + CHECK_EQ(texture_image.rows, default_texture.rows); + CHECK_EQ(texture_image.type(), default_texture.type()); + + cv::Mat texture; + // Padding actual texture with default texture, a bit hacky, but works. + cv::hconcat(texture_image, default_texture, texture); + mesh_3d_viz_props.texture_ = texture; + + mesh_3d_viz_props.tcoords_ = cv::Mat(tcoords, true).reshape(2); + CHECK_EQ(mesh_3d_viz_props.tcoords_.size().height, + mesh_3d.getNumberOfUniqueVertices()); + + return mesh_3d_viz_props; +} + +// Log mesh to ply file. +void OpenCvVisualizer3D::logMesh(const cv::Mat& map_points_3d, + const cv::Mat& colors, + const cv::Mat& polygons_mesh, + const Timestamp& timestamp, + bool log_accumulated_mesh) { + /// Log the mesh in a ply file. + static Timestamp last_timestamp = timestamp; + static const Timestamp first_timestamp = timestamp; + if ((timestamp - last_timestamp) > + 6500000000) { // Log every 6 seconds approx. (a little bit more than + // time-horizon) + LOG(WARNING) << "Logging mesh every (ns) = " << timestamp - last_timestamp; + CHECK(logger_); + logger_->logMesh( + map_points_3d, colors, polygons_mesh, timestamp, log_accumulated_mesh); + last_timestamp = timestamp; + } +} + +// Input the mesh points and triangle clusters, and +// output colors matrix for mesh visualizer. +// This will color the point with the color of the last plane having it. +void OpenCvVisualizer3D::colorMeshByClusters(const std::vector& planes, + const cv::Mat& map_points_3d, + const cv::Mat& polygons_mesh, + cv::Mat* colors) const { + CHECK_NOTNULL(colors); + *colors = cv::Mat(map_points_3d.rows, 1, CV_8UC3, cv::viz::Color::gray()); + + // The code below assumes triangles as polygons. + static constexpr bool log_landmarks = false; + for (const Plane& plane : planes) { + const TriangleCluster& cluster = plane.triangle_cluster_; + // Decide color for cluster. + cv::viz::Color cluster_color = cv::viz::Color::gray(); + getColorById(cluster.cluster_id_, &cluster_color); + + for (const size_t& triangle_id : cluster.triangle_ids_) { + size_t triangle_idx = std::round(triangle_id * 4); + DCHECK_LE(triangle_idx + 3, polygons_mesh.rows) + << "Visualizer3D: an id in triangle_ids_ is too large."; + const int32_t& idx_1 = polygons_mesh.at(triangle_idx + 1); + const int32_t& idx_2 = polygons_mesh.at(triangle_idx + 2); + const int32_t& idx_3 = polygons_mesh.at(triangle_idx + 3); + // Overrides potential previous color. + colors->row(idx_1) = cluster_color; + colors->row(idx_2) = cluster_color; + colors->row(idx_3) = cluster_color; + } + } +} + +// Decide color of the cluster depending on its id. +void OpenCvVisualizer3D::getColorById(const size_t& id, + cv::viz::Color* color) const { + CHECK_NOTNULL(color); + switch (id) { + case 0: { + *color = cv::viz::Color::red(); + break; + } + case 1: { + *color = cv::viz::Color::green(); + break; + } + case 2: { + *color = cv::viz::Color::blue(); + break; + } + default: { + *color = cv::viz::Color::gray(); + break; + } + } +} + +// Draw a line from lmk to plane center. +void OpenCvVisualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, + const double& plane_n_x, + const double& plane_n_y, + const double& plane_n_z, + const double& plane_d, + const double& point_x, + const double& point_y, + const double& point_z, + WidgetsMap* widgets) { + const cv::Point3d center( + plane_d * plane_n_x, plane_d * plane_n_y, plane_d * plane_n_z); + const cv::Point3d point(point_x, point_y, point_z); + drawLine(line_id, center, point, widgets); +} + +// Update line from lmk to plane center. +void OpenCvVisualizer3D::updateLineFromPlaneToPoint(const std::string& line_id, + const double& plane_n_x, + const double& plane_n_y, + const double& plane_n_z, + const double& plane_d, + const double& point_x, + const double& point_y, + const double& point_z, + WidgetsMap* widgets) { + removeWidget(line_id); + drawLineFromPlaneToPoint(line_id, + plane_n_x, + plane_n_y, + plane_n_z, + plane_d, + point_x, + point_y, + point_z, + widgets); +} + +} // namespace VIO diff --git a/src/visualizer/Visualizer3D.cpp b/src/visualizer/Visualizer3D.cpp index a216cb223..8f1fb6780 100644 --- a/src/visualizer/Visualizer3D.cpp +++ b/src/visualizer/Visualizer3D.cpp @@ -14,1278 +14,9 @@ #include "kimera-vio/visualizer/Visualizer3D.h" -#include // for min -#include // for shared_ptr<> -#include // for string -#include // for unordered_map<> -#include // for pair<> -#include // for vector<> - -#include - -#include - -#include "kimera-vio/backend/VioBackEnd-definitions.h" -#include "kimera-vio/utils/FilesystemUtils.h" -#include "kimera-vio/utils/Statistics.h" -#include "kimera-vio/utils/Timer.h" -#include "kimera-vio/utils/UtilsGTSAM.h" -#include "kimera-vio/utils/UtilsOpenCV.h" - -#include "kimera-vio/factors/PointPlaneFactor.h" // For visualization of constraints. - -// TODO(Toni): remove visualizer gflags! There are far too many, use a -// yaml params class (aka inherit from PipelineParams. -DEFINE_bool(visualize_mesh, false, "Enable 3D mesh visualization."); - -DEFINE_bool(visualize_mesh_2d, false, "Visualize mesh 2D."); - -DEFINE_bool(visualize_semantic_mesh, - false, - "Color the 3d mesh according to their semantic labels."); -DEFINE_bool(visualize_mesh_with_colored_polygon_clusters, - false, - "Color the polygon clusters according to their cluster id."); -DEFINE_bool(visualize_point_cloud, true, "Enable point cloud visualization."); -DEFINE_bool(visualize_convex_hull, false, "Enable convex hull visualization."); -DEFINE_bool(visualize_plane_constraints, - false, - "Enable plane constraints" - " visualization."); -DEFINE_bool(visualize_planes, false, "Enable plane visualization."); -DEFINE_bool(visualize_plane_label, false, "Enable plane label visualization."); -DEFINE_bool(visualize_mesh_in_frustum, - false, - "Enable mesh visualization in " - "camera frustum."); -DEFINE_string( - visualize_load_mesh_filename, - "", - "Load a mesh in the visualization, i.e. to visualize ground-truth " - "point cloud from Euroc's Vicon dataset."); - -// 3D Mesh related flags. -DEFINE_bool(texturize_3d_mesh, - false, - "Whether you want to add texture to the 3d" - "mesh. The texture is taken from the image" - " frame."); -DEFINE_bool(log_mesh, false, "Log the mesh at time horizon."); -DEFINE_bool(log_accumulated_mesh, false, "Accumulate the mesh when logging."); - -DEFINE_int32(displayed_trajectory_length, - 50, - "Set length of plotted trajectory." - "If -1 then all the trajectory is plotted."); - namespace VIO { Visualizer3D::Visualizer3D(const VisualizationType& viz_type) : visualization_type_(viz_type) {} -/* -------------------------------------------------------------------------- */ -OpenCvVisualizer3D::OpenCvVisualizer3D(const VisualizationType& viz_type, - const BackendType& backend_type) - : Visualizer3D(viz_type), backend_type_(backend_type), logger_(nullptr) { - if (FLAGS_log_mesh) { - logger_ = VIO::make_unique(); - } -} - -/* -------------------------------------------------------------------------- */ -// Returns true if visualization is ready, false otherwise. -// TODO(Toni): Put all flags inside spinOnce into Visualizer3DParams! -VisualizerOutput::UniquePtr OpenCvVisualizer3D::spinOnce( - const VisualizerInput& input) { - DCHECK(input.frontend_output_); - DCHECK(input.backend_output_); - - VisualizerOutput::UniquePtr output = VIO::make_unique(); - - // Ensure we have mesher output if the user requested mesh visualization - // otherwise, switch to pointcloud visualization. - if (visualization_type_ == VisualizationType::kMesh2dTo3dSparse && - !input.mesher_output_) { - LOG(ERROR) << "Mesh visualization requested, but no " - "mesher output available. Switching to Pointcloud" - "visualization only."; - visualization_type_ = VisualizationType::kPointcloud; - } - output->visualization_type_ = visualization_type_; - - cv::Mat mesh_2d_img; // Only for visualization. - const Frame& left_stereo_keyframe = - input.frontend_output_->stereo_frame_lkf_.getLeftFrame(); - switch (visualization_type_) { - // Computes and visualizes 3D mesh from 2D triangulation. - // vertices: all leftframe kps with right-VALID (3D), lmkId != -1 and - // inside the image triangles: all the ones with edges inside images as - // produced by cv::subdiv, which have uniform gradient (updateMesh3D also - // filters out geometrically) Sparsity comes from filtering out triangles - // corresponding to non planar obstacles which are assumed to have - // non-uniform gradient. - case VisualizationType::kMesh2dTo3dSparse: { - CHECK(input.mesher_output_); - // Visualize 2d mesh. - if (FLAGS_visualize_mesh_2d || FLAGS_visualize_mesh_in_frustum) { - const ImageToDisplay& mesh_display = ImageToDisplay( - "Mesh 2D", - visualizeMesh2DStereo(input.mesher_output_->mesh_2d_for_viz_, - left_stereo_keyframe)); - if (FLAGS_visualize_mesh_2d) { - output->images_to_display_.push_back(mesh_display); - } - if (FLAGS_visualize_mesh_in_frustum) { - mesh_2d_img = mesh_display.image_; - } - } - - // 3D mesh visualization - VLOG(10) << "Starting 3D mesh visualization..."; - - static std::vector planes_prev; - static PointsWithIdMap points_with_id_VIO_prev; - static LmkIdToLmkTypeMap lmk_id_to_lmk_type_map_prev; - static cv::Mat vertices_mesh_prev; - static cv::Mat polygons_mesh_prev; - static Mesh3DVizProperties mesh_3d_viz_props_prev; - - if (FLAGS_visualize_mesh) { - VLOG(10) << "Visualize mesh."; - if (FLAGS_visualize_semantic_mesh) { - VLOG(10) << "Visualize Semantic mesh."; - LOG_IF(WARNING, FLAGS_visualize_mesh_with_colored_polygon_clusters) - << "Both gflags visualize_semantic_mesh and " - "visualize_mesh_with_colored_polygon_cluster are set to True," - " but visualization of the semantic mesh has priority over " - "visualization of the polygon clusters."; - visualizeMesh3D(vertices_mesh_prev, - mesh_3d_viz_props_prev.colors_, - polygons_mesh_prev, - &output->widgets_, - mesh_3d_viz_props_prev.tcoords_, - mesh_3d_viz_props_prev.texture_); - } else { - VLOG(10) << "Visualize mesh with colored clusters."; - LOG_IF(ERROR, mesh_3d_viz_props_prev.colors_.rows > 0u) - << "The 3D mesh is being colored with semantic information, but" - " gflag visualize_semantic_mesh is set to false..."; - visualizeMesh3DWithColoredClusters( - planes_prev, - vertices_mesh_prev, - polygons_mesh_prev, - &output->widgets_, - FLAGS_visualize_mesh_with_colored_polygon_clusters, - input.timestamp_); - } - } - - if (FLAGS_visualize_point_cloud) { - visualizePoints3D(points_with_id_VIO_prev, - lmk_id_to_lmk_type_map_prev, - &output->widgets_); - } - - if (!FLAGS_visualize_load_mesh_filename.empty()) { - // TODO(Toni): remove static, never use static wo const/constexpr - static bool visualize_ply_mesh_once = true; - if (visualize_ply_mesh_once) { - visualizePlyMesh(FLAGS_visualize_load_mesh_filename.c_str(), - &output->widgets_); - visualize_ply_mesh_once = false; - } - } - - if (FLAGS_visualize_convex_hull) { - if (planes_prev.size() != 0) { - visualizeConvexHull(planes_prev.at(0).triangle_cluster_, - vertices_mesh_prev, - polygons_mesh_prev, - &output->widgets_); - } - } - - if (backend_type_ == BackendType::kStructuralRegularities && - FLAGS_visualize_plane_constraints) { - LandmarkIds lmk_ids_in_current_pp_factors; - for (const auto& g : input.backend_output_->graph_) { - const auto& ppf = - boost::dynamic_pointer_cast(g); - if (ppf) { - // We found a PointPlaneFactor. - // Get point key. - Key point_key = ppf->getPointKey(); - LandmarkId lmk_id = gtsam::Symbol(point_key).index(); - lmk_ids_in_current_pp_factors.push_back(lmk_id); - // Get point estimate. - gtsam::Point3 point; - // This call makes visualizer unable to perform this - // in parallel. But you can just copy the state_ - CHECK(getEstimateOfKey( - input.backend_output_->state_, point_key, &point)); - // Visualize. - const Key& ppf_plane_key = ppf->getPlaneKey(); - // not sure, we are having some w planes_prev - // others with planes... - for (const Plane& plane : input.mesher_output_->planes_) { - if (ppf_plane_key == plane.getPlaneSymbol().key()) { - gtsam::OrientedPlane3 current_plane_estimate; - CHECK(getEstimateOfKey( // This call makes visualizer - // unable to perform this in - // parallel. - input.backend_output_->state_, - ppf_plane_key, - ¤t_plane_estimate)); - // WARNING assumes the backend updates normal and distance - // of plane and that no one modifies it afterwards... - visualizePlaneConstraints( - plane.getPlaneSymbol().key(), - current_plane_estimate.normal().point3(), - current_plane_estimate.distance(), - lmk_id, - point, - &output->widgets_); - // Stop since there are not multiple planes for one - // ppf. - break; - } - } - } - } - - // Remove lines that are not representing a point plane factor - // in the current graph. - removeOldLines(lmk_ids_in_current_pp_factors); - } - - // Must go after visualize plane constraints. - if (FLAGS_visualize_planes || FLAGS_visualize_plane_constraints) { - for (const Plane& plane : input.mesher_output_->planes_) { - const gtsam::Symbol& plane_symbol = plane.getPlaneSymbol(); - const std::uint64_t& plane_index = plane_symbol.index(); - gtsam::OrientedPlane3 current_plane_estimate; - if (getEstimateOfKey( - input.backend_output_->state_, - plane_symbol.key(), - ¤t_plane_estimate)) { - const cv::Point3d& plane_normal_estimate = - UtilsOpenCV::unit3ToPoint3d(current_plane_estimate.normal()); - CHECK(plane.normal_ == plane_normal_estimate); - // We have the plane in the optimization. - // Visualize plane. - visualizePlane(plane_index, - plane_normal_estimate.x, - plane_normal_estimate.y, - plane_normal_estimate.z, - current_plane_estimate.distance(), - &output->widgets_, - FLAGS_visualize_plane_label, - plane.triangle_cluster_.cluster_id_); - } else { - // We could not find the plane in the optimization... - // Careful cause we might enter here because there are new - // segmented planes. - // Delete the plane. - LOG(ERROR) << "Remove plane viz for id:" << plane_index; - if (FLAGS_visualize_plane_constraints) { - removePlaneConstraintsViz(plane_index); - } - removePlane(plane_index); - } - } - - // Also remove planes that were deleted by the backend... - for (const Plane& plane : planes_prev) { - const gtsam::Symbol& plane_symbol = plane.getPlaneSymbol(); - const std::uint64_t& plane_index = plane_symbol.index(); - gtsam::OrientedPlane3 current_plane_estimate; - if (!getEstimateOfKey(input.backend_output_->state_, - plane_symbol.key(), - ¤t_plane_estimate)) { - // We could not find the plane in the optimization... - // Delete the plane. - if (FLAGS_visualize_plane_constraints) { - removePlaneConstraintsViz(plane_index); - } - removePlane(plane_index); - } - } - } - - planes_prev = input.mesher_output_->planes_; - vertices_mesh_prev = input.mesher_output_->vertices_mesh_; - polygons_mesh_prev = input.mesher_output_->polygons_mesh_; - points_with_id_VIO_prev = input.backend_output_->landmarks_with_id_map_; - lmk_id_to_lmk_type_map_prev = - input.backend_output_->lmk_id_to_lmk_type_map_; - LOG_IF(WARNING, mesh3d_viz_properties_callback_) - << "Coloring the mesh using semantic segmentation colors."; - mesh_3d_viz_props_prev = - // Call semantic mesh segmentation if someone registered a callback. - mesh3d_viz_properties_callback_ - ? mesh3d_viz_properties_callback_(left_stereo_keyframe.timestamp_, - left_stereo_keyframe.img_, - input.mesher_output_->mesh_2d_, - input.mesher_output_->mesh_3d_) - : (FLAGS_texturize_3d_mesh ? OpenCvVisualizer3D::texturizeMesh3D( - left_stereo_keyframe.timestamp_, - left_stereo_keyframe.img_, - input.mesher_output_->mesh_2d_, - input.mesher_output_->mesh_3d_) - : Mesh3DVizProperties()), - VLOG(10) << "Finished mesh visualization."; - - break; - } - - // Computes and visualizes a 3D point cloud with VIO points in current time - // horizon of the optimization. - case VisualizationType::kPointcloud: { - // Do not color the cloud, send empty lmk id to lmk type map - visualizePoints3D(input.backend_output_->landmarks_with_id_map_, - input.backend_output_->lmk_id_to_lmk_type_map_, - &output->widgets_); - break; - } - case VisualizationType::kNone: { - break; - } - } - - // Visualize trajectory. - VLOG(10) << "Starting trajectory visualization..."; - addPoseToTrajectory(input.backend_output_->W_State_Blkf_.pose_.compose( - input.frontend_output_->stereo_frame_lkf_.getBPoseCamLRect())); - visualizeTrajectory3D( - FLAGS_visualize_mesh_in_frustum ? mesh_2d_img : left_stereo_keyframe.img_, - &output->frustum_pose_, - &output->widgets_); - VLOG(10) << "Finished trajectory visualization."; - - return output; -} - -/* -------------------------------------------------------------------------- */ -// Create a 2D mesh from 2D corners in an image -cv::Mat OpenCvVisualizer3D::visualizeMesh2D( - const std::vector& triangulation2D, - const cv::Mat& img, - const KeypointsCV& extra_keypoints) { - static const cv::Scalar kDelaunayColor(0u, 255u, 0u); - static const cv::Scalar kPointsColor(255u, 0u, 0u); - - // Duplicate image for annotation and visualization. - cv::Mat img_clone = img.clone(); - cv::cvtColor(img_clone, img_clone, cv::COLOR_GRAY2BGR); - const cv::Size& size = img_clone.size(); - cv::Rect rect(0, 0, size.width, size.height); - std::vector pt(3); - for (size_t i = 0; i < triangulation2D.size(); i++) { - const cv::Vec6f& t = triangulation2D[i]; - - // Visualize mesh vertices. - pt[0] = cv::Point(cvRound(t[0]), cvRound(t[1])); - pt[1] = cv::Point(cvRound(t[2]), cvRound(t[3])); - pt[2] = cv::Point(cvRound(t[4]), cvRound(t[5])); - - // Visualize mesh edges. - cv::line(img_clone, pt[0], pt[1], kDelaunayColor, 1, CV_AA, 0); - cv::line(img_clone, pt[1], pt[2], kDelaunayColor, 1, CV_AA, 0); - cv::line(img_clone, pt[2], pt[0], kDelaunayColor, 1, CV_AA, 0); - } - - // Visualize extra vertices. - for (const auto& keypoint : extra_keypoints) { - cv::circle(img_clone, keypoint, 2, kPointsColor, CV_FILLED, CV_AA, 0); - } - - return img_clone; -} - -/* -------------------------------------------------------------------------- */ -// Visualize 2d mesh. -cv::Mat OpenCvVisualizer3D::visualizeMesh2DStereo( - const std::vector& triangulation_2D, - const Frame& ref_frame) { - static const cv::Scalar kDelaunayColor(0, 255, 0); // Green - static const cv::Scalar kMeshVertexColor(255, 0, 0); // Blue - static const cv::Scalar kInvalidKeypointsColor(0, 0, 255); // Red - - // Sanity check. - DCHECK(ref_frame.landmarks_.size() == ref_frame.keypoints_.size()) - << "Frame: wrong dimension for the landmarks."; - - // Duplicate image for annotation and visualization. - cv::Mat img_clone = ref_frame.img_.clone(); - cv::cvtColor(img_clone, img_clone, cv::COLOR_GRAY2BGR); - - // Visualize extra vertices. - for (size_t i = 0; i < ref_frame.keypoints_.size(); i++) { - // Only for valid keypoints, but possibly without a right pixel. - // Kpts that are both valid and have a right pixel are currently the ones - // passed to the mesh. - if (ref_frame.landmarks_[i] != -1) { - cv::circle(img_clone, - ref_frame.keypoints_[i], - 2, - kInvalidKeypointsColor, - CV_FILLED, - CV_AA, - 0); - } - } - - std::vector pt(3); - for (const cv::Vec6f& triangle : triangulation_2D) { - // Visualize mesh vertices. - pt[0] = cv::Point(cvRound(triangle[0]), cvRound(triangle[1])); - pt[1] = cv::Point(cvRound(triangle[2]), cvRound(triangle[3])); - pt[2] = cv::Point(cvRound(triangle[4]), cvRound(triangle[5])); - cv::circle(img_clone, pt[0], 2, kMeshVertexColor, CV_FILLED, CV_AA, 0); - cv::circle(img_clone, pt[1], 2, kMeshVertexColor, CV_FILLED, CV_AA, 0); - cv::circle(img_clone, pt[2], 2, kMeshVertexColor, CV_FILLED, CV_AA, 0); - - // Visualize mesh edges. - cv::line(img_clone, pt[0], pt[1], kDelaunayColor, 1, CV_AA, 0); - cv::line(img_clone, pt[1], pt[2], kDelaunayColor, 1, CV_AA, 0); - cv::line(img_clone, pt[2], pt[0], kDelaunayColor, 1, CV_AA, 0); - } - - return img_clone; -} - -/* -------------------------------------------------------------------------- */ -// Visualize a 3D point cloud of unique 3D landmarks. -void OpenCvVisualizer3D::visualizePoints3D( - const PointsWithIdMap& points_with_id, - const LmkIdToLmkTypeMap& lmk_id_to_lmk_type_map, - WidgetsMap* widgets_map) { - CHECK(widgets_map); - bool color_the_cloud = false; - if (lmk_id_to_lmk_type_map.size() != 0) { - color_the_cloud = true; - CHECK_EQ(points_with_id.size(), lmk_id_to_lmk_type_map.size()); - } - - // Sanity check dimension. - if (points_with_id.size() == 0) { - // No points to visualize. - LOG(WARNING) << "No landmark information for Visualizer. " - "Not displaying 3D points."; - return; - } - - // Populate cloud structure with 3D points. - cv::Mat point_cloud(1, points_with_id.size(), CV_32FC3); - cv::Mat point_cloud_color( - 1, lmk_id_to_lmk_type_map.size(), CV_8UC3, cloud_color_); - cv::Point3f* data = point_cloud.ptr(); - size_t i = 0; - for (const std::pair& id_point : points_with_id) { - const gtsam::Point3& point_3d = id_point.second; - data[i].x = static_cast(point_3d.x()); - data[i].y = static_cast(point_3d.y()); - data[i].z = static_cast(point_3d.z()); - if (color_the_cloud) { - DCHECK(lmk_id_to_lmk_type_map.find(id_point.first) != - lmk_id_to_lmk_type_map.end()); - switch (lmk_id_to_lmk_type_map.at(id_point.first)) { - case LandmarkType::SMART: { - point_cloud_color.col(i) = cv::viz::Color::white(); - break; - } - case LandmarkType::PROJECTION: { - point_cloud_color.col(i) = cv::viz::Color::green(); - break; - } - default: { - point_cloud_color.col(i) = cv::viz::Color::white(); - break; - } - } - } - i++; - } - - // Create a cloud widget. - std::unique_ptr cloud_widget = - VIO::make_unique(point_cloud, cloud_color_); - if (color_the_cloud) { - *cloud_widget = cv::viz::WCloud(point_cloud, point_cloud_color); - } - cloud_widget->setRenderingProperty(cv::viz::POINT_SIZE, 6); - - (*widgets_map)["Point cloud"] = std::move(cloud_widget); -} - -/* -------------------------------------------------------------------------- */ -// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. -void OpenCvVisualizer3D::visualizePlane(const PlaneId& plane_index, - const double& n_x, - const double& n_y, - const double& n_z, - const double& d, - WidgetsMap* widgets, - const bool& visualize_plane_label, - const int& cluster_id) { - CHECK_NOTNULL(widgets); - const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); - // Create a plane widget. - const cv::Vec3d normal(n_x, n_y, n_z); - const cv::Point3d center(d * n_x, d * n_y, d * n_z); - static const cv::Vec3d new_yaxis(0, 1, 0); - static const cv::Size2d size(1.0, 1.0); - - cv::viz::Color plane_color; - getColorById(cluster_id, &plane_color); - - if (visualize_plane_label) { - static double increase = 0.0; - const cv::Point3d text_position( - d * n_x, d * n_y, d * n_z + std::fmod(increase, 1)); - increase += 0.1; - (*widgets)[plane_id_for_viz + "_label"] = - VIO::make_unique( - plane_id_for_viz, text_position, 0.07, true); - } - - (*widgets)[plane_id_for_viz] = VIO::make_unique( - center, normal, new_yaxis, size, plane_color); - is_plane_id_in_window_[plane_index] = true; -} - -/* -------------------------------------------------------------------------- */ -// Draw a line in opencv. -void OpenCvVisualizer3D::drawLine(const std::string& line_id, - const double& from_x, - const double& from_y, - const double& from_z, - const double& to_x, - const double& to_y, - const double& to_z, - WidgetsMap* widgets) { - cv::Point3d pt1(from_x, from_y, from_z); - cv::Point3d pt2(to_x, to_y, to_z); - drawLine(line_id, pt1, pt2, widgets); -} - -/* -------------------------------------------------------------------------- */ -void OpenCvVisualizer3D::drawLine(const std::string& line_id, - const cv::Point3d& pt1, - const cv::Point3d& pt2, - WidgetsMap* widgets) { - CHECK_NOTNULL(widgets); - (*widgets)[line_id] = VIO::make_unique(pt1, pt2); -} - -/* -------------------------------------------------------------------------- */ -// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. -void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets) { - cv::Mat colors(0, 1, CV_8UC3, cv::viz::Color::gray()); // Do not color mesh. - visualizeMesh3D(map_points_3d, colors, polygons_mesh, widgets); -} - -/* -------------------------------------------------------------------------- */ -// Visualize a 3D point cloud of unique 3D landmarks with its connectivity, -// and provide color for each polygon. -void OpenCvVisualizer3D::visualizeMesh3D(const cv::Mat& map_points_3d, - const cv::Mat& colors, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets, - const cv::Mat& tcoords, - const cv::Mat& texture) { - CHECK_NOTNULL(widgets); - // Check data - bool color_mesh = false; - if (colors.rows != 0) { - CHECK_EQ(map_points_3d.rows, colors.rows) - << "Map points and Colors should have same number of rows. One" - " color per map point."; - LOG(ERROR) << "Coloring mesh!"; - color_mesh = true; - } - - if (tcoords.rows != 0) { - CHECK_EQ(map_points_3d.rows, tcoords.rows) - << "Map points and tcoords should have same number of rows. One" - "tcoord per map point."; - CHECK(!texture.empty()); - } - - // No points/mesh to visualize. - if (map_points_3d.rows == 0 || polygons_mesh.rows == 0) { - return; - } - - cv::viz::Mesh cv_mesh; - cv_mesh.cloud = map_points_3d.t(); - cv_mesh.polygons = polygons_mesh; - cv_mesh.colors = color_mesh ? colors.t() : cv::Mat(); - cv_mesh.tcoords = tcoords.t(); - cv_mesh.texture = texture; - - // Plot mesh. - (*widgets)["Mesh"] = VIO::make_unique(cv_mesh); -} - -/* -------------------------------------------------------------------------- */ -// Visualize a PLY from filename (absolute path). -void OpenCvVisualizer3D::visualizePlyMesh(const std::string& filename, - WidgetsMap* widgets) { - CHECK_NOTNULL(widgets); - LOG(INFO) << "Showing ground truth mesh: " << filename; - // The ply file must have in the header a "element vertex" and - // a "element face" primitives, otherwise you'll get a - // "Cannot read geometry" error. - cv::viz::Mesh mesh(cv::viz::Mesh::load(filename)); - if (mesh.polygons.size[1] == 0) { - LOG(WARNING) << "No polygons available for mesh, showing point cloud only."; - // If there are no polygons, convert to point cloud, otw there will be - // nothing displayed... - std::unique_ptr cloud = - VIO::make_unique(mesh.cloud, cv::viz::Color::lime()); - cloud->setRenderingProperty(cv::viz::REPRESENTATION, - cv::viz::REPRESENTATION_POINTS); - cloud->setRenderingProperty(cv::viz::POINT_SIZE, 2); - cloud->setRenderingProperty(cv::viz::OPACITY, 0.1); - - // Plot point cloud. - (*widgets)["Mesh from ply"] = std::move(cloud); - } else { - // Plot mesh. - (*widgets)["Mesh from ply"] = VIO::make_unique(mesh); - } -} - -/* -------------------------------------------------------------------------- */ -// Visualize a 3D point cloud of unique 3D landmarks with its connectivity. -/// Each triangle is colored depending on the cluster it is in, or gray if it -/// is in no cluster. -/// [in] clusters: a set of triangle clusters. The ids of the triangles must -/// match the order in polygons_mesh. -/// [in] map_points_3d: set of 3d points in the mesh, format is n rows, with -/// three columns (x, y, z). -/// [in] polygons_mesh: mesh faces, format is n rows, 1 column, -/// with [n id_a id_b id_c, ..., n /id_x id_y id_z], where n = polygon size -/// n=3 for triangles. -/// [in] color_mesh whether to color the mesh or not -/// [in] timestamp to store the timestamp of the mesh when logging the mesh. -void OpenCvVisualizer3D::visualizeMesh3DWithColoredClusters( - const std::vector& planes, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets, - const bool visualize_mesh_with_colored_polygon_clusters, - const Timestamp& timestamp) { - if (visualize_mesh_with_colored_polygon_clusters) { - // Color the mesh. - cv::Mat colors; - colorMeshByClusters(planes, map_points_3d, polygons_mesh, &colors); - // Visualize the colored mesh. - visualizeMesh3D(map_points_3d, colors, polygons_mesh, widgets); - // Log the mesh. - if (FLAGS_log_mesh) { - logMesh(map_points_3d, - colors, - polygons_mesh, - timestamp, - FLAGS_log_accumulated_mesh); - } - } else { - // Visualize the mesh with same colour. - visualizeMesh3D(map_points_3d, polygons_mesh, widgets); - } -} - -/* -------------------------------------------------------------------------- */ -// Visualize convex hull in 2D for set of points in triangle cluster, -// projected along the normal of the cluster. -void OpenCvVisualizer3D::visualizeConvexHull(const TriangleCluster& cluster, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - WidgetsMap* widgets_map) { - CHECK_NOTNULL(widgets_map); - - // Create a new coord system, which has as z the normal. - const cv::Point3f& normal = cluster.cluster_direction_; - - // Find first axis of the coord system. - // Pick random x and y - static constexpr float random_x = 0.1; - static constexpr float random_y = 0.0; - // Find z, such that dot product with the normal is 0. - float z = -(normal.x * random_x + normal.y * random_y) / normal.z; - // Create new first axis: - cv::Point3f x_axis(random_x, random_y, z); - // Normalize new first axis: - x_axis /= cv::norm(x_axis); - // Create new second axis, by cross product normal to x_axis; - cv::Point3f y_axis = normal.cross(x_axis); - // Normalize just in case? - y_axis /= cv::norm(y_axis); - // Construct new cartesian coord system: - cv::Mat new_coordinates(3, 3, CV_32FC1); - new_coordinates.at(0, 0) = x_axis.x; - new_coordinates.at(0, 1) = x_axis.y; - new_coordinates.at(0, 2) = x_axis.z; - new_coordinates.at(1, 0) = y_axis.x; - new_coordinates.at(1, 1) = y_axis.y; - new_coordinates.at(1, 2) = y_axis.z; - new_coordinates.at(2, 0) = normal.x; - new_coordinates.at(2, 1) = normal.y; - new_coordinates.at(2, 2) = normal.z; - - std::vector points_2d; - std::vector z_s; - for (const size_t& triangle_id : cluster.triangle_ids_) { - const size_t& triangle_idx = std::round(triangle_id * 4); - LOG_IF(FATAL, triangle_idx + 3 >= polygons_mesh.rows) - << "Visualizer3D: an id in triangle_ids_ is too large."; - const int32_t& idx_1 = polygons_mesh.at(triangle_idx + 1); - const int32_t& idx_2 = polygons_mesh.at(triangle_idx + 2); - const int32_t& idx_3 = polygons_mesh.at(triangle_idx + 3); - - // Project points to new coord system - const cv::Point3f& new_map_point_1 = // new_coordinates * - // map_points_3d.row(idx_1).t(); - map_points_3d.at(idx_1); - const cv::Point3f& new_map_point_2 = // new_coordinates * - // map_points_3d.row(idx_2).t(); - map_points_3d.at(idx_2); - const cv::Point3f& new_map_point_3 = // new_coordinates * - // map_points_3d.row(idx_3).t(); - map_points_3d.at(idx_3); - - // Keep only 1st and 2nd component, aka the projection of the point on the - // plane. - points_2d.push_back(cv::Point2f(new_map_point_1.x, new_map_point_1.y)); - z_s.push_back(new_map_point_1.z); - points_2d.push_back(cv::Point2f(new_map_point_2.x, new_map_point_2.y)); - z_s.push_back(new_map_point_2.z); - points_2d.push_back(cv::Point2f(new_map_point_3.x, new_map_point_3.y)); - z_s.push_back(new_map_point_3.z); - } - - // Create convex hull. - if (points_2d.size() != 0) { - std::vector hull_idx; - convexHull(cv::Mat(points_2d), hull_idx, false); - - // Add the z component. - std::vector hull_3d; - for (const int& idx : hull_idx) { - hull_3d.push_back( - cv::Point3f(points_2d.at(idx).x, points_2d.at(idx).y, z_s.at(idx))); - } - - static constexpr bool visualize_hull_as_polyline = false; - if (visualize_hull_as_polyline) { - // Close the hull. - CHECK_NE(hull_idx.size(), 0); - hull_3d.push_back(cv::Point3f(points_2d.at(hull_idx.at(0)).x, - points_2d.at(hull_idx.at(0)).y, - z_s.at(hull_idx.at(0)))); - // Visualize convex hull. - (*widgets_map)["Convex Hull"] = - VIO::make_unique(hull_3d); - } else { - // Visualize convex hull as a mesh of one polygon with multiple points. - if (hull_3d.size() > 2) { - cv::Mat polygon_hull = cv::Mat(4 * (hull_3d.size() - 2), 1, CV_32SC1); - size_t i = 1; - for (size_t k = 0; k + 3 < polygon_hull.rows; k += 4) { - polygon_hull.row(k) = 3; - polygon_hull.row(k + 1) = 0; - polygon_hull.row(k + 2) = static_cast(i); - polygon_hull.row(k + 3) = static_cast(i) + 1; - i++; - } - cv::viz::Color mesh_color; - getColorById(cluster.cluster_id_, &mesh_color); - cv::Mat normals = cv::Mat(0, 1, CV_32FC3); - for (size_t k = 0; k < hull_3d.size(); k++) { - normals.push_back(normal); - } - cv::Mat colors(hull_3d.size(), 1, CV_8UC3, mesh_color); - (*widgets_map)["Convex Hull"] = VIO::make_unique( - hull_3d, polygon_hull, colors.t(), normals.t()); - } - } - } -} - -/* -------------------------------------------------------------------------- */ -// Visualize trajectory. Adds an image to the frustum if cv::Mat is not empty. -void OpenCvVisualizer3D::visualizeTrajectory3D(const cv::Mat& frustum_image, - cv::Affine3d* frustum_pose, - WidgetsMap* widgets_map) { - CHECK_NOTNULL(frustum_pose); - CHECK_NOTNULL(widgets_map); - - if (trajectory_poses_3d_.size() == 0) { // no points to visualize - return; - } - - // Show current camera pose. - static const cv::Matx33d K(458, 0.0, 360, 0.0, 458, 240, 0.0, 0.0, 1.0); - std::unique_ptr cam_widget_ptr = nullptr; - if (frustum_image.empty()) { - cam_widget_ptr = VIO::make_unique( - K, 1.0, cv::viz::Color::white()); - } else { - cam_widget_ptr = VIO::make_unique( - K, frustum_image, 1.0, cv::viz::Color::white()); - } - CHECK(cam_widget_ptr); - // Normally you would use Widget3D.setPose(), or updatePose(), but it does not - // seem to work, so we send the pose to the display module, which then calls - // the window_.setWidgetPose()... - *frustum_pose = trajectory_poses_3d_.back(); - (*widgets_map)["Camera Pose with Frustum"] = std::move(cam_widget_ptr); - - // Option A: This does not work very well. - // window_data_.window_.resetCameraViewpoint("Camera Pose with Frustum"); - // Viewer is our viewpoint, camera the pose estimate (frustum). - static constexpr bool follow_camera = false; - if (follow_camera) { - cv::Affine3f camera_in_world_coord = trajectory_poses_3d_.back(); - // Option B: specify viewer wrt camera. Works, but motion is non-smooth. - // cv::Affine3f viewer_in_camera_coord (Vec3f( - // -0.3422019, -0.3422019, 1.5435732), - // Vec3f(3.0, 0.0, -4.5)); - // cv::Affine3f viewer_in_world_coord = - // viewer_in_camera_coord.concatenate(camera_in_world_coord); - - // Option C: use "look-at" camera parametrization. - // Works, but motion is non-smooth as well. - cv::Vec3d cam_pos(-6.0, 0.0, 6.0); - cv::Vec3d cam_focal_point(camera_in_world_coord.translation()); - cv::Vec3d cam_y_dir(0.0, 0.0, -1.0); - cv::Affine3f cam_pose = - cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir); - // window_data_.window_.setViewerPose(cam_pose); - // window_data_.window_.setViewerPose(viewer_in_world_coord); - } - - // Create a Trajectory frustums widget. - std::vector trajectory_frustums( - trajectory_poses_3d_.end() - - std::min(trajectory_poses_3d_.size(), size_t(10u)), - trajectory_poses_3d_.end()); - (*widgets_map)["Trajectory Frustums"] = - VIO::make_unique( - trajectory_frustums, K, 0.2, cv::viz::Color::red()); - - // Create a Trajectory widget. (argument can be PATH, FRAMES, BOTH). - std::vector trajectory(trajectory_poses_3d_.begin(), - trajectory_poses_3d_.end()); - (*widgets_map)["Trajectory"] = VIO::make_unique( - trajectory, cv::viz::WTrajectory::PATH, 1.0, cv::viz::Color::red()); -} - -/* -------------------------------------------------------------------------- */ -// Remove widget. True if successful, false if not. -bool OpenCvVisualizer3D::removeWidget(const std::string& widget_id) { - try { - // window_data_.window_.removeWidget(widget_id); - return true; - } catch (const cv::Exception& e) { - VLOG(20) << e.what(); - LOG(ERROR) << "Widget with id: " << widget_id.c_str() - << " is not in window."; - } catch (...) { - LOG(ERROR) << "Unrecognized exception when using " - "window_data_.window_.removeWidget() " - << "with widget with id: " << widget_id.c_str(); - } - return false; -} - -/* -------------------------------------------------------------------------- */ -// Visualize line widgets from plane to lmks. -// Point key is required to avoid duplicated lines! -void OpenCvVisualizer3D::visualizePlaneConstraints(const PlaneId& plane_id, - const gtsam::Point3& normal, - const double& distance, - const LandmarkId& lmk_id, - const gtsam::Point3& point, - WidgetsMap* widgets) { - PlaneIdMap::iterator plane_id_it = plane_id_map_.find(plane_id); - LmkIdToLineIdMap* lmk_id_to_line_id_map_ptr = nullptr; - LineNr* line_nr_ptr = nullptr; - if (plane_id_it != plane_id_map_.end()) { - // We already have this plane id stored. - lmk_id_to_line_id_map_ptr = &(plane_id_it->second); - - // Ensure we also have the line nr stored. - const auto& line_nr_it = plane_to_line_nr_map_.find(plane_id); - CHECK(line_nr_it != plane_to_line_nr_map_.end()); - line_nr_ptr = &(line_nr_it->second); - } else { - // We have not this plane id stored. - // Create it by calling default ctor. - lmk_id_to_line_id_map_ptr = &(plane_id_map_[plane_id]); - plane_id_it = plane_id_map_.find(plane_id); - DCHECK(plane_id_it != plane_id_map_.end()); - - // Also start line nr to 0. - plane_to_line_nr_map_[plane_id] = 0; - DCHECK(plane_to_line_nr_map_.find(plane_id) != plane_to_line_nr_map_.end()); - line_nr_ptr = &(plane_to_line_nr_map_[plane_id]); - } - CHECK_NOTNULL(lmk_id_to_line_id_map_ptr); - CHECK_NOTNULL(line_nr_ptr); - - // TODO should use map from line_id_to_lmk_id as well, - // to remove the line_ids which are not having a lmk_id... - const auto& lmk_id_to_line_id = lmk_id_to_line_id_map_ptr->find(lmk_id); - if (lmk_id_to_line_id == lmk_id_to_line_id_map_ptr->end()) { - // We have never drawn this line. - // Store line nr (as line id). - (*lmk_id_to_line_id_map_ptr)[lmk_id] = *line_nr_ptr; - std::string line_id = "Line " + - std::to_string(static_cast(plane_id_it->first)) + - std::to_string(static_cast(*line_nr_ptr)); - // Draw it. - drawLineFromPlaneToPoint(line_id, - normal.x(), - normal.y(), - normal.z(), - distance, - point.x(), - point.y(), - point.z(), - widgets); - // Augment line_nr for next line_id. - (*line_nr_ptr)++; - } else { - // We have drawn this line before. - // Update line. - std::string line_id = - "Line " + std::to_string(static_cast(plane_id_it->first)) + - std::to_string(static_cast(lmk_id_to_line_id->second)); - updateLineFromPlaneToPoint(line_id, - normal.x(), - normal.y(), - normal.z(), - distance, - point.x(), - point.y(), - point.z(), - widgets); - } -} - -/* -------------------------------------------------------------------------- */ -// Remove line widgets from plane to lmks, for lines that are not pointing -// to any lmk_id in lmk_ids. -void OpenCvVisualizer3D::removeOldLines(const LandmarkIds& lmk_ids) { - for (PlaneIdMap::value_type& plane_id_pair : plane_id_map_) { - LmkIdToLineIdMap& lmk_id_to_line_id_map = plane_id_pair.second; - for (LmkIdToLineIdMap::iterator lmk_id_to_line_id_it = - lmk_id_to_line_id_map.begin(); - lmk_id_to_line_id_it != lmk_id_to_line_id_map.end();) { - if (std::find(lmk_ids.begin(), - lmk_ids.end(), - lmk_id_to_line_id_it->first) == lmk_ids.end()) { - // We did not find the lmk_id of the current line in the list - // of lmk_ids... - // Delete the corresponding line. - std::string line_id = - "Line " + std::to_string(static_cast(plane_id_pair.first)) + - std::to_string(static_cast(lmk_id_to_line_id_it->second)); - removeWidget(line_id); - // Delete the corresponding entry in the map from lmk id to line id. - lmk_id_to_line_id_it = - lmk_id_to_line_id_map.erase(lmk_id_to_line_id_it); - } else { - lmk_id_to_line_id_it++; - } - } - } -} - -/* -------------------------------------------------------------------------- */ -// Remove line widgets from plane to lmks. -void OpenCvVisualizer3D::removePlaneConstraintsViz(const PlaneId& plane_id) { - PlaneIdMap::iterator plane_id_it = plane_id_map_.find(plane_id); - if (plane_id_it != plane_id_map_.end()) { - VLOG(0) << "Removing line constraints for plane with id: " << plane_id; - for (const auto& lmk_id_to_line_id : plane_id_it->second) { - std::string line_id = - "Line " + std::to_string(static_cast(plane_id_it->first)) + - std::to_string(static_cast(lmk_id_to_line_id.second)); - removeWidget(line_id); - } - // Delete the corresponding entry in the map for this plane. - plane_id_map_.erase(plane_id_it); - // Same for the map holding the line nr. - auto line_nr_it = plane_to_line_nr_map_.find(plane_id); - CHECK(line_nr_it != plane_to_line_nr_map_.end()); - plane_to_line_nr_map_.erase(line_nr_it); - } else { - // Careful if we did not find, might be because it is a newly segmented - // plane. - LOG(WARNING) << "Could not find plane with id: " << plane_id - << " from plane_id_map_..."; - } -} - -/* -------------------------------------------------------------------------- */ -// Remove plane widget. -void OpenCvVisualizer3D::removePlane(const PlaneId& plane_index, - const bool& remove_plane_label) { - const std::string& plane_id_for_viz = "Plane " + std::to_string(plane_index); - if (is_plane_id_in_window_.find(plane_index) != - is_plane_id_in_window_.end() && - is_plane_id_in_window_[plane_index]) { - if (removeWidget(plane_id_for_viz)) { - if (remove_plane_label) { - if (!removeWidget(plane_id_for_viz + "_label")) { - LOG(WARNING) << "Did you disable labels of planes?, then also" - "disable label removal. Otherwise, did you change " - "the id of the label? then change it here as well."; - } - } - is_plane_id_in_window_[plane_index] = false; - } else { - is_plane_id_in_window_[plane_index] = true; - } - } -} - -/* -------------------------------------------------------------------------- */ -// Add pose to the previous trajectory. -void OpenCvVisualizer3D::addPoseToTrajectory( - const gtsam::Pose3& current_pose_gtsam) { - trajectory_poses_3d_.push_back( - UtilsOpenCV::gtsamPose3ToCvAffine3d(current_pose_gtsam)); - if (FLAGS_displayed_trajectory_length > 0) { - while (trajectory_poses_3d_.size() > FLAGS_displayed_trajectory_length) { - trajectory_poses_3d_.pop_front(); - } - } -} - -/* ------------------------------------------------------------------------ */ -Mesh3DVizProperties OpenCvVisualizer3D::texturizeMesh3D( - const Timestamp& image_timestamp, - const cv::Mat& texture_image, - const Mesh2D& mesh_2d, - const Mesh3D& mesh_3d) { - // Dummy checks for valid data. - CHECK(!texture_image.empty()); - CHECK_GE(mesh_2d.getNumberOfUniqueVertices(), 0); - CHECK_GE(mesh_3d.getNumberOfUniqueVertices(), 0); - - // Let us fill the mesh 3d viz properties structure. - Mesh3DVizProperties mesh_3d_viz_props; - - // Color all vertices in red. Each polygon will be colored according - // to a mix of the three vertices colors I think... - mesh_3d_viz_props.colors_ = cv::Mat( - mesh_3d.getNumberOfUniqueVertices(), 1, CV_8UC3, cv::viz::Color::red()); - - // Add texture to the mesh using the given image. - // README: tcoords specify the texture coordinates of the 3d mesh wrt 2d - // image. As a small hack, we not only use the left_image as texture but we - // also horizontally concatenate a white image so we can set a white texture - // to those 3d mesh faces which should not have a texture. Below we init all - // tcoords to 0.99 (1.0) gives a weird texture... Meaning that all faces - // start with a default white texture, and then we change that texture to - // the right texture for each 2d triangle that has a corresponding 3d face. - Mesh2D::Polygon polygon; - std::vector tcoords(mesh_3d.getNumberOfUniqueVertices(), - cv::Vec2d(0.9, 0.9)); - for (size_t i = 0; i < mesh_2d.getNumberOfPolygons(); i++) { - CHECK(mesh_2d.getPolygon(i, &polygon)) << "Could not retrieve 2d polygon."; - - const LandmarkId& lmk0 = polygon.at(0).getLmkId(); - const LandmarkId& lmk1 = polygon.at(1).getLmkId(); - const LandmarkId& lmk2 = polygon.at(2).getLmkId(); - - // Returns indices of points in the 3D mesh corresponding to the vertices - // in the 2D mesh. - int p0_id, p1_id, p2_id; - if (mesh_3d.getVertex(lmk0, nullptr, &p0_id) && - mesh_3d.getVertex(lmk1, nullptr, &p1_id) && - mesh_3d.getVertex(lmk2, nullptr, &p2_id)) { - // Sanity check. - CHECK_LE(p0_id, tcoords.size()); - CHECK_LE(p1_id, tcoords.size()); - CHECK_LE(p2_id, tcoords.size()); - - // Get pixel coordinates of the vertices of the 2D mesh. - const auto& px0 = polygon.at(0).getVertexPosition(); - const auto& px1 = polygon.at(1).getVertexPosition(); - const auto& px2 = polygon.at(2).getVertexPosition(); - - // These pixels correspond to the tcoords in the image for the 3d mesh - // vertices. - VLOG(100) << "Pixel: with id: " << p0_id << ", x: " << px0.x - << ", y: " << px0.y; - // We divide by 2.0 to account for fake default texture padded to the - // right of the texture_image. - tcoords.at(p0_id) = cv::Vec2d(px0.x / texture_image.cols / 2.0, - px0.y / texture_image.rows); - tcoords.at(p1_id) = cv::Vec2d(px1.x / texture_image.cols / 2.0, - px1.y / texture_image.rows); - tcoords.at(p2_id) = cv::Vec2d(px2.x / texture_image.cols / 2.0, - px2.y / texture_image.rows); - mesh_3d_viz_props.colors_.row(p0_id) = cv::viz::Color::white(); - mesh_3d_viz_props.colors_.row(p1_id) = cv::viz::Color::white(); - mesh_3d_viz_props.colors_.row(p2_id) = cv::viz::Color::white(); - } else { - // If we did not find a corresponding 3D triangle for the 2D triangle - // leave tcoords and colors to the default values. - LOG_EVERY_N(ERROR, 1000) << "Polygon in 2d mesh did not have a " - "corresponding polygon in 3d mesh!"; - } - } - - // Add a column with a fixed color at the end so that we can specify an - // "invalid" or "default" texture for those points which we do not want to - // texturize. - static cv::Mat default_texture(texture_image.rows, - texture_image.cols, - texture_image.type(), - cv::viz::Color::white()); - CHECK_EQ(texture_image.dims, default_texture.dims); - CHECK_EQ(texture_image.rows, default_texture.rows); - CHECK_EQ(texture_image.type(), default_texture.type()); - - cv::Mat texture; - // Padding actual texture with default texture, a bit hacky, but works. - cv::hconcat(texture_image, default_texture, texture); - mesh_3d_viz_props.texture_ = texture; - - mesh_3d_viz_props.tcoords_ = cv::Mat(tcoords, true).reshape(2); - CHECK_EQ(mesh_3d_viz_props.tcoords_.size().height, - mesh_3d.getNumberOfUniqueVertices()); - - return mesh_3d_viz_props; -} - -/* -------------------------------------------------------------------------- */ -// Log mesh to ply file. -void OpenCvVisualizer3D::logMesh(const cv::Mat& map_points_3d, - const cv::Mat& colors, - const cv::Mat& polygons_mesh, - const Timestamp& timestamp, - bool log_accumulated_mesh) { - /// Log the mesh in a ply file. - static Timestamp last_timestamp = timestamp; - static const Timestamp first_timestamp = timestamp; - if ((timestamp - last_timestamp) > - 6500000000) { // Log every 6 seconds approx. (a little bit more than - // time-horizon) - LOG(WARNING) << "Logging mesh every (ns) = " << timestamp - last_timestamp; - CHECK(logger_); - logger_->logMesh( - map_points_3d, colors, polygons_mesh, timestamp, log_accumulated_mesh); - last_timestamp = timestamp; - } -} - -/* -------------------------------------------------------------------------- */ -// Input the mesh points and triangle clusters, and -// output colors matrix for mesh visualizer. -// This will color the point with the color of the last plane having it. -void OpenCvVisualizer3D::colorMeshByClusters(const std::vector& planes, - const cv::Mat& map_points_3d, - const cv::Mat& polygons_mesh, - cv::Mat* colors) const { - CHECK_NOTNULL(colors); - *colors = cv::Mat(map_points_3d.rows, 1, CV_8UC3, cv::viz::Color::gray()); - - // The code below assumes triangles as polygons. - static constexpr bool log_landmarks = false; - for (const Plane& plane : planes) { - const TriangleCluster& cluster = plane.triangle_cluster_; - // Decide color for cluster. - cv::viz::Color cluster_color = cv::viz::Color::gray(); - getColorById(cluster.cluster_id_, &cluster_color); - - for (const size_t& triangle_id : cluster.triangle_ids_) { - size_t triangle_idx = std::round(triangle_id * 4); - DCHECK_LE(triangle_idx + 3, polygons_mesh.rows) - << "Visualizer3D: an id in triangle_ids_ is too large."; - const int32_t& idx_1 = polygons_mesh.at(triangle_idx + 1); - const int32_t& idx_2 = polygons_mesh.at(triangle_idx + 2); - const int32_t& idx_3 = polygons_mesh.at(triangle_idx + 3); - // Overrides potential previous color. - colors->row(idx_1) = cluster_color; - colors->row(idx_2) = cluster_color; - colors->row(idx_3) = cluster_color; - } - } -} - -/* -------------------------------------------------------------------------- */ -// Decide color of the cluster depending on its id. -void OpenCvVisualizer3D::getColorById(const size_t& id, - cv::viz::Color* color) const { - CHECK_NOTNULL(color); - switch (id) { - case 0: { - *color = cv::viz::Color::red(); - break; - } - case 1: { - *color = cv::viz::Color::green(); - break; - } - case 2: { - *color = cv::viz::Color::blue(); - break; - } - default: { - *color = cv::viz::Color::gray(); - break; - } - } -} - -/* -------------------------------------------------------------------------- */ -// Draw a line from lmk to plane center. -void OpenCvVisualizer3D::drawLineFromPlaneToPoint(const std::string& line_id, - const double& plane_n_x, - const double& plane_n_y, - const double& plane_n_z, - const double& plane_d, - const double& point_x, - const double& point_y, - const double& point_z, - WidgetsMap* widgets) { - const cv::Point3d center( - plane_d * plane_n_x, plane_d * plane_n_y, plane_d * plane_n_z); - const cv::Point3d point(point_x, point_y, point_z); - drawLine(line_id, center, point, widgets); -} - -/* -------------------------------------------------------------------------- */ -// Update line from lmk to plane center. -void OpenCvVisualizer3D::updateLineFromPlaneToPoint(const std::string& line_id, - const double& plane_n_x, - const double& plane_n_y, - const double& plane_n_z, - const double& plane_d, - const double& point_x, - const double& point_y, - const double& point_z, - WidgetsMap* widgets) { - removeWidget(line_id); - drawLineFromPlaneToPoint(line_id, - plane_n_x, - plane_n_y, - plane_n_z, - plane_d, - point_x, - point_y, - point_z, - widgets); -} - } // namespace VIO diff --git a/src/visualizer/Visualizer3DFactory.cpp b/src/visualizer/Visualizer3DFactory.cpp index 566ab49c2..0f8fc6118 100644 --- a/src/visualizer/Visualizer3DFactory.cpp +++ b/src/visualizer/Visualizer3DFactory.cpp @@ -13,10 +13,11 @@ */ #include "kimera-vio/visualizer/Visualizer3DFactory.h" +#include "kimera-vio/visualizer/OpenCvVisualizer3D.h" namespace VIO { -OpenCvVisualizer3D::UniquePtr VisualizerFactory::createVisualizer( +Visualizer3D::UniquePtr VisualizerFactory::createVisualizer( const VisualizerType visualizer_type, const VisualizationType& viz_type, const BackendType& backend_type) { From 24565893f4f2cc761a1cea72fc46e8250556b154 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 11 May 2020 23:59:21 -0400 Subject: [PATCH 15/15] Fix one include --- src/visualizer/OpenCvVisualizer3D.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/visualizer/OpenCvVisualizer3D.cpp b/src/visualizer/OpenCvVisualizer3D.cpp index 17756ad74..882187dc4 100644 --- a/src/visualizer/OpenCvVisualizer3D.cpp +++ b/src/visualizer/OpenCvVisualizer3D.cpp @@ -12,7 +12,7 @@ * @author Antoni Rosinol */ -#include "kimera-vio/visualizer/Visualizer3D.h" +#include "kimera-vio/visualizer/OpenCvVisualizer3D.h" #include // for min #include // for shared_ptr<>