From 87da36af389b2707746ca03285901cb71adf4011 Mon Sep 17 00:00:00 2001 From: Osenberg Date: Mon, 14 Jan 2019 15:14:58 +0800 Subject: [PATCH 01/12] fix(device.cc:456): fixed compile error on windows --- src/mynteye/device/device.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mynteye/device/device.cc b/src/mynteye/device/device.cc index 6e1bd6e..344eea7 100644 --- a/src/mynteye/device/device.cc +++ b/src/mynteye/device/device.cc @@ -455,7 +455,7 @@ device::StreamData Device::GetStreamData(const Stream &stream) { } device::StreamData Device::GetLatestStreamData(const Stream &stream) { - GetStreamData(stream); + return GetStreamData(stream); } std::vector Device::GetStreamDatas(const Stream &stream) { From d1432e2745646800ac732f89d364b9a92d6bdf1b Mon Sep 17 00:00:00 2001 From: TinyOh Date: Mon, 14 Jan 2019 15:25:51 +0800 Subject: [PATCH 02/12] fix(cam models): sample enable bug and remove unused include --- CMakeLists.txt | 14 +++++++------- .../tutorials/intermediate/get_depth_and_points.cc | 1 + src/mynteye/api/processor/rectify_processor.h | 2 -- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 745ba70..b491d07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,15 +236,15 @@ if(WITH_API) src/mynteye/api/processor/depth_processor_ocv.cc src/mynteye/api/processor/rectify_processor_ocv.cc ) + if(WITH_CAM_MODELS) + list(APPEND MYNTEYE_SRCS + src/mynteye/api/processor/depth_processor.cc + src/mynteye/api/processor/points_processor.cc + src/mynteye/api/processor/rectify_processor.cc + ) + endif() endif() -if(WITH_CAM_MODELS) - list(APPEND MYNTEYE_SRCS - src/mynteye/api/processor/depth_processor.cc - src/mynteye/api/processor/points_processor.cc - src/mynteye/api/processor/rectify_processor.cc - ) -endif() if(NOT WITH_GLOG) list(APPEND MYNTEYE_SRCS src/mynteye/miniglog.cc) endif() diff --git a/samples/tutorials/intermediate/get_depth_and_points.cc b/samples/tutorials/intermediate/get_depth_and_points.cc index 5a2d5d8..ea3901f 100644 --- a/samples/tutorials/intermediate/get_depth_and_points.cc +++ b/samples/tutorials/intermediate/get_depth_and_points.cc @@ -158,6 +158,7 @@ int main(int argc, char *argv[]) { api->SetOptionValue(Option::IR_CONTROL, 80); api->EnableStreamData(Stream::DISPARITY_NORMALIZED); + api->EnableStreamData(Stream::POINTS); api->EnableStreamData(Stream::DEPTH); api->Start(Source::VIDEO_STREAMING); diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 9721a38..5929dfc 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -25,9 +25,7 @@ #include "mynteye/device/device.h" #include #include -#include #include -#include #include #include #include From d42b8ef69c1234fe7a0845f16bd7ff269227571b Mon Sep 17 00:00:00 2001 From: TinyOh Date: Mon, 14 Jan 2019 16:06:35 +0800 Subject: [PATCH 03/12] fix(calib model): default config now work --- CMakeLists.txt | 1 + src/mynteye/api/api.cc | 3 +++ src/mynteye/api/config.cc | 3 +++ src/mynteye/api/synthetic.cc | 37 +++++++++++++++++++++++------------- src/mynteye/api/synthetic.h | 2 +- 5 files changed, 32 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b491d07..09a1fb9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -235,6 +235,7 @@ if(WITH_API) src/mynteye/api/processor/points_processor_ocv.cc src/mynteye/api/processor/depth_processor_ocv.cc src/mynteye/api/processor/rectify_processor_ocv.cc + src/mynteye/api/config.cc ) if(WITH_CAM_MODELS) list(APPEND MYNTEYE_SRCS diff --git a/src/mynteye/api/api.cc b/src/mynteye/api/api.cc index 106a93a..8e09c9c 100644 --- a/src/mynteye/api/api.cc +++ b/src/mynteye/api/api.cc @@ -246,13 +246,16 @@ std::shared_ptr API::Create(const std::shared_ptr &device) { "to learn more."; LOG(WARNING) << "use pinhole as default"; api = std::make_shared(device, CalibrationModel::UNKNOW); + return api; } else { if (left_intr->calib_model() != right_intr->calib_model()) { LOG(ERROR) << "left camera and right camera use different calib models!"; LOG(WARNING) << "use pinhole as default"; api = std::make_shared(device, CalibrationModel::UNKNOW); + return api; } else { api = std::make_shared(device, left_intr->calib_model()); + return api; } } } else { diff --git a/src/mynteye/api/config.cc b/src/mynteye/api/config.cc index e39af95..242aa9b 100644 --- a/src/mynteye/api/config.cc +++ b/src/mynteye/api/config.cc @@ -21,6 +21,9 @@ MYNTEYE_BEGIN_NAMESPACE std::shared_ptr getDefaultIntrinsics() { auto res = std::make_shared(); + res->width = 640; + res->height = 400; + res->model = 0; res->fx = 3.6220059643202876e+02; res->fy = 3.6350065250745848e+02; res->cx = 4.0658699068023441e+02; diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index ef5c14d..dbaf4db 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -28,6 +28,7 @@ #include "mynteye/api/processor/rectify_processor_ocv.h" #include "mynteye/api/processor/depth_processor_ocv.h" #include "mynteye/api/processor/points_processor_ocv.h" +#include "mynteye/api/config.h" #ifdef WITH_CAM_MODELS #include "mynteye/api/processor/depth_processor.h" #include "mynteye/api/processor/points_processor.h" @@ -74,20 +75,26 @@ void process_childs( } // namespace void Synthetic::InitCalibInfo() { - if (calib_model_ == CalibrationModel::UNKNOW) { - calib_model_ = CalibrationModel::PINHOLE; - LOG(INFO) << "camera calib model: unknow"; - // use default - } else { - if (calib_model_ == CalibrationModel::PINHOLE) { - LOG(INFO) << "camera calib model: pinhole"; - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { - LOG(INFO) << "camera calib model: kannala_brandt"; - } + if (calib_model_ == CalibrationModel::PINHOLE) { + LOG(INFO) << "camera calib model: pinhole"; intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); extr_ = std::make_shared( api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); +#ifdef WITH_CAM_MODELS + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + LOG(INFO) << "camera calib model: kannala_brandt"; + intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); + intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); + extr_ = std::make_shared( + api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); +#endif + } else { + calib_model_ = CalibrationModel::PINHOLE; + LOG(INFO) << "camera calib model: unknow ,use default pinhole data"; + intr_left_ = getDefaultIntrinsics(); + intr_right_ = getDefaultIntrinsics(); + extr_ = getDefaultExtrinsics(); } } @@ -110,16 +117,20 @@ Synthetic::~Synthetic() { } } -void Synthetic::NotifyImageParamsChanged() { +void Synthetic::NotifyImageParamsChanged(bool is_from_dev) { + if (is_from_dev && calib_model_ == CalibrationModel::PINHOLE) { intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); extr_ = std::make_shared( api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); - if (calib_model_ == CalibrationModel::PINHOLE) { auto &&processor = find_processor(processor_); if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); #ifdef WITH_CAM_MODELS - } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { + } else if (is_from_dev && calib_model_ == CalibrationModel::KANNALA_BRANDT) { + intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); + intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); + extr_ = std::make_shared( + api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); auto &&processor = find_processor(processor_); if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); #endif diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index eff452d..1777d38 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -44,7 +44,7 @@ class Synthetic { explicit Synthetic(API *api, CalibrationModel calib_model); ~Synthetic(); - void NotifyImageParamsChanged(); + void NotifyImageParamsChanged(bool is_from_dev = false); bool Supports(const Stream &stream) const; mode_t SupportsMode(const Stream &stream) const; From e46c9371db35fa69141cae3c57b5e0c903d01743 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Mon, 14 Jan 2019 16:40:05 +0800 Subject: [PATCH 04/12] fix(calib models): default work improve --- src/mynteye/api/synthetic.cc | 16 ++++++++-------- src/mynteye/api/synthetic.h | 3 ++- 2 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index dbaf4db..02810fa 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -90,6 +90,7 @@ void Synthetic::InitCalibInfo() { api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); #endif } else { + calib_default_tag_ = true; calib_model_ = CalibrationModel::PINHOLE; LOG(INFO) << "camera calib model: unknow ,use default pinhole data"; intr_left_ = getDefaultIntrinsics(); @@ -101,7 +102,8 @@ void Synthetic::InitCalibInfo() { Synthetic::Synthetic(API *api, CalibrationModel calib_model) : api_(api), plugin_(nullptr), - calib_model_(calib_model) { + calib_model_(calib_model), + calib_default_tag_(false) { VLOG(2) << __func__; CHECK_NOTNULL(api_); InitCalibInfo(); @@ -117,20 +119,18 @@ Synthetic::~Synthetic() { } } -void Synthetic::NotifyImageParamsChanged(bool is_from_dev) { - if (is_from_dev && calib_model_ == CalibrationModel::PINHOLE) { +void Synthetic::NotifyImageParamsChanged() { + if (!calib_default_tag_) { intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); extr_ = std::make_shared( api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); + } + if (calib_model_ == CalibrationModel::PINHOLE) { auto &&processor = find_processor(processor_); if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); #ifdef WITH_CAM_MODELS - } else if (is_from_dev && calib_model_ == CalibrationModel::KANNALA_BRANDT) { - intr_left_ = api_->GetIntrinsicsBase(Stream::LEFT); - intr_right_ = api_->GetIntrinsicsBase(Stream::RIGHT); - extr_ = std::make_shared( - api_->GetExtrinsics(Stream::LEFT, Stream::RIGHT)); + } else if (calib_model_ == CalibrationModel::KANNALA_BRANDT) { auto &&processor = find_processor(processor_); if (processor) processor->ReloadImageParams(intr_left_, intr_right_, extr_); #endif diff --git a/src/mynteye/api/synthetic.h b/src/mynteye/api/synthetic.h index 1777d38..3625a9f 100644 --- a/src/mynteye/api/synthetic.h +++ b/src/mynteye/api/synthetic.h @@ -44,7 +44,7 @@ class Synthetic { explicit Synthetic(API *api, CalibrationModel calib_model); ~Synthetic(); - void NotifyImageParamsChanged(bool is_from_dev = false); + void NotifyImageParamsChanged(); bool Supports(const Stream &stream) const; mode_t SupportsMode(const Stream &stream) const; @@ -120,6 +120,7 @@ class Synthetic { std::shared_ptr intr_left_; std::shared_ptr intr_right_; std::shared_ptr extr_; + bool calib_default_tag_; }; template From b114dc8159961ebd327d4e07f8fd77038d9e8ee2 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Mon, 14 Jan 2019 17:25:41 +0800 Subject: [PATCH 05/12] fix(cmake): fix include ceres and move definitions to mynteye.h --- CMakeLists.txt | 20 ++++++--- cmake/DetectGLog.cmake | 26 ------------ cmake/DetectOpenCV.cmake | 7 ++-- cmake/Option.cmake | 14 +------ include/mynteye/logger.h | 13 ++++++ include/mynteye/mynteye.h.in | 15 +++++++ mynteye-config.cmake.in | 3 +- .../src/mynt_eye_ros_wrapper/CMakeLists.txt | 5 +-- .../cmake/DetectOpenCV.cmake | 42 ------------------- .../cmake/IncludeGuard.cmake | 23 ---------- 10 files changed, 49 insertions(+), 119 deletions(-) delete mode 100644 cmake/DetectGLog.cmake delete mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/cmake/DetectOpenCV.cmake delete mode 100644 wrappers/ros/src/mynt_eye_ros_wrapper/cmake/IncludeGuard.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index b491d07..21f39af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -141,7 +141,7 @@ if(WITH_CAM_MODELS) message(STATUS "EIGEN_INCLUDE_DIRS: ${EIGEN_INCLUDE_DIRS}") include_directories( - ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS} src/mynteye/api/camodocal/include ) @@ -245,7 +245,7 @@ if(WITH_API) endif() endif() -if(NOT WITH_GLOG) +if(NOT WITH_GLOG AND NOT WITH_CAM_MODELS) list(APPEND MYNTEYE_SRCS src/mynteye/miniglog.cc) endif() @@ -268,8 +268,7 @@ if(OS_MAC) target_link_libraries( ${MYNTEYE_NAME} ${OSX_EXTRA_LIBS} ) endif() if(WITH_CAM_MODELS) -# link_directories("./_output/lib") -target_link_libraries(${MYNTEYE_NAME} "${OUT_DIR}/lib/libcamodocal.a") + target_link_libraries(${MYNTEYE_NAME} camodocal) endif() target_link_threads(${MYNTEYE_NAME}) @@ -333,6 +332,12 @@ if(NOT WITH_GLOG) DESTINATION ${MYNTEYE_CMAKE_INCLUDE_DIR} ) endif() +if(WITH_CAM_MODELS) + install(FILES + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/ceres/include/ceres/internal/miniglog/glog/logging.h + DESTINATION ${MYNTEYE_CMAKE_INCLUDE_DIR}/glog + ) +endif() install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/deprecated/mynteye/callbacks.h @@ -354,7 +359,12 @@ if(WITH_API) ) endif() -install(TARGETS ${MYNTEYE_NAME} +set(__install_targets ${MYNTEYE_NAME}) +if(WITH_CAM_MODELS) + list(APPEND __install_targets camodocal) +endif() + +install(TARGETS ${__install_targets} EXPORT ${MYNTEYE_NAME}-targets RUNTIME DESTINATION ${MYNTEYE_CMAKE_BINDIR} LIBRARY DESTINATION ${MYNTEYE_CMAKE_LIBDIR} diff --git a/cmake/DetectGLog.cmake b/cmake/DetectGLog.cmake deleted file mode 100644 index daf42d7..0000000 --- a/cmake/DetectGLog.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# Copyright 2018 Slightech Co., Ltd. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -include(${CMAKE_CURRENT_LIST_DIR}/IncludeGuard.cmake) -cmake_include_guard() - -get_filename_component(__pro_dir ${CMAKE_CURRENT_LIST_DIR} DIRECTORY) -LIST(APPEND CMAKE_PREFIX_PATH ${__pro_dir}/third_party/glog/_build) - -find_package(glog REQUIRED) -if(glog_FOUND) - add_definitions(-DWITH_GLOG) -endif() - -unset(__pro_dir) diff --git a/cmake/DetectOpenCV.cmake b/cmake/DetectOpenCV.cmake index d166f50..ce15c40 100644 --- a/cmake/DetectOpenCV.cmake +++ b/cmake/DetectOpenCV.cmake @@ -26,14 +26,13 @@ if(OpenCV_FOUND) #message(STATUS "Found OpenCV: ${OpenCV_VERSION}") set(WITH_OPENCV TRUE) -add_definitions(-DWITH_OPENCV) if(OpenCV_VERSION VERSION_LESS 3.0) - add_definitions(-DWITH_OPENCV2) + set(WITH_OPENCV2 TRUE) elseif(OpenCV_VERSION VERSION_LESS 4.0) - add_definitions(-DWITH_OPENCV3) + set(WITH_OPENCV3 TRUE) else() - add_definitions(-DWITH_OPENCV4) + set(WITH_OPENCV4 TRUE) endif() list(FIND OpenCV_LIBS "opencv_world" __index) diff --git a/cmake/Option.cmake b/cmake/Option.cmake index cdbd897..7380672 100644 --- a/cmake/Option.cmake +++ b/cmake/Option.cmake @@ -42,22 +42,12 @@ else() set(WITH_CAM_MODELS OFF) endif() -if(WITH_DEVICE_INFO_REQUIRED) - add_definitions(-DWITH_DEVICE_INFO_REQUIRED) -endif() - -if(WITH_CAM_MODELS) - add_definitions(-DWITH_CAM_MODELS) -endif() - if(WITH_BOOST) find_package(Boost COMPONENTS filesystem) if(Boost_FOUND) set(Boost_VERSION_STRING "${Boost_MAJOR_VERSION}.${Boost_MINOR_VERSION}.${Boost_SUBMINOR_VERSION}") set(WITH_FILESYSTEM TRUE) set(WITH_BOOST_FILESYSTEM TRUE) - add_definitions(-DWITH_FILESYSTEM) - add_definitions(-DWITH_BOOST_FILESYSTEM) endif() endif() @@ -65,13 +55,11 @@ if(NOT WITH_FILESYSTEM) if(MSVC OR MSYS OR MINGW) # win set(WITH_FILESYSTEM TRUE) set(WITH_NATIVE_FILESYSTEM TRUE) - add_definitions(-DWITH_FILESYSTEM) - add_definitions(-DWITH_NATIVE_FILESYSTEM) endif() endif() if(WITH_GLOG) - include(${CMAKE_CURRENT_LIST_DIR}/DetectGLog.cmake) + find_package(glog REQUIRED) endif() find_package(CUDA QUIET) diff --git a/include/mynteye/logger.h b/include/mynteye/logger.h index 83a7642..91b83fb 100644 --- a/include/mynteye/logger.h +++ b/include/mynteye/logger.h @@ -74,10 +74,23 @@ struct glog_init { } }; +#include "mynteye/mynteye.h" + +#ifdef WITH_CAM_MODELS + +#define MAX_LOG_LEVEL google::INFO + +// include ceres miniglog +#include "glog/logging.h" + +#else + #define MYNTEYE_MAX_LOG_LEVEL google::INFO #include "mynteye/miniglog.h" #endif +#endif + #endif // MYNTEYE_LOGGER_H_ diff --git a/include/mynteye/mynteye.h.in b/include/mynteye/mynteye.h.in index ee58cdf..bd2fafb 100644 --- a/include/mynteye/mynteye.h.in +++ b/include/mynteye/mynteye.h.in @@ -68,4 +68,19 @@ void UNUSED(T &&...) {} MYNTEYE_END_NAMESPACE +#cmakedefine WITH_API +#cmakedefine WITH_DEVICE_INFO_REQUIRED +#cmakedefine WITH_CAM_MODELS + +#cmakedefine WITH_OPENCV +#cmakedefine WITH_OPENCV2 +#cmakedefine WITH_OPENCV3 +#cmakedefine WITH_OPENCV4 +#cmakedefine WITH_OPENCV_WORLD + +#cmakedefine WITH_GLOG +#cmakedefine WITH_FILESYSTEM +#cmakedefine WITH_BOOST_FILESYSTEM +#cmakedefine WITH_NATIVE_FILESYSTEM + #endif // MYNTEYE_MYNTEYE_H_ diff --git a/mynteye-config.cmake.in b/mynteye-config.cmake.in index f9f666a..d064911 100644 --- a/mynteye-config.cmake.in +++ b/mynteye-config.cmake.in @@ -13,10 +13,9 @@ # limitations under the License. @PACKAGE_INIT@ -set(mynteye_WITH_API @WITH_API@) -set(mynteye_WITH_GLOG @WITH_GLOG@) set(mynteye_WITH_API @WITH_API@) set(mynteye_WITH_GLOG @WITH_GLOG@) +set(mynteye_WITH_CAM_MODELS @WITH_CAM_MODELS@) include("${CMAKE_CURRENT_LIST_DIR}/mynteye-targets.cmake") diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt index dc8129f..41ad760 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/CMakeLists.txt @@ -83,13 +83,10 @@ if(NOT mynteye_WITH_API) message(FATAL_ERROR "Must with API layer :(") endif() -include(cmake/DetectOpenCV.cmake) +find_package(OpenCV REQUIRED) if(mynteye_WITH_GLOG) find_package(glog REQUIRED) - if(glog_FOUND) - add_definitions(-DWITH_GLOG) - endif() endif() # targets diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/cmake/DetectOpenCV.cmake b/wrappers/ros/src/mynt_eye_ros_wrapper/cmake/DetectOpenCV.cmake deleted file mode 100644 index 4a9ef81..0000000 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/cmake/DetectOpenCV.cmake +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright 2018 Slightech Co., Ltd. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -include(${CMAKE_CURRENT_LIST_DIR}/IncludeGuard.cmake) -cmake_include_guard() - -find_package(OpenCV REQUIRED) - -message(STATUS "Found OpenCV: ${OpenCV_VERSION}") - -set(WITH_OPENCV TRUE) -add_definitions(-DWITH_OPENCV) - -if(OpenCV_VERSION VERSION_LESS 3.0) - add_definitions(-DWITH_OPENCV2) -elseif(OpenCV_VERSION VERSION_LESS 4.0) - add_definitions(-DWITH_OPENCV3) -else() - add_definitions(-DWITH_OPENCV4) -endif() - -list(FIND OpenCV_LIBS "opencv_world" __index) -if(${__index} GREATER -1) - set(WITH_OPENCV_WORLD TRUE) -endif() - -if(MSVC OR MSYS OR MINGW) - get_filename_component(OpenCV_LIB_SEARCH_PATH "${OpenCV_LIB_PATH}/../bin" ABSOLUTE) -else() - set(OpenCV_LIB_SEARCH_PATH "${OpenCV_LIB_PATH}") -endif() diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/cmake/IncludeGuard.cmake b/wrappers/ros/src/mynt_eye_ros_wrapper/cmake/IncludeGuard.cmake deleted file mode 100644 index 4c5dd68..0000000 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/cmake/IncludeGuard.cmake +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2018 Slightech Co., Ltd. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# include_guard: https://cmake.org/cmake/help/latest/command/include_guard.html - -macro(cmake_include_guard) - get_property(INCLUDE_GUARD GLOBAL PROPERTY "_INCLUDE_GUARD_${CMAKE_CURRENT_LIST_FILE}") - if(INCLUDE_GUARD) - return() - endif() - set_property(GLOBAL PROPERTY "_INCLUDE_GUARD_${CMAKE_CURRENT_LIST_FILE}" TRUE) -endmacro() From 7985a8ffe7abe207a0750542dc68517cab606e91 Mon Sep 17 00:00:00 2001 From: TinyOh Date: Mon, 14 Jan 2019 17:28:57 +0800 Subject: [PATCH 06/12] fix(calib models): use shared ptr value change instead of ptr change --- src/mynteye/api/processor/rectify_processor.cc | 14 +++++++++++--- src/mynteye/api/processor/rectify_processor.h | 4 ---- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index cf34e73..6a130bc 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -339,13 +339,21 @@ void RectifyProcessor::InitParams( generateCameraFromIntrinsicsEquidistant(in_left); camodocal::CameraPtr camera_odo_ptr_right = generateCameraFromIntrinsicsEquidistant(in_right); - calib_infos = - stereoRectify(camera_odo_ptr_left, + if (calib_infos) { + auto calib_info_tmp = stereoRectify(camera_odo_ptr_left, + camera_odo_ptr_right, + in_left, + in_right, + ex_right_to_left); + *calib_infos = *calib_info_tmp; + } else { + calib_infos = + stereoRectify(camera_odo_ptr_left, camera_odo_ptr_right, in_left, in_right, ex_right_to_left); - + } cv::Mat rect_R_l = cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); for (size_t i = 0; i < 3; i++) { diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 5929dfc..147bc1e 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -23,11 +23,7 @@ #include "mynteye/types.h" #include "mynteye/api/processor.h" #include "mynteye/device/device.h" -#include -#include #include -#include -#include #include #include #include From e56bf190f6df8a0af3b1c90b0e2008c8ae68309f Mon Sep 17 00:00:00 2001 From: TinyOh Date: Mon, 14 Jan 2019 17:48:24 +0800 Subject: [PATCH 07/12] fix(calib models): calib struct update --- src/mynteye/api/processor/rectify_processor.cc | 14 +++----------- 1 file changed, 3 insertions(+), 11 deletions(-) diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 6a130bc..e668eca 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -339,21 +339,12 @@ void RectifyProcessor::InitParams( generateCameraFromIntrinsicsEquidistant(in_left); camodocal::CameraPtr camera_odo_ptr_right = generateCameraFromIntrinsicsEquidistant(in_right); - if (calib_infos) { - auto calib_info_tmp = stereoRectify(camera_odo_ptr_left, + auto calib_info_tmp = stereoRectify(camera_odo_ptr_left, camera_odo_ptr_right, in_left, in_right, ex_right_to_left); - *calib_infos = *calib_info_tmp; - } else { - calib_infos = - stereoRectify(camera_odo_ptr_left, - camera_odo_ptr_right, - in_left, - in_right, - ex_right_to_left); - } + *calib_infos = *calib_info_tmp; cv::Mat rect_R_l = cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); for (size_t i = 0; i < 3; i++) { @@ -390,6 +381,7 @@ RectifyProcessor::RectifyProcessor( : Processor(std::move(proc_period)), calib_model(CalibrationModel::UNKNOW) { VLOG(2) << __func__ << ": proc_period=" << proc_period; + calib_infos = std::make_shared(); InitParams( *std::dynamic_pointer_cast(intr_left), *std::dynamic_pointer_cast(intr_right), From 56d9ab6e2d59146ffe9a5b473a8ac2bdb5ce610b Mon Sep 17 00:00:00 2001 From: kalman Date: Mon, 14 Jan 2019 18:51:06 +0800 Subject: [PATCH 08/12] fix(rectify): delete useless log --- src/mynteye/api/processor/depth_processor.cc | 4 ++-- src/mynteye/api/processor/points_processor.cc | 2 +- src/mynteye/api/processor/rectify_processor.cc | 17 +++++++++++++++-- src/mynteye/api/processor/rectify_processor.h | 4 ---- 4 files changed, 18 insertions(+), 9 deletions(-) diff --git a/src/mynteye/api/processor/depth_processor.cc b/src/mynteye/api/processor/depth_processor.cc index 4107065..ae8f5e5 100644 --- a/src/mynteye/api/processor/depth_processor.cc +++ b/src/mynteye/api/processor/depth_processor.cc @@ -29,7 +29,7 @@ DepthProcessor::DepthProcessor( std::int32_t proc_period) : Processor(std::move(proc_period)), calib_infos_(calib_infos) { - VLOG(2) << __func__ << ": proc_period=" << proc_period; + VLOG(2) << __func__; } DepthProcessor::~DepthProcessor() { @@ -51,7 +51,7 @@ bool DepthProcessor::OnProcess( ObjMat *output = Object::Cast(out); int rows = input->value.rows; int cols = input->value.cols; - // std::cout << calib_infos_->T_mul_f << std::endl; + std::cout << calib_infos_->T_mul_f << std::endl; // 0.0793434 cv::Mat depth_mat = cv::Mat::zeros(rows, cols, CV_16U); for (int i = 0; i < rows; i++) { diff --git a/src/mynteye/api/processor/points_processor.cc b/src/mynteye/api/processor/points_processor.cc index 162f7f7..fd8f204 100644 --- a/src/mynteye/api/processor/points_processor.cc +++ b/src/mynteye/api/processor/points_processor.cc @@ -57,7 +57,7 @@ PointsProcessor::PointsProcessor( std::int32_t proc_period) : Processor(std::move(proc_period)), calib_infos_(calib_infos) { - VLOG(2) << __func__ << ": proc_period=" << proc_period; + VLOG(2) << __func__; } PointsProcessor::~PointsProcessor() { diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index cf34e73..30a2442 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -339,13 +339,22 @@ void RectifyProcessor::InitParams( generateCameraFromIntrinsicsEquidistant(in_left); camodocal::CameraPtr camera_odo_ptr_right = generateCameraFromIntrinsicsEquidistant(in_right); - calib_infos = +<<<<<<< HEAD + auto calib_infos_temp = stereoRectify(camera_odo_ptr_left, camera_odo_ptr_right, in_left, in_right, ex_right_to_left); - + *calib_infos = *calib_infos_temp; +======= + auto calib_info_tmp = stereoRectify(camera_odo_ptr_left, + camera_odo_ptr_right, + in_left, + in_right, + ex_right_to_left); + *calib_infos = *calib_info_tmp; +>>>>>>> e56bf190f6df8a0af3b1c90b0e2008c8ae68309f cv::Mat rect_R_l = cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); for (size_t i = 0; i < 3; i++) { @@ -381,7 +390,11 @@ RectifyProcessor::RectifyProcessor( std::int32_t proc_period) : Processor(std::move(proc_period)), calib_model(CalibrationModel::UNKNOW) { +<<<<<<< HEAD +======= VLOG(2) << __func__ << ": proc_period=" << proc_period; +>>>>>>> e56bf190f6df8a0af3b1c90b0e2008c8ae68309f + calib_infos = std::make_shared(); InitParams( *std::dynamic_pointer_cast(intr_left), *std::dynamic_pointer_cast(intr_right), diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index 5929dfc..147bc1e 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -23,11 +23,7 @@ #include "mynteye/types.h" #include "mynteye/api/processor.h" #include "mynteye/device/device.h" -#include -#include #include -#include -#include #include #include #include From 13833ce2b0f852ca476c6bb21e8bab21e428c7ac Mon Sep 17 00:00:00 2001 From: kalman Date: Mon, 14 Jan 2019 18:55:53 +0800 Subject: [PATCH 09/12] fix(dataset): fix record bug --- tools/dataset/dataset.cc | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/tools/dataset/dataset.cc b/tools/dataset/dataset.cc index 7d2065d..eab794f 100644 --- a/tools/dataset/dataset.cc +++ b/tools/dataset/dataset.cc @@ -95,15 +95,13 @@ void Dataset::SaveMotionData(const device::MotionData &data) { auto &&writer = GetMotionWriter(); // auto seq = data.imu->serial_number; auto seq = motion_count_; - if (data.imu->flag == 1 || data.imu->flag == 2) { - writer->ofs << seq << ", " << static_cast(data.imu->flag) << ", " - << data.imu->timestamp << ", " << data.imu->accel[0] << ", " - << data.imu->accel[1] << ", " << data.imu->accel[2] << ", " - << data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", " - << data.imu->gyro[2] << ", " << data.imu->temperature - << std::endl; - ++motion_count_; - } + writer->ofs << seq << ", " << static_cast(data.imu->flag) << ", " + << data.imu->timestamp << ", " << data.imu->accel[0] << ", " + << data.imu->accel[1] << ", " << data.imu->accel[2] << ", " + << data.imu->gyro[0] << ", " << data.imu->gyro[1] << ", " + << data.imu->gyro[2] << ", " << data.imu->temperature + << std::endl; + ++motion_count_; /* if(motion_count_ != seq) { LOG(INFO) << "motion_count_ != seq !" << " motion_count_: " << motion_count_ From feabcf294fd9c70edf35c3ac12786ce8334bd191 Mon Sep 17 00:00:00 2001 From: kalman Date: Mon, 14 Jan 2019 19:05:16 +0800 Subject: [PATCH 10/12] fix(ros): fix record -a bug --- .../src/mynt_eye_ros_wrapper/launch/mynteye.launch | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch index 17c38d5..dd1048b 100644 --- a/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch +++ b/wrappers/ros/src/mynt_eye_ros_wrapper/launch/mynteye.launch @@ -310,7 +310,7 @@ - 'image_transport/compressedDepth' - + - 'image_transport/compressedDepth' @@ -320,6 +320,16 @@ - 'image_transport/compressedDepth' + + + - 'image_transport/compressedDepth' + + + + + - 'image_transport/compressedDepth' + + - 'image_transport/compressedDepth' From c82a29968d02333e64a83aeb9fa0dbe723602a8d Mon Sep 17 00:00:00 2001 From: TinyOh Date: Mon, 14 Jan 2019 20:08:14 +0800 Subject: [PATCH 11/12] feat(bm model): add bm model --- .../api/processor/disparity_processor.cc | 112 +++++++++++++----- .../api/processor/disparity_processor.h | 21 +++- src/mynteye/api/synthetic.cc | 3 +- 3 files changed, 99 insertions(+), 37 deletions(-) diff --git a/src/mynteye/api/processor/disparity_processor.cc b/src/mynteye/api/processor/disparity_processor.cc index 1c4f939..dff775d 100644 --- a/src/mynteye/api/processor/disparity_processor.cc +++ b/src/mynteye/api/processor/disparity_processor.cc @@ -16,6 +16,7 @@ #include #include +#include #include "mynteye/logger.h" @@ -23,41 +24,76 @@ MYNTEYE_BEGIN_NAMESPACE const char DisparityProcessor::NAME[] = "DisparityProcessor"; -DisparityProcessor::DisparityProcessor(std::int32_t proc_period) - : Processor(std::move(proc_period)) { +DisparityProcessor::DisparityProcessor(DisparityProcessorType type, + std::int32_t proc_period) + : Processor(std::move(proc_period)), type_(type) { VLOG(2) << __func__ << ": proc_period=" << proc_period; - int sgbmWinSize = 3; - int numberOfDisparities = 64; + + if (type_ == DisparityProcessorType::SGBM) { + int sgbmWinSize = 3; + int numberOfDisparities = 64; #ifdef WITH_OPENCV2 - // StereoSGBM - // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm - sgbm_ = cv::Ptr( - new cv::StereoSGBM( - 0, // minDisparity - numberOfDisparities, // numDisparities - sgbmWinSize, // SADWindowSize - 8 * sgbmWinSize * sgbmWinSize, // P1 - 32 * sgbmWinSize * sgbmWinSize, // P2 - 1, // disp12MaxDiff - 63, // preFilterCap - 10, // uniquenessRatio - 100, // speckleWindowSize - 32, // speckleRange - false)); // fullDP + // StereoSGBM + // http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?#stereosgbm + sgbm_matcher = cv::Ptr( + new cv::StereoSGBM( + 0, // minDisparity + numberOfDisparities, // numDisparities + sgbmWinSize, // SADWindowSize + 8 * sgbmWinSize * sgbmWinSize, // P1 + 32 * sgbmWinSize * sgbmWinSize, // P2 + 1, // disp12MaxDiff + 63, // preFilterCap + 10, // uniquenessRatio + 100, // speckleWindowSize + 32, // speckleRange + false)); // fullDP #else - sgbm_ = cv::StereoSGBM::create(0, 16, 3); - sgbm_->setPreFilterCap(63); - sgbm_->setBlockSize(sgbmWinSize); - sgbm_->setP1(8 * sgbmWinSize * sgbmWinSize); - sgbm_->setP2(32 * sgbmWinSize * sgbmWinSize); - sgbm_->setMinDisparity(0); - sgbm_->setNumDisparities(numberOfDisparities); - sgbm_->setUniquenessRatio(10); - sgbm_->setSpeckleWindowSize(100); - sgbm_->setSpeckleRange(32); - sgbm_->setDisp12MaxDiff(1); + sgbm_matcher = cv::StereoSGBM::create(0, 16, 3); + sgbm_matcher->setPreFilterCap(63); + sgbm_matcher->setBlockSize(sgbmWinSize); + sgbm_matcher->setP1(8 * sgbmWinSize * sgbmWinSize); + sgbm_matcher->setP2(32 * sgbmWinSize * sgbmWinSize); + sgbm_matcher->setMinDisparity(0); + sgbm_matcher->setNumDisparities(numberOfDisparities); + sgbm_matcher->setUniquenessRatio(10); + sgbm_matcher->setSpeckleWindowSize(100); + sgbm_matcher->setSpeckleRange(32); + sgbm_matcher->setDisp12MaxDiff(1); #endif + } else if (type_ == DisparityProcessorType::BM) { +#ifdef WITH_OPENCV2 + int bmWinSize = 3; + // StereoBM + // https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#stereobm-stereobm + bm_matcher = cv::Ptr(new cv::StereoBM( + int 0, + 64, + 100, + 8 * bmWinSize * bmWinSize, + 32 * bmWinSize * bmWinSize, + int -1, + int 31, + 15, + 100, + 4)); +#else + bm_matcher = cv::StereoBM::create(0, 3); + bm_matcher->setPreFilterSize(9); + bm_matcher->setPreFilterCap(31); + bm_matcher->setBlockSize(15); + bm_matcher->setMinDisparity(0); + bm_matcher->setNumDisparities(64); + bm_matcher->setUniquenessRatio(15); + bm_matcher->setTextureThreshold(10); + bm_matcher->setSpeckleWindowSize(100); + bm_matcher->setSpeckleRange(4); + bm_matcher->setPreFilterType(cv::StereoBM::PREFILTER_XSOBEL); +#endif + } else { + LOG(ERROR) << "no enum DisparityProcessorType" << static_cast(type); + } } DisparityProcessor::~DisparityProcessor() { @@ -87,7 +123,11 @@ bool DisparityProcessor::OnProcess( // It contains disparity values scaled by 16. So, to get the floating-point // disparity map, // you need to divide each disp element by 16. - (*sgbm_)(input->first, input->second, disparity); + if (type_ == SGBM) { + (*sgbm_matcher)(input->first, input->second, disparity); + } else if (type_ == BM) { + (*bm_matcher)(input->first, input->second, disparity); + } #else // compute() // http://docs.opencv.org/master/d2/d6e/classcv_1_1StereoMatcher.html @@ -96,7 +136,15 @@ bool DisparityProcessor::OnProcess( // disparity map // (where each disparity value has 4 fractional bits), // whereas other algorithms output 32-bit floating-point disparity map. - sgbm_->compute(input->first, input->second, disparity); + if (type_ == DisparityProcessorType::SGBM) { + sgbm_matcher->compute(input->first, input->second, disparity); + } else if (type_ == DisparityProcessorType::BM) { + CvSize size = input->first.size(); + cv::Mat tmp1, tmp2; + cv::cvtColor(input->first, tmp1, CV_RGB2GRAY); + cv::cvtColor(input->second, tmp2, CV_RGB2GRAY); + bm_matcher->compute(tmp1, tmp2, disparity); + } #endif disparity.convertTo(output->value, CV_32F, 1./16, 1); output->id = input->first_id; diff --git a/src/mynteye/api/processor/disparity_processor.h b/src/mynteye/api/processor/disparity_processor.h index 370dca3..f7fd952 100644 --- a/src/mynteye/api/processor/disparity_processor.h +++ b/src/mynteye/api/processor/disparity_processor.h @@ -16,22 +16,33 @@ #pragma once #include - +#include #include "mynteye/api/processor.h" namespace cv { class StereoSGBM; - +class StereoBM; } // namespace cv +enum class DisparityProcessorType : std::uint8_t { + /** bm */ + SGBM = 0, + /** Equidistant: KANNALA_BRANDT */ + BM = 1, + /** Unknow */ + UNKNOW +}; + + MYNTEYE_BEGIN_NAMESPACE class DisparityProcessor : public Processor { public: static const char NAME[]; - explicit DisparityProcessor(std::int32_t proc_period = 0); + explicit DisparityProcessor(DisparityProcessorType type, + std::int32_t proc_period = 0); virtual ~DisparityProcessor(); std::string Name() override; @@ -42,7 +53,9 @@ class DisparityProcessor : public Processor { Object *const in, Object *const out, Processor *const parent) override; private: - cv::Ptr sgbm_; + cv::Ptr sgbm_matcher; + cv::Ptr bm_matcher; + DisparityProcessorType type_; }; MYNTEYE_END_NAMESPACE diff --git a/src/mynteye/api/synthetic.cc b/src/mynteye/api/synthetic.cc index 02810fa..ccd3c11 100644 --- a/src/mynteye/api/synthetic.cc +++ b/src/mynteye/api/synthetic.cc @@ -613,7 +613,8 @@ void Synthetic::InitProcessors() { rectify_processor = rectify_processor_ocv; } auto &&disparity_processor = - std::make_shared(DISPARITY_PROC_PERIOD); + std::make_shared(DisparityProcessorType::BM, + DISPARITY_PROC_PERIOD); auto &&disparitynormalized_processor = std::make_shared( DISPARITY_NORM_PROC_PERIOD); From 050d0ce703d92da2c320b21335f09d634f844fc4 Mon Sep 17 00:00:00 2001 From: John Zhao Date: Mon, 14 Jan 2019 22:27:38 +0800 Subject: [PATCH 12/12] fix(cmake): fix inlcude ceres miniglog --- 3rdparty/glog/logging.h | 427 ++++++++++++++++++++++++++++++++++++++++ CMakeLists.txt | 17 +- 2 files changed, 433 insertions(+), 11 deletions(-) create mode 100644 3rdparty/glog/logging.h diff --git a/3rdparty/glog/logging.h b/3rdparty/glog/logging.h new file mode 100644 index 0000000..b67bf96 --- /dev/null +++ b/3rdparty/glog/logging.h @@ -0,0 +1,427 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: settinger@google.com (Scott Ettinger) +// mierle@gmail.com (Keir Mierle) +// +// Simplified Glog style logging with Android support. Supported macros in +// decreasing severity level per line: +// +// VLOG(2), VLOG(N) +// VLOG(1), +// LOG(INFO), VLOG(0), LG +// LOG(WARNING), +// LOG(ERROR), +// LOG(FATAL), +// +// With VLOG(n), the output is directed to one of the 5 Android log levels: +// +// 2 - Verbose +// 1 - Debug +// 0 - Info +// -1 - Warning +// -2 - Error +// -3 - Fatal +// +// Any logging of level 2 and above is directed to the Verbose level. All +// Android log output is tagged with the string "native". +// +// If the symbol ANDROID is not defined, all output goes to std::cerr. +// This allows code to be built on a different system for debug. +// +// Portions of this code are taken from the GLOG package. This code is only a +// small subset of the GLOG functionality. Notable differences from GLOG +// behavior include lack of support for displaying unprintable characters and +// lack of stack trace information upon failure of the CHECK macros. On +// non-Android systems, log output goes to std::cerr and is not written to a +// file. +// +// CHECK macros are defined to test for conditions within code. Any CHECK that +// fails will log the failure and terminate the application. +// e.g. CHECK_GE(3, 2) will pass while CHECK_GE(3, 4) will fail after logging +// "Check failed 3 >= 4". +// +// The following CHECK macros are defined: +// +// CHECK(condition) - fails if condition is false and logs condition. +// CHECK_NOTNULL(variable) - fails if the variable is NULL. +// +// The following binary check macros are also defined : +// +// Macro Operator equivalent +// -------------------- ------------------- +// CHECK_EQ(val1, val2) val1 == val2 +// CHECK_NE(val1, val2) val1 != val2 +// CHECK_GT(val1, val2) val1 > val2 +// CHECK_GE(val1, val2) val1 >= val2 +// CHECK_LT(val1, val2) val1 < val2 +// CHECK_LE(val1, val2) val1 <= val2 +// +// Debug only versions of all of the check macros are also defined. These +// macros generate no code in a release build, but avoid unused variable +// warnings / errors. +// +// To use the debug only versions, prepend a D to the normal check macros, e.g. +// DCHECK_EQ(a, b). + +#ifndef CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_ +#define CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_ + +#ifdef ANDROID +# include +#endif // ANDROID + +#include +#include +#include +#include +#include +#include +#include +#include + +// For appropriate definition of CERES_EXPORT macro. +// #include "ceres/internal/port.h" +// #include "ceres/internal/disable_warnings.h" +#include "mynteye/mynteye.h" + +// Log severity level constants. +const int FATAL = -3; +const int ERROR = -2; +const int WARNING = -1; +const int INFO = 0; + +// ------------------------- Glog compatibility ------------------------------ + +namespace google { + +typedef int LogSeverity; +const int INFO = ::INFO; +const int WARNING = ::WARNING; +const int ERROR = ::ERROR; +const int FATAL = ::FATAL; + +// Sink class used for integration with mock and test functions. If sinks are +// added, all log output is also sent to each sink through the send function. +// In this implementation, WaitTillSent() is called immediately after the send. +// This implementation is not thread safe. +class MYNTEYE_API LogSink { + public: + virtual ~LogSink() {} + virtual void send(LogSeverity severity, + const char* full_filename, + const char* base_filename, + int line, + const struct tm* tm_time, + const char* message, + size_t message_len) = 0; + virtual void WaitTillSent() = 0; +}; + +// Global set of log sinks. The actual object is defined in logging.cc. +extern MYNTEYE_API std::set log_sinks_global; + +inline void InitGoogleLogging(char *argv) { + // Do nothing; this is ignored. +} + +// Note: the Log sink functions are not thread safe. +inline void AddLogSink(LogSink *sink) { + // TODO(settinger): Add locks for thread safety. + log_sinks_global.insert(sink); +} +inline void RemoveLogSink(LogSink *sink) { + log_sinks_global.erase(sink); +} + +} // namespace google + +// ---------------------------- Logger Class -------------------------------- + +// Class created for each use of the logging macros. +// The logger acts as a stream and routes the final stream contents to the +// Android logcat output at the proper filter level. If ANDROID is not +// defined, output is directed to std::cerr. This class should not +// be directly instantiated in code, rather it should be invoked through the +// use of the log macros LG, LOG, or VLOG. +class MYNTEYE_API MessageLogger { + public: + MessageLogger(const char *file, int line, const char *tag, int severity) + : file_(file), line_(line), tag_(tag), severity_(severity) { + // Pre-pend the stream with the file and line number. + StripBasename(std::string(file), &filename_only_); + stream_ << filename_only_ << ":" << line << " "; + } + + // Output the contents of the stream to the proper channel on destruction. + ~MessageLogger() { + stream_ << "\n"; + +#ifdef ANDROID + static const int android_log_levels[] = { + ANDROID_LOG_FATAL, // LOG(FATAL) + ANDROID_LOG_ERROR, // LOG(ERROR) + ANDROID_LOG_WARN, // LOG(WARNING) + ANDROID_LOG_INFO, // LOG(INFO), LG, VLOG(0) + ANDROID_LOG_DEBUG, // VLOG(1) + ANDROID_LOG_VERBOSE, // VLOG(2) .. VLOG(N) + }; + + // Bound the logging level. + const int kMaxVerboseLevel = 2; + int android_level_index = std::min(std::max(FATAL, severity_), + kMaxVerboseLevel) - FATAL; + int android_log_level = android_log_levels[android_level_index]; + + // Output the log string the Android log at the appropriate level. + __android_log_write(android_log_level, tag_.c_str(), stream_.str().c_str()); + + // Indicate termination if needed. + if (severity_ == FATAL) { + __android_log_write(ANDROID_LOG_FATAL, + tag_.c_str(), + "terminating.\n"); + } +#else + // If not building on Android, log all output to std::cerr. + std::cerr << stream_.str(); +#endif // ANDROID + + LogToSinks(severity_); + WaitForSinks(); + + // Android logging at level FATAL does not terminate execution, so abort() + // is still required to stop the program. + if (severity_ == FATAL) { + abort(); + } + } + + // Return the stream associated with the logger object. + std::stringstream &stream() { return stream_; } + + private: + void LogToSinks(int severity) { + time_t rawtime; + time (&rawtime); + + struct tm* timeinfo; +#if defined(WIN32) || defined(_WIN32) || defined(__WIN32__) + // On Windows, use secure localtime_s not localtime. + struct tm windows_timeinfo; + timeinfo = &windows_timeinfo; + localtime_s(timeinfo, &rawtime); +#else + timeinfo = localtime(&rawtime); +#endif + + std::set::iterator iter; + // Send the log message to all sinks. + for (iter = google::log_sinks_global.begin(); + iter != google::log_sinks_global.end(); ++iter) { + (*iter)->send(severity, file_.c_str(), filename_only_.c_str(), line_, + timeinfo, stream_.str().c_str(), stream_.str().size()); + } + } + + void WaitForSinks() { + // TODO(settinger): Add locks for thread safety. + std::set::iterator iter; + + // Call WaitTillSent() for all sinks. + for (iter = google::log_sinks_global.begin(); + iter != google::log_sinks_global.end(); ++iter) { + (*iter)->WaitTillSent(); + } + } + + void StripBasename(const std::string &full_path, std::string *filename) { + // TODO(settinger): Add support for OSs with different path separators. + const char kSeparator = '/'; + size_t pos = full_path.rfind(kSeparator); + if (pos != std::string::npos) { + *filename = full_path.substr(pos + 1, std::string::npos); + } else { + *filename = full_path; + } + } + + std::string file_; + std::string filename_only_; + int line_; + std::string tag_; + std::stringstream stream_; + int severity_; +}; + +// ---------------------- Logging Macro definitions -------------------------- + +// This class is used to explicitly ignore values in the conditional +// logging macros. This avoids compiler warnings like "value computed +// is not used" and "statement has no effect". +class MYNTEYE_API LoggerVoidify { + public: + LoggerVoidify() { } + // This has to be an operator with a precedence lower than << but + // higher than ?: + void operator&(const std::ostream &s) { } +}; + +// Log only if condition is met. Otherwise evaluates to void. +#define LOG_IF(severity, condition) \ + !(condition) ? (void) 0 : LoggerVoidify() & \ + MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream() + +// Log only if condition is NOT met. Otherwise evaluates to void. +#define LOG_IF_FALSE(severity, condition) LOG_IF(severity, !(condition)) + +// LG is a convenient shortcut for LOG(INFO). Its use is in new +// google3 code is discouraged and the following shortcut exists for +// backward compatibility with existing code. +#ifdef MAX_LOG_LEVEL +# define LOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL) +# define VLOG(n) LOG_IF(n, n <= MAX_LOG_LEVEL) +# define LG LOG_IF(INFO, INFO <= MAX_LOG_LEVEL) +# define VLOG_IF(n, condition) LOG_IF(n, (n <= MAX_LOG_LEVEL) && condition) +#else +# define LOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream() // NOLINT +# define VLOG(n) MessageLogger((char *)__FILE__, __LINE__, "native", n).stream() // NOLINT +# define LG MessageLogger((char *)__FILE__, __LINE__, "native", INFO).stream() // NOLINT +# define VLOG_IF(n, condition) LOG_IF(n, condition) +#endif + +// Currently, VLOG is always on for levels below MAX_LOG_LEVEL. +#ifndef MAX_LOG_LEVEL +# define VLOG_IS_ON(x) (1) +#else +# define VLOG_IS_ON(x) (x <= MAX_LOG_LEVEL) +#endif + +#ifndef NDEBUG +# define DLOG LOG +#else +# define DLOG(severity) true ? (void) 0 : LoggerVoidify() & \ + MessageLogger((char *)__FILE__, __LINE__, "native", severity).stream() +#endif + + +// Log a message and terminate. +template +void LogMessageFatal(const char *file, int line, const T &message) { + MessageLogger((char *)__FILE__, __LINE__, "native", FATAL).stream() + << message; +} + +// ---------------------------- CHECK macros --------------------------------- + +// Check for a given boolean condition. +#define CHECK(condition) LOG_IF_FALSE(FATAL, condition) \ + << "Check failed: " #condition " " + +#ifndef NDEBUG +// Debug only version of CHECK +# define DCHECK(condition) LOG_IF_FALSE(FATAL, condition) \ + << "Check failed: " #condition " " +#else +// Optimized version - generates no code. +# define DCHECK(condition) if (false) LOG_IF_FALSE(FATAL, condition) \ + << "Check failed: " #condition " " +#endif // NDEBUG + +// ------------------------- CHECK_OP macros --------------------------------- + +// Generic binary operator check macro. This should not be directly invoked, +// instead use the binary comparison macros defined below. +#define CHECK_OP(val1, val2, op) LOG_IF_FALSE(FATAL, ((val1) op (val2))) \ + << "Check failed: " #val1 " " #op " " #val2 " " + +// Check_op macro definitions +#define CHECK_EQ(val1, val2) CHECK_OP(val1, val2, ==) +#define CHECK_NE(val1, val2) CHECK_OP(val1, val2, !=) +#define CHECK_LE(val1, val2) CHECK_OP(val1, val2, <=) +#define CHECK_LT(val1, val2) CHECK_OP(val1, val2, <) +#define CHECK_GE(val1, val2) CHECK_OP(val1, val2, >=) +#define CHECK_GT(val1, val2) CHECK_OP(val1, val2, >) + +#ifndef NDEBUG +// Debug only versions of CHECK_OP macros. +# define DCHECK_EQ(val1, val2) CHECK_OP(val1, val2, ==) +# define DCHECK_NE(val1, val2) CHECK_OP(val1, val2, !=) +# define DCHECK_LE(val1, val2) CHECK_OP(val1, val2, <=) +# define DCHECK_LT(val1, val2) CHECK_OP(val1, val2, <) +# define DCHECK_GE(val1, val2) CHECK_OP(val1, val2, >=) +# define DCHECK_GT(val1, val2) CHECK_OP(val1, val2, >) +#else +// These versions generate no code in optimized mode. +# define DCHECK_EQ(val1, val2) if (false) CHECK_OP(val1, val2, ==) +# define DCHECK_NE(val1, val2) if (false) CHECK_OP(val1, val2, !=) +# define DCHECK_LE(val1, val2) if (false) CHECK_OP(val1, val2, <=) +# define DCHECK_LT(val1, val2) if (false) CHECK_OP(val1, val2, <) +# define DCHECK_GE(val1, val2) if (false) CHECK_OP(val1, val2, >=) +# define DCHECK_GT(val1, val2) if (false) CHECK_OP(val1, val2, >) +#endif // NDEBUG + +// ---------------------------CHECK_NOTNULL macros --------------------------- + +// Helpers for CHECK_NOTNULL(). Two are necessary to support both raw pointers +// and smart pointers. +template +T& CheckNotNullCommon(const char *file, int line, const char *names, T& t) { + if (t == NULL) { + LogMessageFatal(file, line, std::string(names)); + } + return t; +} + +template +T* CheckNotNull(const char *file, int line, const char *names, T* t) { + return CheckNotNullCommon(file, line, names, t); +} + +template +T& CheckNotNull(const char *file, int line, const char *names, T& t) { + return CheckNotNullCommon(file, line, names, t); +} + +// Check that a pointer is not null. +#define CHECK_NOTNULL(val) \ + CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val)) + +#ifndef NDEBUG +// Debug only version of CHECK_NOTNULL +#define DCHECK_NOTNULL(val) \ + CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val)) +#else +// Optimized version - generates no code. +#define DCHECK_NOTNULL(val) if (false)\ + CheckNotNull(__FILE__, __LINE__, "'" #val "' Must be non NULL", (val)) +#endif // NDEBUG + +// #include "ceres/internal/reenable_warnings.h" + +#endif // CERCES_INTERNAL_MINIGLOG_GLOG_LOGGING_H_ diff --git a/CMakeLists.txt b/CMakeLists.txt index ed9d1ce..0605ead 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -55,7 +55,7 @@ macro(target_link_threads NAME) target_compile_options(PUBLIC ${NAME} "-pthread") endif() if(CMAKE_THREAD_LIBS_INIT) - target_link_libraries(${NAME} "${CMAKE_THREAD_LIBS_INIT}") + target_link_libraries(${NAME} PUBLIC "${CMAKE_THREAD_LIBS_INIT}") endif() endmacro() @@ -264,12 +264,12 @@ endif() #message(STATUS "MYNTEYE_LINKLIBS: ${MYNTEYE_LINKLIBS}") add_library(${MYNTEYE_NAME} SHARED ${MYNTEYE_SRCS}) -target_link_libraries(${MYNTEYE_NAME} ${MYNTEYE_LINKLIBS}) +target_link_libraries(${MYNTEYE_NAME} PUBLIC ${MYNTEYE_LINKLIBS}) if(OS_MAC) - target_link_libraries( ${MYNTEYE_NAME} ${OSX_EXTRA_LIBS} ) + target_link_libraries(${MYNTEYE_NAME} PUBLIC ${OSX_EXTRA_LIBS} ) endif() if(WITH_CAM_MODELS) - target_link_libraries(${MYNTEYE_NAME} camodocal) + target_link_libraries(${MYNTEYE_NAME} PRIVATE camodocal) endif() target_link_threads(${MYNTEYE_NAME}) @@ -335,7 +335,7 @@ if(NOT WITH_GLOG) endif() if(WITH_CAM_MODELS) install(FILES - ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/ceres/include/ceres/internal/miniglog/glog/logging.h + ${CMAKE_CURRENT_SOURCE_DIR}/3rdparty/glog/logging.h DESTINATION ${MYNTEYE_CMAKE_INCLUDE_DIR}/glog ) endif() @@ -360,12 +360,7 @@ if(WITH_API) ) endif() -set(__install_targets ${MYNTEYE_NAME}) -if(WITH_CAM_MODELS) - list(APPEND __install_targets camodocal) -endif() - -install(TARGETS ${__install_targets} +install(TARGETS ${MYNTEYE_NAME} EXPORT ${MYNTEYE_NAME}-targets RUNTIME DESTINATION ${MYNTEYE_CMAKE_BINDIR} LIBRARY DESTINATION ${MYNTEYE_CMAKE_LIBDIR}