diff --git a/.appveyor.yml b/.appveyor.yml new file mode 100644 index 00000000..d57abdbe --- /dev/null +++ b/.appveyor.yml @@ -0,0 +1,39 @@ +version: 1.0.{build} +os: Visual Studio 2017 +platform: x64 +clone_folder: C:\projects\auvlib +#shallow_clone: true +branches: + only: + - master +environment: + matrix: + - CONFIG: Release + BOOST_ROOT: C:/Libraries/boost_1_65_1 + OpenCV_DIR: C:/tools/opencv/build/x64/vc14/lib + PYTHON: 27 +install: + - cd C:\projects\auvlib + - git submodule update --init + #- cinst python2 appveyor already has python + - cinst OpenCV --version 3.4.7 +build: + parallel: true +build_script: + # Tutorials and tests + #- set PATH=C:\Python%PYTHON%-x64;C:\Python%PYTHON%-x64\Scripts;%PATH% + - mkdir build + - cd build + - cmake -DCMAKE_BUILD_TYPE=%CONFIG% + -G "Visual Studio 15 2017 Win64" + -DAUVLIB_WITH_GSF=OFF + -DAUVLIB_USE_LIBIGL_GLFW=ON + -DAUVLIB_USE_LIBIGL_TINYXML=ON + -DAUVLIB_EXPORT_BUILD=OFF + -DCMAKE_SKIP_INSTALL_RULES=true + ../ + #-DCMAKE_INSTALL_PREFIX=../install + - set MSBuildLogger="C:\Program Files\AppVeyor\BuildAgent\Appveyor.MSBuildLogger.dll" + - set MSBuildOptions=/v:m /m /p:BuildInParallel=true /p:Configuration=%CONFIG% /logger:%MSBuildLogger% + #- msbuild %MSBuildOptions% INSTALL.vcxproj + - msbuild %MSBuildOptions% ALL_BUILD.vcxproj diff --git a/.gitmodules b/.gitmodules index 8b4fd1c7..e66a53d9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -11,3 +11,6 @@ [submodule "src/cxxopts"] path = src/cxxopts url = https://github.com/jarro2783/cxxopts.git +[submodule "src/cereal"] + path = src/cereal + url = https://github.com/USCiLab/cereal.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 38cfe106..15c7b2f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,3 +1,4 @@ cmake_minimum_required(VERSION 2.8) +project(auvlib) add_subdirectory(src) diff --git a/README.md b/README.md index aba58b1d..f85928c5 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # auvlib [![Build Status](https://travis-ci.org/nilsbore/auvlib.svg?branch=master)](https://travis-ci.org/nilsbore/auvlib) +[![Build status](https://ci.appveyor.com/api/projects/status/kcfxp0jlpwqxt2fs/branch/master?svg=true)](https://ci.appveyor.com/project/nilsbore/auvlib/branch/master) [![license](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) Tools for reading AUV deployment data files and for diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3e67fafe..98d1be59 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -11,11 +11,12 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) # locations on all platforms. include(GNUInstallDirs) -find_package(Eigen3 REQUIRED) -find_package(OpenCV REQUIRED) -find_package(OpenGL REQUIRED) -find_package(glfw3 REQUIRED) -find_package(Ceres REQUIRED) +option(AUVLIB_WITH_GSF "Enable reading and processing gsf data" ON) +option(AUVLIB_USE_LIBIGL_EIGEN "Use libigl's version of Eigen" ON) +option(AUVLIB_USE_LIBIGL_GLFW "Use libigl's version of glfw" OFF) +option(AUVLIB_USE_LIBIGL_TINYXML "Use libigl's version of tinyxml" OFF) +option(AUVLIB_USE_PYTHON3 "Use python3 instead of python2" OFF) +option(AUVLIB_EXPORT_BUILD "Export build dir configs" ON) # libigl option(LIBIGL_USE_STATIC_LIBRARY "Use libigl as static library" OFF) @@ -35,13 +36,55 @@ option(LIBIGL_WITH_PYTHON "Use Python" OFF) option(LIBIGL_WITH_TETGEN "Use Tetgen" OFF) option(LIBIGL_WITH_TRIANGLE "Use Triangle" OFF) option(LIBIGL_WITH_VIEWER "Use OpenGL viewer" ON) -option(LIBIGL_WITH_XML "Use XML" OFF) +option(LIBIGL_WITH_XML "Use XML" ON) #option(LIBIGL_EXPORT_TARGETS "Export libigl CMake targets" ON) +find_package(OpenCV REQUIRED core highgui imgproc) +find_package(OpenGL REQUIRED) +if (AUVLIB_USE_LIBIGL_TINYXML) + set(TinyXML2_LIBRARIES tinyxml2) +else() + #find_package(TinyXML2 REQUIRED) + set(TinyXML2_LIBRARIES -ltinyxml2) +endif() + set(ENV{LIBIGL_DIR} ${CMAKE_CURRENT_SOURCE_DIR}/libigl) find_package(LIBIGL REQUIRED QUIET) include(${CMAKE_CURRENT_BINARY_DIR}/embree/embree-config.cmake) +if (AUVLIB_USE_LIBIGL_GLFW) + #add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/libigl/external/glfw) # this might be needed on windows? + set(GLFW3_LIBRARY glfw) +else() + find_package(glfw3 REQUIRED) +endif() + +if (AUVLIB_USE_LIBIGL_EIGEN) + set(EIGEN3_INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/libigl/external/eigen) +else() + find_package(Eigen3 REQUIRED) +endif() + +if(MSVC) + if (AUVLIB_USE_PYTHON3) + set(PYTHON_EXECUTABLE C:/Python35/python.exe) + else() + #set(PYTHON_EXECUTABLE C:/Python27/python.exe) + set(PYTHON_EXECUTABLE C:/Python27-x64/python.exe) + endif() +else() + if (AUVLIB_USE_PYTHON3) + set(PYTHON_EXECUTABLE /usr/bin/python3.5) + else() + set(PYTHON_EXECUTABLE /usr/bin/python2.7) + endif() +endif() + +if (MSVC) + set(Boost_USE_STATIC_LIBS ON CACHE BOOL "use static libraries from Boost") + set(Boost_USE_MULTITHREADED ON) + set(Boost_USE_DEBUG_RUNTIME OFF) +endif() # For some reason it seems like we need to do this after libigl find_package(Boost COMPONENTS system filesystem date_time REQUIRED) @@ -51,18 +94,21 @@ set(PYBIND11_CPP_STANDARD -std=c++11) #add_definitions(-DPYBIND11_PYTHON_VERSION=2.7) #set(PYBIND11_PYTHON_VERSION 2.7) #set(PYTHON_LIBRARIES /usr/lib/x86_64-linux-gnu/libpython2.7.so) -set(PYTHON_EXECUTABLE /usr/bin/python2.7) + add_subdirectory(pybind11) message("Pybind11: " ${PYBIND11_INCLUDE_DIR}) add_subdirectory(cxxopts) -include_directories(${PYBIND11_INCLUDE_DIR} ${CERES_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS}) +include_directories(${PYBIND11_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/cereal/include ${TinyXML2_INCLUDE_DIRS}) +link_directories(${Boost_LIBRARY_DIRS}) #include_directories(eigen_cereal/include) include(cmake/glad.cmake) -add_subdirectory(libgsf) +if(AUVLIB_WITH_GSF) + add_subdirectory(libgsf) +endif() add_subdirectory(libxtf) add_subdirectory(eigen_cereal) add_subdirectory(data_tools) diff --git a/src/bathy_maps/CMakeLists.txt b/src/bathy_maps/CMakeLists.txt index 3454d286..c52684d1 100644 --- a/src/bathy_maps/CMakeLists.txt +++ b/src/bathy_maps/CMakeLists.txt @@ -44,7 +44,9 @@ add_library(sss_meas_data src/sss_meas_data.cpp) add_library(sss_gen_sim src/sss_gen_sim.cpp) -add_executable(test_mesh src/test_mesh.cpp) +if(AUVLIB_WITH_GSF) + add_executable(test_mesh src/test_mesh.cpp) +endif() # Define headers for this library. PUBLIC headers are used for # compiling the library, and will be added to consumers' build @@ -110,14 +112,16 @@ target_include_directories(sss_gen_sim PUBLIC target_link_libraries(draw_map std_data ${OpenCV_LIBS}) #target_link_libraries(mesh_map std_data igl::embree ${OpenCV_LIBS} glad ${GLFW3_LIBRARY} ${OPENGL_LIBRARY} ${OPENGL_glu_LIBRARY} -lpthread) -target_link_libraries(mesh_map PRIVATE std_data ${OpenCV_LIBS} ${GLFW3_LIBRARY} auvlib_glad -lpthread -ltinyxml2) # ${TinyXML2_LIBRARIES}) +target_link_libraries(mesh_map std_data ${OpenCV_LIBS} ${GLFW3_LIBRARY} auvlib_glad -lpthread ${TinyXML2_LIBRARIES}) target_link_libraries(align_map mesh_map std_data xyz_data ${GLFW3_LIBRARY} auvlib_glad -lpthread) # ${TinyXML2_LIBRARIES}) target_link_libraries(drape_mesh snell_ray_tracing xtf_data ${GLFW3_LIBRARY} auvlib_glad) -target_link_libraries(test_mesh std_data gsf_data xtf_data csv_data navi_data mesh_map draw_map patch_draper igl::embree ${OpenCV_LIBS} cxxopts) +if(AUVLIB_WITH_GSF) + target_link_libraries(test_mesh std_data gsf_data xtf_data csv_data navi_data mesh_map draw_map patch_draper igl::embree ${OpenCV_LIBS} cxxopts) +endif() target_link_libraries(patch_views eigen_cereal ${OpenCV_LIBS}) @@ -146,5 +150,7 @@ install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) # just go into 'cmake'. install(EXPORT BathyMapsConfig DESTINATION share/BathyMaps/cmake) -# This makes the project importable from the build directory -export(TARGETS draw_map mesh_map align_map drape_mesh patch_draper base_draper map_draper patch_views sss_map_image sss_meas_data sss_gen_sim FILE BathyMapsConfig.cmake) +if (AUVLIB_EXPORT_BUILD) + # This makes the project importable from the build directory + export(TARGETS draw_map mesh_map align_map drape_mesh patch_draper base_draper map_draper patch_views sss_map_image sss_meas_data sss_gen_sim FILE BathyMapsConfig.cmake) +endif() diff --git a/src/bathy_maps/include/bathy_maps/align_map.h b/src/bathy_maps/include/bathy_maps/align_map.h index 7e156a92..8901970f 100644 --- a/src/bathy_maps/include/bathy_maps/align_map.h +++ b/src/bathy_maps/include/bathy_maps/align_map.h @@ -1,7 +1,7 @@ #ifndef ALIGN_MAP_H #define ALIGN_MAP_H -#include +#include #include #include diff --git a/src/bathy_maps/include/bathy_maps/base_draper.h b/src/bathy_maps/include/bathy_maps/base_draper.h index 558a32ef..bbee6998 100644 --- a/src/bathy_maps/include/bathy_maps/base_draper.h +++ b/src/bathy_maps/include/bathy_maps/base_draper.h @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include diff --git a/src/bathy_maps/include/bathy_maps/drape_mesh.h b/src/bathy_maps/include/bathy_maps/drape_mesh.h index 77fd177b..352f7300 100644 --- a/src/bathy_maps/include/bathy_maps/drape_mesh.h +++ b/src/bathy_maps/include/bathy_maps/drape_mesh.h @@ -12,7 +12,7 @@ #ifndef DRAPE_MESH_H #define DRAPE_MESH_H -#include +#include #include #include diff --git a/src/bathy_maps/include/bathy_maps/mesh_map.h b/src/bathy_maps/include/bathy_maps/mesh_map.h index 283b0d3f..32940d9e 100644 --- a/src/bathy_maps/include/bathy_maps/mesh_map.h +++ b/src/bathy_maps/include/bathy_maps/mesh_map.h @@ -12,7 +12,7 @@ #ifndef MESH_MAP_H #define MESH_MAP_H -#include +#include #include #include diff --git a/src/bathy_maps/include/bathy_maps/patch_views.h b/src/bathy_maps/include/bathy_maps/patch_views.h index f24b2b19..4a98477c 100644 --- a/src/bathy_maps/include/bathy_maps/patch_views.h +++ b/src/bathy_maps/include/bathy_maps/patch_views.h @@ -12,7 +12,7 @@ #ifndef PATCH_VIEWS_H #define PATCH_VIEWS_H -#include +#include #include #include #include diff --git a/src/bathy_maps/include/bathy_maps/sss_map_image.h b/src/bathy_maps/include/bathy_maps/sss_map_image.h index 40208fea..9643961a 100644 --- a/src/bathy_maps/include/bathy_maps/sss_map_image.h +++ b/src/bathy_maps/include/bathy_maps/sss_map_image.h @@ -12,7 +12,7 @@ #ifndef SSS_MAP_IMAGE_H #define SSS_MAP_IMAGE_H -#include +#include #include #include #include @@ -27,7 +27,7 @@ struct sss_map_image { BoundsT bounds; - Eigen::MatrixXd sss_map_image; + Eigen::MatrixXd sss_map_image_; double sss_ping_duration; // max time in waterfall image Eigen::MatrixXf sss_waterfall_image; @@ -40,7 +40,7 @@ struct sss_map_image { template void serialize( Archive & ar ) { - ar(CEREAL_NVP(bounds), CEREAL_NVP(sss_map_image), CEREAL_NVP(sss_ping_duration), + ar(CEREAL_NVP(bounds), CEREAL_NVP(sss_map_image_), CEREAL_NVP(sss_ping_duration), CEREAL_NVP(sss_waterfall_image), CEREAL_NVP(sss_waterfall_cross_track), CEREAL_NVP(sss_waterfall_depth), CEREAL_NVP(sss_waterfall_model), CEREAL_NVP(pos)); } diff --git a/src/bathy_maps/include/bathy_maps/sss_meas_data.h b/src/bathy_maps/include/bathy_maps/sss_meas_data.h index 637fa86b..4cdb9f0f 100644 --- a/src/bathy_maps/include/bathy_maps/sss_meas_data.h +++ b/src/bathy_maps/include/bathy_maps/sss_meas_data.h @@ -12,7 +12,7 @@ #ifndef SSS_MEAS_DATA_H #define SSS_MEAS_DATA_H -#include +#include #include #include #include diff --git a/src/bathy_maps/src/align_map.cpp b/src/bathy_maps/src/align_map.cpp index a08652ea..72b2c906 100644 --- a/src/bathy_maps/src/align_map.cpp +++ b/src/bathy_maps/src/align_map.cpp @@ -31,7 +31,7 @@ double compute_overlap_ratio(const Eigen::MatrixXd& P1, const Eigen::MatrixXd& P int overlapping_points = 0; for (int i = 0; i < P1.rows(); i += skip) { Eigen::MatrixXd::Index index; - // find nearest neighbour + // find near_est neighbour //(P2_sub.rowwise() - P1.row(i)).colwise().squaredNorm().minCoeff(&index); double dist = sqrt((P2_sub.rowwise() - P1.row(i)).colwise().squaredNorm().minCoeff(&index)); //double dist = (P2_sub.row(index) - P1.row(i)).norm(); @@ -113,16 +113,16 @@ double points_to_mesh_rmse(const Eigen::MatrixXd& P, const Eigen::MatrixXd& V, c tree.squared_distance(V, F, P, sqrD, I, Q); //Eigen::ArrayXd temp = (P - Q).rowwise().squaredNorm().array(); //cout << "Temp: " << temp.transpose() << endl; - Eigen::Array near = (P - Q).rowwise().squaredNorm().array() < assoc_threshold*assoc_threshold; + Eigen::Array near_ = (P - Q).rowwise().squaredNorm().array() < assoc_threshold*assoc_threshold; - int nbr_near = near.cast().sum(); + int nbr_near_ = near_.cast().sum(); - cout << "Number points near surface: " << nbr_near << endl; + cout << "Number points near_ surface: " << nbr_near_ << endl; cout << "Out of: " << P.rows() << endl; cout << "P rows: " << P.rows() << endl; - Eigen::MatrixXd goodP = igl::slice_mask(P, near, 1); - Eigen::MatrixXd goodQ = igl::slice_mask(Q, near, 1); + Eigen::MatrixXd goodP = igl::slice_mask(P, near_, 1); + Eigen::MatrixXd goodQ = igl::slice_mask(Q, near_, 1); cout << "GoodP rows: " << goodP.rows() << ", cols: " << goodP.cols() << endl; @@ -142,13 +142,13 @@ Eigen::MatrixXd filter_points_mesh_offset(const Eigen::MatrixXd& P, const Eigen: cout << "Finding closes mesh points..." << endl; tree.squared_distance(V, F, P, sqrD, I, Q); - Eigen::Array near = sqrD.array() < offset*offset; + Eigen::Array near_ = sqrD.array() < offset*offset; - int nbr_near = near.cast().sum(); + int nbr_near_ = near_.cast().sum(); - cout << "Number points near surface: " << nbr_near << " out of: " << P.rows() << endl; + cout << "Number points near_ surface: " << nbr_near_ << " out of: " << P.rows() << endl; - Eigen::MatrixXd goodP = igl::slice_mask(P, near, 1); + Eigen::MatrixXd goodP = igl::slice_mask(P, near_, 1); cout << "GoodP rows: " << goodP.rows() << ", cols: " << goodP.cols() << endl; @@ -304,26 +304,26 @@ tuple icp_iteration(const Eigen::MatrixXd& P, con Eigen::MatrixXd Q; tree.squared_distance(V, F, P, sqrD, I, Q); - //Eigen::Array near = (P - Q).rowwise().squaredNorm().array() < assoc_threshold*assoc_threshold; + //Eigen::Array near_ = (P - Q).rowwise().squaredNorm().array() < assoc_threshold*assoc_threshold; Eigen::Array close = (P - Q).leftCols<2>().rowwise().squaredNorm().array() < 0.1*0.1; - Eigen::Array near = sqrD.array() < assoc_threshold*assoc_threshold && close; + Eigen::Array near_ = sqrD.array() < assoc_threshold*assoc_threshold && close; - int nbr_near = near.cast().sum(); - cout << "Number points near surface: " << nbr_near << endl; + int nbr_near_ = near_.cast().sum(); + cout << "Number points near_ surface: " << nbr_near_ << endl; cout << "Out of: " << P.rows() << endl; - if (nbr_near < 100) { + if (nbr_near_ < 100) { cout << "Breaking since we do not have enough points!" << endl; return make_tuple(Eigen::Matrix4d::Identity(), 0., false); } - Eigen::MatrixXd goodP = igl::slice_mask(P, near, 1); - Eigen::MatrixXd goodQ = igl::slice_mask(Q, near, 1); + Eigen::MatrixXd goodP = igl::slice_mask(P, near_, 1); + Eigen::MatrixXd goodQ = igl::slice_mask(Q, near_, 1); Eigen::Vector3d meanP = goodP.rowwise().mean().transpose(); Eigen::Vector3d meanQ = goodQ.rowwise().mean().transpose(); - double mean_dist = igl::slice_mask(sqrD, near, 1).mean(); //(goodP - goodQ).rowwise().squaredNorm().mean(); + double mean_dist = igl::slice_mask(sqrD, near_, 1).mean(); //(goodP - goodQ).rowwise().squaredNorm().mean(); Eigen::Matrix3d covariance = 1./double(goodP.rows())*(goodQ.transpose().colwise() - meanQ)*(goodP.rowwise() - meanP.transpose()); diff --git a/src/bathy_maps/src/sss_gen_sim.cpp b/src/bathy_maps/src/sss_gen_sim.cpp index d6ef592e..b7e1a275 100644 --- a/src/bathy_maps/src/sss_gen_sim.cpp +++ b/src/bathy_maps/src/sss_gen_sim.cpp @@ -489,7 +489,7 @@ bool SSSGenSim::callback_pre_draw(igl::opengl::glfw::Viewer& viewer) time_windows.tail(time_windows_right.rows()) = time_windows_right; time_windows.head(time_windows_left.rows()) = time_windows_left.reverse(); - for (int i = 0; i < std::min(time_windows.rows(), long(waterfall_image.cols)); ++i) { + for (int i = 0; i < std::min(time_windows.rows(), int64_t(waterfall_image.cols)); ++i) { waterfall_image.at(0, i) = uint8_t(255.*time_windows(i)); } diff --git a/src/bathy_maps/src/sss_map_image.cpp b/src/bathy_maps/src/sss_map_image.cpp index dbe38314..892f91f7 100644 --- a/src/bathy_maps/src/sss_map_image.cpp +++ b/src/bathy_maps/src/sss_map_image.cpp @@ -82,7 +82,7 @@ sss_map_image sss_map_image_builder::finish() map_image.bounds = bounds; if (sss_map_image_counts.sum() > 0) { sss_map_image_counts.array() += (sss_map_image_counts.array() == 0).cast(); - map_image.sss_map_image.array() = sss_map_image_sums.array() / sss_map_image_counts.array(); + map_image.sss_map_image_.array() = sss_map_image_sums.array() / sss_map_image_counts.array(); } map_image.sss_ping_duration = sss_ping_duration; map_image.pos = poss; @@ -283,8 +283,8 @@ sss_patch_views::ViewsT convert_maps_to_patches(const sss_map_image::ImagesT& ma sss_patch_views::ViewsT patches; sss_map_image::BoundsT bounds = map_images[0].bounds; - int image_rows = map_images[0].sss_map_image.rows(); - int image_cols = map_images[0].sss_map_image.cols(); + int image_rows = map_images[0].sss_map_image_.rows(); + int image_cols = map_images[0].sss_map_image_.cols(); double resolution = double(image_cols)/(bounds(1, 0) - bounds(0, 0)); @@ -313,9 +313,9 @@ sss_patch_views::ViewsT convert_maps_to_patches(const sss_map_image::ImagesT& ma //patch_views.patch_height = Eigen::MatrixXd::Zero(image_size, image_size); patch_views.patch_height = height_map.block(i*image_size, j*image_size, image_size, image_size); for (int n = 0; n < map_images.size(); ++n) { - //cout << "current x start: " << j*image_size << " out of " << map_images[n].sss_map_image.cols() << endl; - //cout << "current y start: " << i*image_size << " out of " << map_images[n].sss_map_image.rows() << endl; - Eigen::MatrixXd view = map_images[n].sss_map_image.block(i*image_size, j*image_size, image_size, image_size); + //cout << "current x start: " << j*image_size << " out of " << map_images[n].sss_map_image_.cols() << endl; + //cout << "current y start: " << i*image_size << " out of " << map_images[n].sss_map_image_.rows() << endl; + Eigen::MatrixXd view = map_images[n].sss_map_image_.block(i*image_size, j*image_size, image_size, image_size); //cout << "view mean: " << view.mean() << endl; double fraction_zeros = (view.array() == 0).cast().mean(); // / patch_area; if (fraction_zeros < 1.) { @@ -512,7 +512,7 @@ sss_patch_views::ViewsT convert_maps_to_single_angle_patches(const sss_map_image sss_patch_views::ViewsT patches; sss_map_image::BoundsT bounds = map_images[0].bounds; - int image_cols = map_images[0].sss_map_image.cols(); + int image_cols = map_images[0].sss_map_image_.cols(); double resolution = double(image_cols)/(bounds(1, 0) - bounds(0, 0)); int image_size = patch_size*resolution; @@ -521,11 +521,11 @@ sss_patch_views::ViewsT convert_maps_to_single_angle_patches(const sss_map_image for (int n = 0; n < map_images.size(); ++n) { - cv::Mat sss_image(map_images[n].sss_map_image.rows(), map_images[n].sss_map_image.rows(), CV_32FC1); - cv::Mat map_image(map_images[n].sss_map_image.rows(), map_images[n].sss_map_image.rows(), CV_32FC1); + cv::Mat sss_image(map_images[n].sss_map_image_.rows(), map_images[n].sss_map_image_.rows(), CV_32FC1); + cv::Mat map_image(map_images[n].sss_map_image_.rows(), map_images[n].sss_map_image_.rows(), CV_32FC1); for (int i = 0; i < sss_image.rows; ++i) { for (int j = 0; j < sss_image.cols; ++j) { - sss_image.at(i, j) = map_images[n].sss_map_image(i, j); + sss_image.at(i, j) = map_images[n].sss_map_image_(i, j); map_image.at(i, j) = height_map(i, j); } } diff --git a/src/cereal b/src/cereal new file mode 160000 index 00000000..51cbda5f --- /dev/null +++ b/src/cereal @@ -0,0 +1 @@ +Subproject commit 51cbda5f30e56c801c07fe3d3aba5d7fb9e6cca4 diff --git a/src/data_tools/CMakeLists.txt b/src/data_tools/CMakeLists.txt index 6b22608c..4899325c 100644 --- a/src/data_tools/CMakeLists.txt +++ b/src/data_tools/CMakeLists.txt @@ -11,7 +11,10 @@ add_library(benchmark src/benchmark.cpp) add_library(navi_data src/navi_data.cpp) -add_library(gsf_data src/gsf_data.cpp) +if(AUVLIB_WITH_GSF) + add_library(gsf_data src/gsf_data.cpp) + add_executable(test_submap_tracks src/test_submap_tracks.cpp) +endif() add_library(csv_data src/csv_data.cpp) @@ -25,8 +28,6 @@ add_library(xyz_data src/xyz_data.cpp) add_executable(test_xtf src/test_xtf.cpp) -add_executable(test_submap_tracks src/test_submap_tracks.cpp) - add_executable(test_all src/test_all.cpp) # Define headers for this library. PUBLIC headers are used for @@ -57,10 +58,12 @@ target_include_directories(navi_data PUBLIC $ PRIVATE src) -target_include_directories(gsf_data PUBLIC - $ - $ - PRIVATE src) +if(AUVLIB_WITH_GSF) + target_include_directories(gsf_data PUBLIC + $ + $ + PRIVATE src) +endif() target_include_directories(csv_data PUBLIC $ @@ -87,33 +90,46 @@ target_include_directories(xyz_data PUBLIC $ PRIVATE src) -target_link_libraries(test_xtf xtf_data std_data navi_data ${OpenCV_LIBS} ${Boost_LIBRARIES}) +if(MSVC) + set(EXTRA_BOOST_LIBS "") +else() + set(EXTRA_BOOST_LIBS ${Boost_LIBRARIES}) +endif() -target_link_libraries(test_submap_tracks gsf_data std_data navi_data ${OpenCV_LIBS} ${Boost_LIBRARIES}) +target_link_libraries(test_xtf xtf_data std_data navi_data ${OpenCV_LIBS} ${EXTRA_BOOST_LIBS}) -target_link_libraries(test_all all_data ${OpenCV_LIBS} ${Boost_LIBRARIES}) +target_link_libraries(test_all all_data ${OpenCV_LIBS} ${EXTRA_BOOST_LIBS}) # Link the libraries -target_link_libraries(submaps eigen_cereal ${Boost_LIBRARIES}) # ${PCL_LIBRARIES}) +target_link_libraries(submaps eigen_cereal ${EXTRA_BOOST_LIBS}) # ${PCL_LIBRARIES}) -target_link_libraries(std_data PUBLIC eigen_cereal ${Boost_LIBRARIES}) +target_link_libraries(std_data PUBLIC eigen_cereal ${EXTRA_BOOST_LIBS}) target_link_libraries(benchmark PUBLIC eigen_cereal std_data ${OpenCV_LIBS}) target_link_libraries(navi_data PUBLIC eigen_cereal data_transforms) # ${PCL_LIBRARIES}) -target_link_libraries(gsf_data PUBLIC gsf std_data navi_data lat_long_utm) +target_link_libraries(csv_data PUBLIC std_data navi_data xtf_data) -target_link_libraries(csv_data PUBLIC std_data navi_data gsf_data xtf_data) +if(AUVLIB_WITH_GSF) + target_link_libraries(gsf_data PUBLIC gsf std_data navi_data lat_long_utm csv_data) + target_link_libraries(test_submap_tracks gsf_data std_data navi_data ${OpenCV_LIBS} ${EXTRA_BOOST_LIBS}) +endif() target_link_libraries(xtf_data PUBLIC std_data navi_data xtf_reader lat_long_utm ${OpenCV_LIBS}) target_link_libraries(all_data navi_data lat_long_utm csv_data ${OpenCV_LIBS}) -target_link_libraries(xyz_data std_data ${Boost_LIBRARIES}) +target_link_libraries(xyz_data std_data ${EXTRA_BOOST_LIBS}) + +set(AUVLIB_DATA_TOOLS_LIBS data_transforms submaps std_data benchmark navi_data csv_data xtf_data all_data xyz_data lat_long_utm) + +if(AUVLIB_WITH_GSF) + set(AUVLIB_DATA_TOOLS_LIBS ${AUVLIB_DATA_TOOLS_LIBS} gsf_data) +endif() # 'make install' to the correct locations (provided by GNUInstallDirs). -install(TARGETS data_transforms submaps std_data benchmark navi_data gsf_data csv_data xtf_data all_data xyz_data lat_long_utm EXPORT DataToolsConfig +install(TARGETS ${AUVLIB_DATA_TOOLS_LIBS} EXPORT DataToolsConfig ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) # This is for Windows @@ -125,4 +141,4 @@ install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) install(EXPORT DataToolsConfig DESTINATION share/DataTools/cmake) # This makes the project importable from the build directory -export(TARGETS data_transforms submaps std_data benchmark navi_data gsf_data csv_data xtf_data all_data xyz_data lat_long_utm FILE DataToolsConfig.cmake) +export(TARGETS ${AUVLIB_DATA_TOOLS_LIBS} FILE DataToolsConfig.cmake) diff --git a/src/data_tools/include/data_tools/all_data.h b/src/data_tools/include/data_tools/all_data.h index 835852af..4dcfe30e 100644 --- a/src/data_tools/include/data_tools/all_data.h +++ b/src/data_tools/include/data_tools/all_data.h @@ -14,7 +14,7 @@ #include #include -#include +#include #define BOOST_NO_CXX11_SCOPED_ENUMS #include #undef BOOST_NO_CXX11_SCOPED_ENUMS @@ -158,17 +158,21 @@ namespace std_data { //template //std::vector > parse_file(const boost::filesystem::path& path); + +template <> +all_data::all_mbes_ping::PingsT parse_file(const boost::filesystem::path& file); + template <> -all_data::all_mbes_ping::PingsT parse_file(const boost::filesystem::path& path); +all_data::all_nav_entry::EntriesT parse_file(const boost::filesystem::path& file); template <> -all_data::all_nav_entry::EntriesT parse_file(const boost::filesystem::path& path); +all_data::all_nav_depth::EntriesT parse_file(const boost::filesystem::path& file); template <> -all_data::all_nav_depth::EntriesT parse_file(const boost::filesystem::path& path); +all_data::all_nav_attitude::EntriesT parse_file(const boost::filesystem::path& file); template <> -all_data::all_nav_attitude::EntriesT parse_file(const boost::filesystem::path& path); +all_data::all_echosounder_depth::EntriesT parse_file(const boost::filesystem::path& file); } diff --git a/src/data_tools/include/data_tools/csv_data.h b/src/data_tools/include/data_tools/csv_data.h index 22e436d8..b174ba4d 100644 --- a/src/data_tools/include/data_tools/csv_data.h +++ b/src/data_tools/include/data_tools/csv_data.h @@ -14,7 +14,6 @@ #include #include -#include #include namespace csv_data { @@ -77,7 +76,6 @@ struct csv_asvp_sound_speed EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -std_data::mbes_ping::PingsT convert_matched_entries(gsf_data::gsf_mbes_ping::PingsT& pings, csv_nav_entry::EntriesT& entries); xtf_data::xtf_sss_ping::PingsT convert_matched_entries(xtf_data::xtf_sss_ping::PingsT& pings, csv_data::csv_nav_entry::EntriesT& entries); } // namespace csv_data diff --git a/src/data_tools/include/data_tools/gsf_data.h b/src/data_tools/include/data_tools/gsf_data.h index ae27f2c8..16dc290b 100644 --- a/src/data_tools/include/data_tools/gsf_data.h +++ b/src/data_tools/include/data_tools/gsf_data.h @@ -14,6 +14,7 @@ #include #include +#include namespace gsf_data { @@ -98,6 +99,8 @@ void match_sound_speeds(gsf_mbes_ping::PingsT& pings, gsf_sound_speed::SpeedsT& std_data::mbes_ping::PingsT convert_matched_entries(gsf_mbes_ping::PingsT& pings, gsf_nav_entry::EntriesT& entries); +std_data::mbes_ping::PingsT convert_matched_entries(gsf_data::gsf_mbes_ping::PingsT& pings, csv_data::csv_nav_entry::EntriesT& entries); + std_data::mbes_ping::PingsT convert_pings(gsf_mbes_ping::PingsT& pings); } // namespace gsf_data diff --git a/src/data_tools/include/data_tools/navi_data.h b/src/data_tools/include/data_tools/navi_data.h index f4e5e831..442990fd 100644 --- a/src/data_tools/include/data_tools/navi_data.h +++ b/src/data_tools/include/data_tools/navi_data.h @@ -12,7 +12,7 @@ #ifndef NAVI_DATA_H #define NAVI_DATA_H -#include +#include #include namespace navi_data { diff --git a/src/data_tools/include/data_tools/std_data.h b/src/data_tools/include/data_tools/std_data.h index 05f68ccd..72457a77 100644 --- a/src/data_tools/include/data_tools/std_data.h +++ b/src/data_tools/include/data_tools/std_data.h @@ -13,7 +13,7 @@ #define DATA_STRUCTURES_H #include -#include +#include #include #include #include @@ -28,6 +28,12 @@ #include #undef BOOST_NO_CXX11_SCOPED_ENUMS +#define _USE_MATH_DEFINES +#include +#ifndef M_PI // For windows + #define M_PI 3.14159265358979323846 +#endif + namespace std_data { struct mbes_ping diff --git a/src/data_tools/include/data_tools/submaps.h b/src/data_tools/include/data_tools/submaps.h index fbb8bb8b..15e5db1c 100644 --- a/src/data_tools/include/data_tools/submaps.h +++ b/src/data_tools/include/data_tools/submaps.h @@ -12,7 +12,7 @@ #ifndef SUBMAPS_H #define SUBMAPS_H -#include +#include #include #define BOOST_NO_CXX11_SCOPED_ENUMS #include diff --git a/src/data_tools/include/data_tools/transforms.h b/src/data_tools/include/data_tools/transforms.h index 9b884d28..0baf3bcf 100644 --- a/src/data_tools/include/data_tools/transforms.h +++ b/src/data_tools/include/data_tools/transforms.h @@ -12,7 +12,7 @@ #ifndef DATA_TRANSFORMS_H #define DATA_TRANSFORMS_H -#include +#include #include namespace data_transforms { diff --git a/src/data_tools/include/data_tools/xtf_data.h b/src/data_tools/include/data_tools/xtf_data.h index cc5bc015..52b45947 100644 --- a/src/data_tools/include/data_tools/xtf_data.h +++ b/src/data_tools/include/data_tools/xtf_data.h @@ -13,7 +13,7 @@ #define XTF_DATA_H #include -#include +#include #define BOOST_NO_CXX11_SCOPED_ENUMS #include #undef BOOST_NO_CXX11_SCOPED_ENUMS diff --git a/src/data_tools/include/data_tools/xyz_data.h b/src/data_tools/include/data_tools/xyz_data.h index beb70803..79594ca8 100644 --- a/src/data_tools/include/data_tools/xyz_data.h +++ b/src/data_tools/include/data_tools/xyz_data.h @@ -13,7 +13,7 @@ #define XYZ_DATA_H #include -#include +#include #define BOOST_NO_CXX11_SCOPED_ENUMS #include #undef BOOST_NO_CXX11_SCOPED_ENUMS diff --git a/src/data_tools/include/liball/all.h b/src/data_tools/include/liball/all.h index cd977329..520b3591 100644 --- a/src/data_tools/include/liball/all.h +++ b/src/data_tools/include/liball/all.h @@ -12,20 +12,26 @@ #ifndef ALL_H #define ALL_H +#ifdef _MSC_VER + #define PACK( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) ) +#else + #define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__)) +#endif + // this is shared as the first part of all .all datagrams -struct all_common_header { +PACK(struct all_common_header { unsigned int bytes; // Number of bytes in datagram unsigned char start_id; // Start identifier = STX (Always 02h) unsigned char data_type; // Type of datagram = X (58h, 88d) -} __attribute__((packed)); +}); -struct all_common_end { +PACK(struct all_common_end { // end of repeat cycle unsigned char end_ident; // End identifier = ETX (Always 03h) unsigned short checksum; // Check sum of data between STX and ETX -} __attribute__((packed)); +}); -struct all_xyz88_datagram { +PACK(struct all_xyz88_datagram { // common header, see all_common_header // data description @@ -46,9 +52,9 @@ struct all_xyz88_datagram { // repeat cycle, see all_xyz88_datagram_repeat // end of repeat cycle, see all_common_end -} __attribute__((packed)); +}); -struct all_position_datagram { +PACK(struct all_position_datagram { // common header, see all_common_header // data description @@ -71,9 +77,9 @@ struct all_position_datagram { // Position input datagram as received // end of repeat cycle, see all_common_end -} __attribute__((packed)); +}); -struct all_depth_datagram { +PACK(struct all_depth_datagram { // common header, see all_common_header // data description @@ -87,9 +93,9 @@ struct all_depth_datagram { unsigned char height_type; // Height type // end of repeat cycle, see all_common_end -} __attribute__((packed)); +}); -struct all_echosounder_depth_datagram { +PACK(struct all_echosounder_depth_datagram { // common header, see all_common_header // data description @@ -105,10 +111,10 @@ struct all_echosounder_depth_datagram { char source_id; // Source identifier (S, T, 1, 2 or 3) // end of repeat cycle, see all_common_end -} __attribute__((packed)); +}); // repeats nbr_beams times as given in all_xyz88_datagram -struct all_xyz88_datagram_repeat { +PACK(struct all_xyz88_datagram_repeat { // repeat cycle data float depth; // Depth (z) from transmit transducer in m float across_track; // Acrosstrack distance (y) in m @@ -119,9 +125,9 @@ struct all_xyz88_datagram_repeat { unsigned char detection_info; // Detection information char rt_cleaning_info; // Real time cleaning information short reflectivity; // Reflectivity (BS) in 0.1 dB resolution (Example: –20.1 dB = FF37h= 65335) -} __attribute__((packed)); +}); -struct all_attitude_datagram { +PACK(struct all_attitude_datagram { unsigned short model_nbr; // EM model number (Example: EM 710 = 710) unsigned int date; // Date = year*10000 + month*100 + day (Example: Sep 26, 2005 = 20050926) unsigned int time; // Time since midnight in milliseconds (Example: 08:12:51.234 = 29570234) @@ -130,15 +136,17 @@ struct all_attitude_datagram { unsigned short nbr_entries; // = N 2U 1 – — //Repeat cycle – N entries of: 12*N — -} __attribute__((packed)); +}); -struct all_attitude_datagram_repeat { +PACK(struct all_attitude_datagram_repeat { unsigned short ms_since_start; // Time in milliseconds since record start 2U 0 to 65534 — unsigned short sensor_status; // Sensor status 2U — 1 short roll; // Roll in 0.01° 2S -18000 to 18000 — short pitch; // Pitch in 0.01° 2S -18000 to 18000 — short heave; // Heave in cm 2S -1000 to 10000 — unsigned short heading; //– Heading in 0.01° 2U 0 to 35999 — -} __attribute__((packed)); +}); + +#undef PACK #endif // ALL_H diff --git a/src/data_tools/src/all_data.cpp b/src/data_tools/src/all_data.cpp index cbdb725f..4a49d2bc 100644 --- a/src/data_tools/src/all_data.cpp +++ b/src/data_tools/src/all_data.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +//#include #include #define BOOST_NO_CXX11_SCOPED_ENUMS @@ -570,33 +570,33 @@ namespace std_data { using namespace all_data; template <> -all_mbes_ping::PingsT parse_file(const boost::filesystem::path& path) +all_mbes_ping::PingsT parse_file(const boost::filesystem::path& file) { - return parse_file_impl(path); + return parse_file_impl(file); } template <> -all_nav_entry::EntriesT parse_file(const boost::filesystem::path& path) +all_nav_entry::EntriesT parse_file(const boost::filesystem::path& file) { - return parse_file_impl(path); + return parse_file_impl(file); } template <> -all_nav_depth::EntriesT parse_file(const boost::filesystem::path& path) +all_nav_depth::EntriesT parse_file(const boost::filesystem::path& file) { - return parse_file_impl(path); + return parse_file_impl(file); } template <> -all_nav_attitude::EntriesT parse_file(const boost::filesystem::path& path) +all_nav_attitude::EntriesT parse_file(const boost::filesystem::path& file) { - return parse_file_impl(path); + return parse_file_impl(file); } template <> -all_echosounder_depth::EntriesT parse_file(const boost::filesystem::path& path) +all_echosounder_depth::EntriesT parse_file(const boost::filesystem::path& file) { - return parse_file_impl(path); + return parse_file_impl(file); } } // namespace std_data diff --git a/src/data_tools/src/csv_data.cpp b/src/data_tools/src/csv_data.cpp index a1db72b3..96a17f82 100644 --- a/src/data_tools/src/csv_data.cpp +++ b/src/data_tools/src/csv_data.cpp @@ -28,88 +28,8 @@ inline std::basic_istream& skip(std::basic_istream& namespace csv_data { using namespace std_data; -using namespace gsf_data; using namespace xtf_data; -mbes_ping::PingsT convert_matched_entries(gsf_mbes_ping::PingsT& pings, csv_nav_entry::EntriesT& entries) -{ - mbes_ping::PingsT new_pings; - - std::stable_sort(entries.begin(), entries.end(), [](const csv_nav_entry& entry1, const csv_nav_entry& entry2) { - return entry1.time_stamp_ < entry2.time_stamp_; - }); - - auto pos = entries.begin(); - for (gsf_mbes_ping& ping : pings) { - pos = std::find_if(pos, entries.end(), [&](const csv_nav_entry& entry) { - return entry.time_stamp_ > ping.time_stamp_; - }); - - mbes_ping new_ping; - new_ping.time_stamp_ = ping.time_stamp_; - new_ping.time_string_ = ping.time_string_; - new_ping.first_in_file_ = ping.first_in_file_; - //cout << "Ping has time: " << ping.time_string_ << ", time stamp: " << ping.time_stamp_ << endl; - if (pos == entries.end()) { - //cout << "Found only last entry with time: " << entries.back().time_string_ << ", time stamp: " << entries.back().time_stamp_ << endl; - new_ping.pos_ = entries.back().pos_; - new_ping.heading_ = entries.back().heading_; - new_ping.pitch_ = entries.back().pitch_; - new_ping.roll_ = entries.back().roll_; - } - else { - if (pos == entries.begin()) { - //cout << "Found only first entry with time: " << pos->time_string_ << ", time stamp: " << pos->time_stamp_ << endl; - new_ping.pos_ = pos->pos_; - if (ping.heading_ == 0) { - new_ping.heading_ = pos->heading_; - new_ping.pitch_ = pos->pitch_; - new_ping.roll_ = pos->roll_; - } - else { - new_ping.heading_ = ping.heading_; - new_ping.pitch_ = ping.pitch_; - new_ping.roll_ = ping.roll_; - } - } - else { - //cout << "Found entry with time: " << pos->time_string_ << ", time stamp: " << pos->time_stamp_ << endl; - csv_nav_entry& previous = *(pos - 1); - double ratio = double(ping.time_stamp_ - previous.time_stamp_)/double(pos->time_stamp_ - previous.time_stamp_); - new_ping.pos_ = previous.pos_ + ratio*(pos->pos_ - previous.pos_); - if (ping.heading_ == 0) { - new_ping.heading_ = previous.heading_ + ratio*(pos->heading_ - previous.heading_); - new_ping.pitch_ = previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_); - new_ping.roll_ = previous.roll_ + ratio*(pos->roll_ - previous.roll_); - } - else { - new_ping.heading_ = ping.heading_; - new_ping.pitch_ = ping.pitch_; - new_ping.roll_ = ping.roll_; - //cout << "heading diff: " << previous.yaw_ + ratio*(pos->yaw_ - previous.yaw_) - new_ping.heading_ << endl; - //cout << "pitch diff: " << previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_) - new_ping.pitch_ << endl; - //cout << "roll diff: " << previous.roll_ + ratio*(pos->roll_ - previous.roll_) - new_ping.roll_ << endl; - } - } - } - - for (const Eigen::Vector3d& beam : ping.beams) { - //new_ping.beams.push_back(new_ping.pos_ + beam); - - Eigen::Matrix3d Rx = Eigen::AngleAxisd(new_ping.roll_, Eigen::Vector3d::UnitX()).matrix(); - Eigen::Matrix3d Ry = Eigen::AngleAxisd(new_ping.pitch_, Eigen::Vector3d::UnitY()).matrix(); - Eigen::Matrix3d Rz = Eigen::AngleAxisd(new_ping.heading_, Eigen::Vector3d::UnitZ()).matrix(); - Eigen::Matrix3d R = Rz*Ry*Rx; - - new_ping.beams.push_back(new_ping.pos_ + R*beam); - } - - new_pings.push_back(new_ping); - } - - return new_pings; -} - xtf_sss_ping::PingsT convert_matched_entries_pitch(xtf_sss_ping::PingsT& pings, csv_nav_entry::EntriesT& entries) { xtf_sss_ping::PingsT new_pings; diff --git a/src/data_tools/src/gsf_data.cpp b/src/data_tools/src/gsf_data.cpp index 75944253..3dcc70a1 100644 --- a/src/data_tools/src/gsf_data.cpp +++ b/src/data_tools/src/gsf_data.cpp @@ -22,6 +22,7 @@ using namespace std; namespace gsf_data { using namespace std_data; +using namespace csv_data; void match_sound_speeds(gsf_mbes_ping::PingsT& pings, gsf_sound_speed::SpeedsT& speeds) { @@ -197,6 +198,85 @@ mbes_ping::PingsT convert_pings(gsf_mbes_ping::PingsT& pings) return new_pings; } +mbes_ping::PingsT convert_matched_entries(gsf_mbes_ping::PingsT& pings, csv_nav_entry::EntriesT& entries) +{ + mbes_ping::PingsT new_pings; + + std::stable_sort(entries.begin(), entries.end(), [](const csv_nav_entry& entry1, const csv_nav_entry& entry2) { + return entry1.time_stamp_ < entry2.time_stamp_; + }); + + auto pos = entries.begin(); + for (gsf_mbes_ping& ping : pings) { + pos = std::find_if(pos, entries.end(), [&](const csv_nav_entry& entry) { + return entry.time_stamp_ > ping.time_stamp_; + }); + + mbes_ping new_ping; + new_ping.time_stamp_ = ping.time_stamp_; + new_ping.time_string_ = ping.time_string_; + new_ping.first_in_file_ = ping.first_in_file_; + //cout << "Ping has time: " << ping.time_string_ << ", time stamp: " << ping.time_stamp_ << endl; + if (pos == entries.end()) { + //cout << "Found only last entry with time: " << entries.back().time_string_ << ", time stamp: " << entries.back().time_stamp_ << endl; + new_ping.pos_ = entries.back().pos_; + new_ping.heading_ = entries.back().heading_; + new_ping.pitch_ = entries.back().pitch_; + new_ping.roll_ = entries.back().roll_; + } + else { + if (pos == entries.begin()) { + //cout << "Found only first entry with time: " << pos->time_string_ << ", time stamp: " << pos->time_stamp_ << endl; + new_ping.pos_ = pos->pos_; + if (ping.heading_ == 0) { + new_ping.heading_ = pos->heading_; + new_ping.pitch_ = pos->pitch_; + new_ping.roll_ = pos->roll_; + } + else { + new_ping.heading_ = ping.heading_; + new_ping.pitch_ = ping.pitch_; + new_ping.roll_ = ping.roll_; + } + } + else { + //cout << "Found entry with time: " << pos->time_string_ << ", time stamp: " << pos->time_stamp_ << endl; + csv_nav_entry& previous = *(pos - 1); + double ratio = double(ping.time_stamp_ - previous.time_stamp_)/double(pos->time_stamp_ - previous.time_stamp_); + new_ping.pos_ = previous.pos_ + ratio*(pos->pos_ - previous.pos_); + if (ping.heading_ == 0) { + new_ping.heading_ = previous.heading_ + ratio*(pos->heading_ - previous.heading_); + new_ping.pitch_ = previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_); + new_ping.roll_ = previous.roll_ + ratio*(pos->roll_ - previous.roll_); + } + else { + new_ping.heading_ = ping.heading_; + new_ping.pitch_ = ping.pitch_; + new_ping.roll_ = ping.roll_; + //cout << "heading diff: " << previous.yaw_ + ratio*(pos->yaw_ - previous.yaw_) - new_ping.heading_ << endl; + //cout << "pitch diff: " << previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_) - new_ping.pitch_ << endl; + //cout << "roll diff: " << previous.roll_ + ratio*(pos->roll_ - previous.roll_) - new_ping.roll_ << endl; + } + } + } + + for (const Eigen::Vector3d& beam : ping.beams) { + //new_ping.beams.push_back(new_ping.pos_ + beam); + + Eigen::Matrix3d Rx = Eigen::AngleAxisd(new_ping.roll_, Eigen::Vector3d::UnitX()).matrix(); + Eigen::Matrix3d Ry = Eigen::AngleAxisd(new_ping.pitch_, Eigen::Vector3d::UnitY()).matrix(); + Eigen::Matrix3d Rz = Eigen::AngleAxisd(new_ping.heading_, Eigen::Vector3d::UnitZ()).matrix(); + Eigen::Matrix3d R = Rz*Ry*Rx; + + new_ping.beams.push_back(new_ping.pos_ + R*beam); + } + + new_pings.push_back(new_ping); + } + + return new_pings; +} + } // namespace gsf_data namespace std_data { diff --git a/src/data_tools/src/lat_long_utm.cpp b/src/data_tools/src/lat_long_utm.cpp index a68aad97..7119abf1 100644 --- a/src/data_tools/src/lat_long_utm.cpp +++ b/src/data_tools/src/lat_long_utm.cpp @@ -11,7 +11,11 @@ #include +#define _USE_MATH_DEFINES #include +#ifndef M_PI // For windows + #define M_PI 3.14159265358979323846 +#endif using namespace std; diff --git a/src/data_tools/src/xtf_data.cpp b/src/data_tools/src/xtf_data.cpp index d1757f24..542819e8 100644 --- a/src/data_tools/src/xtf_data.cpp +++ b/src/data_tools/src/xtf_data.cpp @@ -20,7 +20,11 @@ extern "C" { //#include #include -#include +#ifdef _MSC_VER + #include +#else + #include +#endif //#include //#include #include @@ -123,7 +127,7 @@ xtf_sss_ping process_side_scan_ping(XTFPINGHEADER *PingHeader, XTFFILEHEADER *XT ping.heading_ = 0.5*M_PI-ping.heading_; // TODO: need to keep this for old data ping.sound_vel_ = PingHeader->SoundVelocity; - boost::posix_time::ptime data_time(boost::gregorian::date(PingHeader->Year, PingHeader->Month, PingHeader->Day), boost::posix_time::hours(PingHeader->Hour)+boost::posix_time::minutes(PingHeader->Minute)+boost::posix_time::seconds(PingHeader->Second)+boost::posix_time::milliseconds(10.*int(PingHeader->HSeconds))); + boost::posix_time::ptime data_time(boost::gregorian::date(PingHeader->Year, PingHeader->Month, PingHeader->Day), boost::posix_time::hours(PingHeader->Hour)+boost::posix_time::minutes(PingHeader->Minute)+boost::posix_time::seconds(PingHeader->Second)+boost::posix_time::milliseconds(10*int(PingHeader->HSeconds))); stringstream time_ss; time_ss << data_time; ping.time_string_ = time_ss.str(); diff --git a/src/eigen_cereal/include/eigen_cereal/eigen_cereal.h b/src/eigen_cereal/include/eigen_cereal/eigen_cereal.h index 46c99aed..848e854e 100644 --- a/src/eigen_cereal/include/eigen_cereal/eigen_cereal.h +++ b/src/eigen_cereal/include/eigen_cereal/eigen_cereal.h @@ -3,7 +3,7 @@ #include #include -#include +#include #include namespace cereal diff --git a/src/libxtf/include/libxtf/xtf.h b/src/libxtf/include/libxtf/xtf.h index 05574799..b5ce79f3 100644 --- a/src/libxtf/include/libxtf/xtf.h +++ b/src/libxtf/include/libxtf/xtf.h @@ -197,12 +197,18 @@ typedef unsigned int DWORD; typedef unsigned int UINT; typedef int LONG; +#ifdef _MSC_VER + #define PACK( __Declaration__ ) __pragma( pack(push, 1) ) __Declaration__ __pragma( pack(pop) ) +#else + #define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__)) +#endif + // Channel information structure (contained in the file header). // One-time information describing each channel. 64 bytes long. // This is data pertaining to each channel that will not change // during the course of a run. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { BYTE TypeOfChannel; // PORT, STBD, SBOT or BATH BYTE SubChannelNumber; WORD CorrectionFlags; // 1=raw, 2=Corrected @@ -249,14 +255,14 @@ typedef struct { char ReservedArea2[54]; -} __attribute__((packed)) CHANINFO; +}) CHANINFO; // XTF File header. // Total of 1024 bytes. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { BYTE FileFormat; // 50 for Q-MIPS file format, 51 for Isis format BYTE SystemType; // Type of system used to record this file. 202=Isis char RecordingProgramName[8]; // Example: "Isis" @@ -317,7 +323,7 @@ typedef struct { CHANINFO ChanInfo[6]; // Each CHANINFO struct is 128 bytes. // If more than 6 channels needed, header record // grows 1K in size for each additional 8 channels. -} __attribute__((packed)) XTFFILEHEADER; +}) XTFFILEHEADER; // The XTFATTITUDEDATA structure used to store information from a TSS or @@ -330,7 +336,7 @@ typedef struct { // // Attitude data packet, 64 bytes in length. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { // // Type of header // @@ -369,14 +375,14 @@ typedef struct { BYTE Reserved3[10]; -} __attribute__((packed)) XTFATTITUDEDATA; +}) XTFATTITUDEDATA; // Sonar or Bathy Ping header // The data here can change from ping to ping but will pertain to all // channels that are at the same time as this ping. 256 bytes in length. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { // // Type of header @@ -599,7 +605,7 @@ typedef struct { // BYTE ReservedSpace2[11]; // Currently unused -} __attribute__((packed)) XTFPINGHEADER, XTFBATHHEADER; +}) XTFPINGHEADER, XTFBATHHEADER; @@ -610,7 +616,7 @@ typedef struct { // may be printed in realtime or in playback. This can be activated // in the Print Annotation dialog box. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { WORD MagicNumber; // Set to 0xFACE BYTE HeaderType; // XTF_HEADER_NOTES (1) @@ -632,7 +638,7 @@ typedef struct { char NotesText[256-56]; -} __attribute__((packed)) XTFNOTESHEADER; +}) XTFNOTESHEADER; // RAW ASCII data received over serial port @@ -643,7 +649,7 @@ typedef struct { // data for all usefull information. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { WORD MagicNumber; // Set to 0xFACE BYTE HeaderType; // will be XTF_HEADER_RAW_SERIAL (7) @@ -668,14 +674,14 @@ typedef struct { char RawAsciiData[64-30]; // will be padded in 64-byte increments to make // structure an even multiple of 64 bytes -} __attribute__((packed)) XTFRAWSERIALHEADER; +}) XTFRAWSERIALHEADER; // Ping Channel header // This is data that can be unique to each channel from ping to ping. // Is is stored at the front of each channel of sonar data. /////////////////////////////////////////////////////////////////////////////// -typedef struct { +PACK(typedef struct { WORD ChannelNumber; // Typically, // 0=port (low frequency) @@ -732,7 +738,9 @@ typedef struct { BYTE ReservedSpace[4]; // reserved for future expansion //BYTE ReservedSpace[6]; // reserved for future expansion -} __attribute__((packed)) XTFPINGCHANHEADER; +}) XTFPINGCHANHEADER; //} XTFPINGCHANHEADER; +#undef PACK + #endif // XTF_H diff --git a/src/libxtf/src/demo_xtf_reader.cpp b/src/libxtf/src/demo_xtf_reader.cpp index e5e0c6f0..2b4b8a6f 100644 --- a/src/libxtf/src/demo_xtf_reader.cpp +++ b/src/libxtf/src/demo_xtf_reader.cpp @@ -2,7 +2,11 @@ #include #include #include -#include +#ifdef _MSC_VER + #include +#else + #include +#endif #include #include extern "C" { diff --git a/src/pybathy_maps/src/pymap_draper.cpp b/src/pybathy_maps/src/pymap_draper.cpp index 64415116..9dff2719 100644 --- a/src/pybathy_maps/src/pymap_draper.cpp +++ b/src/pybathy_maps/src/pymap_draper.cpp @@ -32,7 +32,7 @@ PYBIND11_MODULE(map_draper, m) { py::class_(m, "sss_map_image", "Class for sidescan views of a patch from different survey lines") .def(py::init<>()) .def_readwrite("bounds", &sss_map_image::bounds, "Member") - .def_readwrite("sss_map_image", &sss_map_image::sss_map_image, "Member") + .def_readwrite("sss_map_image", &sss_map_image::sss_map_image_, "Member") .def_readwrite("sss_waterfall_image", &sss_map_image::sss_waterfall_image, "Member") .def_readwrite("sss_waterfall_cross_track", &sss_map_image::sss_waterfall_cross_track, "Member") .def_readwrite("sss_waterfall_depth", &sss_map_image::sss_waterfall_depth, "Member") diff --git a/src/pydata_tools/CMakeLists.txt b/src/pydata_tools/CMakeLists.txt index b7096d26..371371f9 100644 --- a/src/pydata_tools/CMakeLists.txt +++ b/src/pydata_tools/CMakeLists.txt @@ -4,7 +4,9 @@ message("Python dir: " ${PYTHON_INSTALL_PATH}) #pybind11_add_module(pystd_data src/pystd_data.cpp) add_library(pystd_data MODULE src/pystd_data.cpp) -add_library(pygsf_data MODULE src/pygsf_data.cpp) +if(AUVLIB_WITH_GSF) + add_library(pygsf_data MODULE src/pygsf_data.cpp) +endif() add_library(pyxtf_data MODULE src/pyxtf_data.cpp) add_library(pycsv_data MODULE src/pycsv_data.cpp) add_library(pyall_data MODULE src/pyall_data.cpp) @@ -16,10 +18,12 @@ target_include_directories(pystd_data PUBLIC $ PRIVATE src) -target_include_directories(pygsf_data PUBLIC - $ - $ - PRIVATE src) +if(AUVLIB_WITH_GSF) + target_include_directories(pygsf_data PUBLIC + $ + $ + PRIVATE src) +endif() target_include_directories(pyxtf_data PUBLIC $ @@ -48,15 +52,17 @@ target_include_directories(pybenchmark PUBLIC #target_link_libraries(pystd_data std_data eigen_cereal) -target_link_libraries(pystd_data PRIVATE std_data eigen_cereal ${BOOST_LIBRARIES} pybind11::module) +target_link_libraries(pystd_data PRIVATE std_data navi_data eigen_cereal ${BOOST_LIBRARIES} pybind11::module) set_target_properties(pystd_data PROPERTIES PREFIX "${PYTHON_MODULE_PREFIX}" OUTPUT_NAME "std_data" SUFFIX "${PYTHON_MODULE_EXTENSION}") -target_link_libraries(pygsf_data PRIVATE std_data gsf_data eigen_cereal ${BOOST_LIBRARIES} pybind11::module) -set_target_properties(pygsf_data PROPERTIES PREFIX "${PYTHON_MODULE_PREFIX}" - OUTPUT_NAME "gsf_data" - SUFFIX "${PYTHON_MODULE_EXTENSION}") +if(AUVLIB_WITH_GSF) + target_link_libraries(pygsf_data PRIVATE std_data gsf_data eigen_cereal ${BOOST_LIBRARIES} pybind11::module) + set_target_properties(pygsf_data PROPERTIES PREFIX "${PYTHON_MODULE_PREFIX}" + OUTPUT_NAME "gsf_data" + SUFFIX "${PYTHON_MODULE_EXTENSION}") +endif() target_link_libraries(pyxtf_data PRIVATE std_data xtf_data eigen_cereal ${BOOST_LIBRARIES} pybind11::module) set_target_properties(pyxtf_data PROPERTIES PREFIX "${PYTHON_MODULE_PREFIX}" @@ -83,7 +89,12 @@ set_target_properties(pybenchmark PROPERTIES PREFIX "${PYTHON_MODULE_PREFIX}" OUTPUT_NAME "benchmark" SUFFIX "${PYTHON_MODULE_EXTENSION}") -install(TARGETS pystd_data pygsf_data pyxtf_data pycsv_data pyall_data pyxyz_data pybenchmark +set(AUVLIB_PYDATA_TOOLS_LIBS pystd_data pyxtf_data pycsv_data pyall_data pyxyz_data pybenchmark) +if(AUVLIB_WITH_GSF) + set(AUVLIB_PYDATA_TOOLS_LIBS ${AUVLIB_PYDATA_TOOLS_LIBS} pygsf_data) +endif() + +install(TARGETS ${AUVLIB_PYDATA_TOOLS_LIBS} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}/auvlib/data_tools ) # This is for Windows diff --git a/src/pydata_tools/src/pystd_data.cpp b/src/pydata_tools/src/pystd_data.cpp index 464988a1..caf75321 100644 --- a/src/pydata_tools/src/pystd_data.cpp +++ b/src/pydata_tools/src/pystd_data.cpp @@ -10,6 +10,7 @@ */ #include +#include #include #include diff --git a/src/sonar_tracing/CMakeLists.txt b/src/sonar_tracing/CMakeLists.txt index 113c5ca0..312aeb55 100644 --- a/src/sonar_tracing/CMakeLists.txt +++ b/src/sonar_tracing/CMakeLists.txt @@ -14,6 +14,10 @@ project(sonar_tracing) # CMAKE_ARGS -DEMBREE_ISPC_SUPPORT=OFF #) +if(NOT MSVC) + find_package(Ceres REQUIRED) + include_directories(${CERES_INCLUDE_DIRS}) +endif() #include_directories(../../libigl/external/glad/include) #include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../../libigl/external/embree/include) diff --git a/src/sonar_tracing/include/sonar_tracing/snell_ray_tracing.h b/src/sonar_tracing/include/sonar_tracing/snell_ray_tracing.h index 8cf2acb0..6c03b089 100644 --- a/src/sonar_tracing/include/sonar_tracing/snell_ray_tracing.h +++ b/src/sonar_tracing/include/sonar_tracing/snell_ray_tracing.h @@ -12,32 +12,8 @@ #ifndef SNELL_RAY_TRACING_H #define SNELL_RAY_TRACING_H -#include -#include +#include -class LayerWidthCostFunctor { -public: - LayerWidthCostFunctor(double height, double speed) : height(height), speed(speed) - { - } - - template - bool operator()(const T* const x1, const T* const x2, T* e) const - { - e[0] = pow((*x2-*x1)*(*x2-*x1)+T(height*height), 0.25)/T(sqrt(speed)); - return true; - } - - static ceres::CostFunction* Create(double height, double speed) - { - return new ceres::AutoDiffCostFunction( - new LayerWidthCostFunctor(height, speed)); - } - -private: - double height; - double speed; -}; // here, we assume that the origin is at (0, 0), and the points all have x > 0 std::pair trace_multiple_layers(const Eigen::VectorXd& layer_depths, const Eigen::VectorXd& layer_speeds, const Eigen::MatrixXd& end_points); diff --git a/src/sonar_tracing/src/snell_ray_tracing.cpp b/src/sonar_tracing/src/snell_ray_tracing.cpp index b59badff..32eec81f 100644 --- a/src/sonar_tracing/src/snell_ray_tracing.cpp +++ b/src/sonar_tracing/src/snell_ray_tracing.cpp @@ -15,6 +15,10 @@ #include #include +#ifndef _MSC_VER + #include +#endif + using namespace std; // here, we assume that the origin is at (0, 0), and the points all have x > 0 @@ -33,8 +37,35 @@ pair trace_multiple_layers(const Eigen::Vector return make_pair(end_times, layer_widths); } +#ifndef _MSC_VER +class LayerWidthCostFunctor { +public: + LayerWidthCostFunctor(double height, double speed) : height(height), speed(speed) + { + } + + template + bool operator()(const T* const x1, const T* const x2, T* e) const + { + e[0] = pow((*x2-*x1)*(*x2-*x1)+T(height*height), 0.25)/T(sqrt(speed)); + return true; + } + + static ceres::CostFunction* Create(double height, double speed) + { + return new ceres::AutoDiffCostFunction( + new LayerWidthCostFunctor(height, speed)); + } + +private: + double height; + double speed; +}; +#endif + pair trace_single_layers(const Eigen::VectorXd& layer_depths, const Eigen::VectorXd& layer_speeds, const Eigen::Vector2d& end_point) { +#ifndef _MSC_VER Eigen::VectorXd layer_widths(layer_depths.rows()+2); layer_widths(0) = 0.; @@ -92,6 +123,8 @@ pair trace_single_layers(const Eigen::VectorXd& layer_d // return the final ray time and the optimized intermediary points return make_pair(2.*summary.final_cost, layer_widths); +#endif + return make_pair(0., Eigen::VectorXd::Zero(layer_depths.rows())); } void visualize_rays(const Eigen::MatrixXd& end_points, const Eigen::VectorXd& layer_depths,