├── .gitignore ├── CMakeLists.txt ├── README.md ├── cmake └── FindEigen3.cmake ├── examples ├── okvis_dataset_example │ ├── CMakeLists.txt │ └── src │ │ ├── okvis_data_reader.cpp │ │ ├── okvis_data_reader.hpp │ │ └── okvis_dataset_example_main.cpp └── simple_example │ ├── CMakeLists.txt │ └── src │ ├── main.cpp │ └── obj.data ├── include └── mesh_based_mapping │ ├── mesh_based_mapping.hpp │ └── utils │ ├── file_io.hpp │ ├── mesh_based_mapping_okvis_wrapper.hpp │ └── triangle_rasterization.hpp ├── src └── mesh_based_mapping.cpp └── third_party ├── fade_v1.46a ├── include_fade2d │ ├── Bbox2.h │ ├── Circle2.h │ ├── Color.h │ ├── ConstraintGraph2.h │ ├── ConstraintSegment2.h │ ├── Edge2.h │ ├── Fade_2D.h │ ├── Label.h │ ├── License.h │ ├── MeshGenParams.h │ ├── MsgBase.h │ ├── Performance.h │ ├── Point2.h │ ├── Publisher.h │ ├── Segment2.h │ ├── SegmentChecker.h │ ├── Triangle2.h │ ├── TriangleAroundVertexIterator.h │ ├── Vector2.h │ ├── Visualizer2.h │ ├── Zone2.h │ ├── common.h │ ├── fadeVersion.h │ └── testDataGenerators.h ├── lib_ubuntu14.04_i686 │ └── libfade2d.so ├── lib_ubuntu14.04_x86_64 │ └── libfade2d.so ├── lib_ubuntu16.10_x86_64 │ └── libfade2d.so └── readme.txt ├── minkindr └── include │ └── kindr │ └── minimal │ ├── angle-axis.h │ ├── common.h │ ├── implementation │ ├── angle-axis-inl.h │ ├── quat-sim-transform-inl.h │ ├── quat-transformation-inl.h │ └── rotation-quaternion-inl.h │ ├── position.h │ ├── quat-sim-transform.h │ ├── quat-transformation.h │ └── rotation-quaternion.h ├── okvis_kinematics ├── CMakeLists.txt ├── include │ └── okvis │ │ └── kinematics │ │ ├── FrameTypedefs.hpp │ │ ├── Transformation.hpp │ │ ├── implementation │ │ └── Transformation.hpp │ │ └── operators.hpp ├── src │ └── dependency-tracker.cc └── test │ ├── TestTransformation.cpp │ └── runTests.cpp └── v4rl_dataset_lib ├── CMakeLists.txt ├── include ├── asl_dataset_helper.h ├── okvis_features_dataset.hpp └── optimization_window_dataset.hpp ├── src └── asl_dataset_helper.cpp └── test └── test_asl_dataset_helper.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | 34 | build/* 35 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.0) 2 | project(v4rl_mesh_based_mapping_lib) 3 | 4 | #set(CMAKE_BUILD_TYPE Debug) 5 | 6 | set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake" ${CMAKE_MODULE_PATH}) 7 | set(Boost_USE_MULTITHREADED ON) 8 | 9 | find_package( Eigen3 REQUIRED ) 10 | find_package( OpenCV REQUIRED core highgui) # contrib 11 | 12 | include_directories( ${EIGEN3_INCLUDE_DIR} ) 13 | include_directories( ${OpenCV_INCLUDE_DIRS} ) 14 | 15 | ADD_DEFINITIONS (-march=native -std=c++11 ) 16 | 17 | add_library(${PROJECT_NAME} STATIC src/mesh_based_mapping.cpp ${HPPS}) 18 | target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 19 | target_include_directories(${PROJECT_NAME} PRIVATE ${CMAKE_SOURCE_DIR}/third_party/fade_v1.46a/include_fade2d) 20 | 21 | set(FADE_LIB_VERSION "lib_ubuntu16.10_x86_64")# lib_ubuntu16.10_x86_64 or lib_ubuntu14.04_x86_64 or lib_ubuntu14.04_i686 22 | 23 | 24 | add_subdirectory(examples/simple_example) 25 | 26 | add_subdirectory(examples/okvis_dataset_example) 27 | 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Code & Dataset for mesh-based scene estimation 2 | This work is described in the paper "Real-Time Mesh-based Scene Estimation for Aerial Inspection", by Lucas Teixeira and Margarita Chli, published in the Proceedings of the IEEE/RSJ Conference on Intelligent Robots and Systems (IROS) 2016 [[paper](http://ieeexplore.ieee.org/document/7759714/)]. 3 | 4 | #### Video: 5 | Mesh 7 | 8 | If you use this Code or Dataset, please cite the following publication: 9 | 10 | ``` 11 | @inproceedings{Teixeira:etal:IROS2016, 12 | title = {{Real-Time Mesh-based Scene Estimation for Aerial Inspection}}, 13 | author = {Lucas Teixeira and Margarita Chli}, 14 | booktitle = {Proceedings of the {IEEE/RSJ} Conference on Intelligent Robots and Systems({IROS})}, 15 | year = {2016} 16 | } 17 | ``` 18 | 19 | ## Code 20 | The original code was developed inside [OKVIS](https://github.com/ethz-asl/okvis) VI-SLAM system and can be found [Here](https://raw.githubusercontent.com/VIS4ROB-lab/mesh_based_mapping/). 21 | This code is a standone version that can be integradeted to other SLAM system. We already sucessfully tested it with the ORB-SLAM output. A new topic has been added to publish a point cloud that is sampled over the mesh that is being constructed. 22 | 23 | ### License 24 | This code can be used for research purposes for free under a GPLv3 license, but we use two libraries that are not: Fade2D is a commercial software that can be used for research proposes for free (please check their website http://www.geom.at/), while [Scratchapixel.com](http://www.scratchapixel.com/lessons/3d-basic-rendering/rasterization-practical-implementation) requests GPLv3 license. We will be working to remove this latter dependence in the future. 25 | 26 | ### Environment 27 | This code is tested for Ubuntu 14.04 and 16.06. If you want to run it on a different configuration you will have to change the pointer to the Fade2D library in the main CMakeLists. Note that the Fade2D binaries are included in this code. 28 | 29 | 30 | ## ETHZ_V4RL-CAB Dataset 31 | The dataset used in the paper can be found below 32 | 33 | ### Examples 34 | Aerial 1 36 | Aerial 2 38 | Aerial 3 40 | Ground 42 | 43 | 44 | ### Data 45 | **Aerial 1** - [Bagfile](https://drive.google.com/open?id=0B82ekrhU9sDmTTdIeFJXTlBBLVE) - [Youtube](http://www.youtube.com/embed/SA4KoRjvx04) 46 | 47 | **Aerial 2** - [Bagfile](https://drive.google.com/open?id=0B82ekrhU9sDmNjZiMTUxUWlHcnc) - [Youtube](http://www.youtube.com/embed/FEQiClIlLZI) 48 | 49 | **Aerial 3** - [Bagfile](https://drive.google.com/open?id=0B82ekrhU9sDmOUkzX2xrMWRSMEE) - [Youtube](http://www.youtube.com/embed/HLIJ59BRaBo) 50 | 51 | **Ground** - [Bagfile](https://drive.google.com/open?id=0B82ekrhU9sDmTjVweklrNGdJTjA) - [Youtube](http://www.youtube.com/embed/a-ITwYMPzZs) 52 | 53 | 54 | ### Calibration 55 | The images were captured using a [VI-Sensor](http://wiki.ros.org/vi_sensor) and calibrated using [ETHZ ASL Kalibr](https://github.com/ethz-asl/kalibr). Below are the calibration parameters. Note that T_SC is the transformation from the Camera to the Sensor (IMU). The two sets of values correspond to camera0's intrinsics and camera1's intrinsics, respectively. 56 | 57 | ```python 58 | 59 | cameras: 60 | - {T_SC: 61 | [ 0.9999921569165363, 0.003945890103835121, 0.0003406709575200133, -0.030976405894694664, 62 | -0.003948017768440125, 0.9999711543561547, 0.0064887295612456805, 0.003944069243840622, 63 | -0.00031505731688472255, -0.0064900236445916415, 0.9999788899431723, -0.016723945219020563, 64 | 0.0, 0.0, 0.0, 1.0], 65 | image_dimension: [752, 480], 66 | distortion_coefficients: [0.0038403216668672986, 0.025065957244781098, -0.05227986912373674, 0.03635919730588422], 67 | distortion_type: equidistant, 68 | focal_length: [464.2604856754006, 463.0164764480498], 69 | principal_point: [372.2582270417875, 235.05442086962864]} 70 | - {T_SC: 71 | [ 0.9999722760516375, 0.007329193771005421, 0.0013153124248847282, 0.0790982900835488, 72 | -0.007335059837348439, 0.9999629194070246, 0.004511840884063492, 0.003549628903031918, 73 | -0.0012821954962168199, -0.00452136369336114, 0.9999889565615523, -0.01713313929463862, 74 | 0.0, 0.0, 0.0, 1.0], 75 | image_dimension: [752, 480], 76 | distortion_coefficients: [0.00823121215582322, -0.015270152487108836, 0.03085334360639285, -0.017760720995454376], 77 | distortion_type: equidistant, 78 | focal_length: [456.3683366282091, 455.03924786357857], 79 | principal_point: [375.1783411236692, 238.22971133267725]} 80 | ``` 81 | ## Qualitative Results 82 | You can reproduce the qualitative results of our [paper](http://ieeexplore.ieee.org/document/7759714/) using this [bagfile](https://drive.google.com/open?id=0B82ekrhU9sDmT3hiV3pPakdrTXc) and this ground-truth [laser scan](https://drive.google.com/open?id=0B82ekrhU9sDmN2QyOFlFNHA5c2c). You can also visualize the point-cloud inside the RViz software together with the laser scan of the facade. You only need to use *rosbag play* to run the bagfile. You can use the ros node *read* from the package [ethz-asl:point_cloud_io]( 83 | https://github.com/ethz-asl/point_cloud_io). 84 | 85 | 86 | ## Contact 87 | For any questions or bug reports, please create an issue if you have questions or bug reports. Alternatively, you can also contact me at lteixeira@mavt.ethz.ch. 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /cmake/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # Once done this will define 3 | # 4 | # EIGEN3_FOUND - system has eigen lib 5 | # EIGEN3_INCLUDE_DIR - the eigen include directory 6 | 7 | # Copyright (c) 2006, 2007 Montel Laurent, 8 | # Redistribution and use is allowed according to the terms of the BSD license. 9 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 10 | 11 | if( EIGEN3_INCLUDE_DIR ) 12 | # in cache already 13 | set( EIGEN3_FOUND TRUE ) 14 | else (EIGEN3_INCLUDE_DIR) 15 | find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core 16 | PATH_SUFFIXES eigen3/ 17 | HINTS 18 | ${INCLUDE_INSTALL_DIR} 19 | /usr/local/include 20 | ${KDE4_INCLUDE_DIR} 21 | ) 22 | include( FindPackageHandleStandardArgs ) 23 | find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) 24 | mark_as_advanced( EIGEN3_INCLUDE_DIR ) 25 | endif(EIGEN3_INCLUDE_DIR) 26 | 27 | -------------------------------------------------------------------------------- /examples/okvis_dataset_example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | ADD_DEFINITIONS (-march=native -std=c++11 ) 3 | 4 | 5 | 6 | find_package( Boost COMPONENTS system filesystem thread REQUIRED ) 7 | 8 | 9 | include_directories( ${Boost_INCLUDE_DIRS} ) 10 | include_directories( ${CMAKE_SOURCE_DIR}/third_party/minkindr/include ) 11 | include_directories( ${CMAKE_SOURCE_DIR}/third_party/v4rl_dataset_lib/include ) 12 | 13 | FILE( 14 | GLOB HPPS 15 | src/*.hpp 16 | ) 17 | 18 | FILE( 19 | GLOB CPPS 20 | src/*.cpp 21 | ) 22 | 23 | 24 | add_executable(okvis_dataset_example ${CPPS} ${HPPS}) 25 | 26 | target_link_libraries(okvis_dataset_example 27 | ${OpenCV_LIBS} 28 | v4rl_mesh_based_mapping_lib 29 | ${Boost_LIBRARIES} 30 | glog 31 | ${CMAKE_SOURCE_DIR}/third_party/fade_v1.46a/${FADE_LIB_VERSION}/libfade2d.so ) 32 | 33 | -------------------------------------------------------------------------------- /examples/okvis_dataset_example/src/okvis_data_reader.cpp: -------------------------------------------------------------------------------- 1 | #include "okvis_data_reader.hpp" 2 | #include 3 | 4 | OkvisDataReader::OkvisDataReader() { 5 | } 6 | 7 | OkvisDataReader::~OkvisDataReader() { 8 | 9 | } 10 | 11 | bool OkvisDataReader::GetNextSample(FeatureLandmarkFrameData &data) { 12 | 13 | uint64_t timestamp; 14 | 15 | do { 16 | if (next_sample_id_ >= timestamps_.size()) { 17 | return false; 18 | } 19 | 20 | timestamp = timestamps_[next_sample_id_]; 21 | next_sample_id_++; 22 | } while (!GetSample(timestamp, data)); 23 | 24 | return true; 25 | } 26 | 27 | bool OkvisDataReader::GetSample(uint64_t timestamp, 28 | FeatureLandmarkFrameData &data) { 29 | 30 | OkvisFeaturesEntry okvisFeaturesEntry; 31 | 32 | if (!features_dataset_.getOkvisFeaturesEntry(timestamp, okvisFeaturesEntry)) { 33 | return false; 34 | } 35 | 36 | if (!features_dataset_.readKeypointsFile(okvisFeaturesEntry, data.keypoints, 37 | data.landmarks_id)) { 38 | return false; 39 | } 40 | 41 | OptimizationWindowEntry optEntry; 42 | 43 | if (!opt_window_dataset_.getOptimizationWindowEntry(timestamp, optEntry)) { 44 | return false; 45 | } 46 | 47 | if (!opt_window_dataset_.readLandmarksFile(optEntry, data.landmarks)) { 48 | return false; 49 | } 50 | 51 | data.timestamp = timestamp; 52 | 53 | data.t_ws = optEntry.t_ws; 54 | 55 | kindr::minimal::QuatTransformation T_WC; 56 | T_WC = data.t_ws * t_sc; 57 | kindr::minimal::QuatTransformation T_CW = T_WC.inverse(); 58 | 59 | //std::cout << T_CW << std::endl; 60 | 61 | //convert from world to camera frame 62 | for (auto it = data.landmarks.begin(); it != data.landmarks.end(); ++it) { 63 | // std::cout << 0.25<<"," << it->second.pos[0] <<","<second.pos[1] <<","<second.pos[2] << std::endl; 64 | it->second.pos = T_CW.transform(it->second.pos); 65 | // std::cout << 1.0<<"," << it->second.pos[0] <<","<second.pos[1] <<","<second.pos[2] << std::endl; 66 | } 67 | return true; 68 | } 69 | 70 | bool OkvisDataReader::Init(std::string features_dataset_filename, 71 | std::string opt_window_dataset_filename) { 72 | next_sample_id_ = 0; 73 | 74 | if (features_dataset_.load(features_dataset_filename) == 0) { 75 | return false; 76 | } 77 | 78 | if (opt_window_dataset_.load(opt_window_dataset_filename) == 0) { 79 | return false; 80 | } 81 | 82 | Eigen::Matrix4d T_SC_Eigen; //TODO(@weblucas) read from file 83 | // T_SC_Eigen = Eigen::MatrixXd::Identity(4,4); 84 | // T_SC_Eigen << 0.9997754002442455 , -0.021161371053313276, 85 | // -0.0011599317232830833, -0.03742703361193287 , 86 | // 0.021167568626536626 , 0.999760145165724 , 0.00562015806284527 , 87 | // 0.0059817697251900205, 88 | // 0.0010407232579056848, -0.005643448711071745, 0.9999835340553093 , 89 | // 0.0005720705107382817, 90 | // 0.0, 0.0, 0.0, 1.0; 91 | 92 | T_SC_Eigen << 0, 0, 1, 0, 93 | -1, 0, 0, 0, 94 | 0,-1, 0, 0, 95 | 0, 0, 0, 1; 96 | 97 | 98 | 99 | t_sc = kindr::minimal::QuatTransformation(T_SC_Eigen); 100 | 101 | InitTimestamps(); 102 | return true; 103 | } 104 | 105 | void OkvisDataReader::InitTimestamps() { 106 | for (auto it = features_dataset_.index.begin(); 107 | it != features_dataset_.index.end(); ++it) { 108 | timestamps_.push_back(it->first); 109 | } 110 | } 111 | -------------------------------------------------------------------------------- /examples/okvis_dataset_example/src/okvis_data_reader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OKVIS_DATA_READER_MESH_MAPPING_H_ 2 | #define OKVIS_DATA_READER_MESH_MAPPING_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | struct FeatureLandmarkFrameData 10 | { 11 | uint64_t timestamp; 12 | std::vector keypoints; 13 | std::vector landmarks_id; 14 | OkvisLandmarkMap landmarks; 15 | kindr::minimal::QuatTransformation t_ws; 16 | }; 17 | 18 | class OkvisDataReader 19 | { 20 | public: 21 | OkvisDataReader(); 22 | ~OkvisDataReader(); 23 | 24 | bool GetNextSample( FeatureLandmarkFrameData& data ) { 25 | 26 | uint64_t timestamp; 27 | 28 | do { 29 | if (next_sample_id_ >= timestamps_.size()) { 30 | return false; 31 | } 32 | 33 | timestamp = timestamps_[next_sample_id_]; 34 | next_sample_id_++; 35 | } while (!GetSample(timestamp, data)); 36 | 37 | return true; 38 | } 39 | bool GetSample(uint64_t timestamp, FeatureLandmarkFrameData& data ){ 40 | 41 | OkvisFeaturesEntry okvisFeaturesEntry; 42 | 43 | if (!features_dataset_.getOkvisFeaturesEntry(timestamp, okvisFeaturesEntry)) { 44 | return false; 45 | } 46 | 47 | if (!features_dataset_.readKeypointsFile(okvisFeaturesEntry, data.keypoints, 48 | data.landmarks_id)) { 49 | return false; 50 | } 51 | 52 | OptimizationWindowEntry optEntry; 53 | 54 | if (!opt_window_dataset_.getOptimizationWindowEntry(timestamp, optEntry)) { 55 | return false; 56 | } 57 | 58 | if (!opt_window_dataset_.readLandmarksFile(optEntry, data.landmarks)) { 59 | return false; 60 | } 61 | 62 | data.timestamp = timestamp; 63 | 64 | data.t_ws = optEntry.t_ws; 65 | 66 | kindr::minimal::QuatTransformation T_WC; 67 | T_WC = data.t_ws * t_sc; 68 | kindr::minimal::QuatTransformation T_CW = T_WC.inverse(); 69 | 70 | //std::cout << T_CW << std::endl; 71 | 72 | //convert from world to camera frame 73 | for (auto it = data.landmarks.begin(); it != data.landmarks.end(); ++it) { 74 | // std::cout << 0.25<<"," << it->second.pos[0] <<","<second.pos[1] <<","<second.pos[2] << std::endl; 75 | it->second.pos = T_CW.transform(it->second.pos); 76 | // std::cout << 1.0<<"," << it->second.pos[0] <<","<second.pos[1] <<","<second.pos[2] << std::endl; 77 | } 78 | } 79 | bool Init(std::string features_dataset_filename, std::string opt_window_dataset_filename ){ 80 | next_sample_id_ = 0; 81 | 82 | if (features_dataset_.load(features_dataset_filename) == 0) { 83 | return false; 84 | } 85 | 86 | if (opt_window_dataset_.load(opt_window_dataset_filename) == 0) { 87 | return false; 88 | } 89 | 90 | Eigen::Matrix4d T_SC_Eigen; //TODO(@weblucas) read from file 91 | // T_SC_Eigen = Eigen::MatrixXd::Identity(4,4); 92 | // T_SC_Eigen << 0.9997754002442455 , -0.021161371053313276, 93 | // -0.0011599317232830833, -0.03742703361193287 , 94 | // 0.021167568626536626 , 0.999760145165724 , 0.00562015806284527 , 95 | // 0.0059817697251900205, 96 | // 0.0010407232579056848, -0.005643448711071745, 0.9999835340553093 , 97 | // 0.0005720705107382817, 98 | // 0.0, 0.0, 0.0, 1.0; 99 | 100 | T_SC_Eigen << 0, 0, 1, 0, 101 | -1, 0, 0, 0, 102 | 0,-1, 0, 0, 103 | 0, 0, 0, 1; 104 | 105 | 106 | 107 | t_sc = kindr::minimal::QuatTransformation(T_SC_Eigen); 108 | 109 | InitTimestamps(); 110 | return true; 111 | } 112 | 113 | private: 114 | void InitTimestamps(){ 115 | for (auto it = features_dataset_.index.begin(); 116 | it != features_dataset_.index.end(); ++it) { 117 | timestamps_.push_back(it->first); 118 | } 119 | } 120 | OkvisFeaturesDataset features_dataset_; 121 | OptimizationWindowDataset opt_window_dataset_; 122 | uint64_t next_sample_id_; 123 | std::vector timestamps_; 124 | kindr::minimal::QuatTransformation t_sc; //TODO(@weblucas) read from file 125 | }; 126 | 127 | #endif 128 | -------------------------------------------------------------------------------- /examples/okvis_dataset_example/src/okvis_dataset_example_main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include "okvis_data_reader.hpp" 14 | 15 | const double kMinQualityLandmark = 0.000001; 16 | const double kMinDistanceFromCamera = 0.005;//meters 17 | 18 | 19 | //void imagesc(const std::string &winname, cv::Mat image, double min = 0, 20 | // double max = 0,int colormap = cv::COLORMAP_JET) { 21 | // cv::Mat g_result_map, c_result_map; 22 | 23 | // if (min == max) { 24 | // cv::minMaxIdx(image, &min, &max); 25 | 26 | // } 27 | 28 | // double scale = 255.0 / std::abs(max - min); 29 | // std::cout << winname << " min:" << min << " max:" << max << std::endl; 30 | // image.convertTo(g_result_map, CV_8UC1, scale, -scale * min); 31 | 32 | // cv::applyColorMap(g_result_map, c_result_map, colormap); 33 | // cv::imshow(winname, c_result_map); 34 | //} 35 | 36 | 37 | void ConvertSelectOkvisLandmarks(const FeatureLandmarkFrameData &frame_data, 38 | mesh_based_mapping::VecPoint2f &keypoints, 39 | mesh_based_mapping::VecPoint3f &landmarks) { 40 | landmarks.clear(); 41 | keypoints.clear(); 42 | 43 | for (int i = 0; i < frame_data.keypoints.size(); i++) { 44 | if (frame_data.landmarks.count(frame_data.landmarks_id[i])) { 45 | const OkvisLandmark &okvis_landmark = frame_data.landmarks.at( 46 | frame_data.landmarks_id[i]); 47 | 48 | // if ((okvis_landmark.quality > kMinQualityLandmark) && 49 | // okvis_landmark.pos[2] > kMinDistanceFromCamera) { 50 | landmarks.push_back(okvis_landmark.pos.cast()); 51 | keypoints.push_back(Eigen::Vector2f(frame_data.keypoints[i].pt.x, 52 | frame_data.keypoints[i].pt.y)); 53 | // } 54 | } 55 | } 56 | } 57 | 58 | //void ReadCsvDepth(uint64 timestamp, cv::Mat &image) { 59 | // const std::string folder = "/home/lucas/tmp/lagout_slow/vi_dataset/depth_cam0/"; 60 | // std::string filename = folder + std::to_string(timestamp) + 61 | // ".yml"; //"/home/lucas/tmp/lagout_slow/blender_result_2017-08-28-21-03-27/depth/19230000001.yml"; 62 | // cv::FileStorage fs2(filename, cv::FileStorage::READ); 63 | // fs2["one_matrix"] >> image; 64 | 65 | // if (image.rows) { 66 | // imagesc("gt map", image, 4, 9,cv::COLORMAP_BONE); 67 | // } 68 | //} 69 | 70 | int main(int, char **) 71 | { 72 | std::cout << "OKVIS Dataset Mesh Generator" << std::endl; 73 | 74 | std::string datasetFeatures = "/home/fabiola/datasets/visensor_simulator/lagout_45/okvis_dataset/brisk_keypoints/data.csv"; 75 | std::string datasetOptWindow = "/home/fabiola/datasets/visensor_simulator/lagout_45/okvis_dataset/optimization_window/data.csv"; 76 | std::string meshesFolder = "/home/fabiola/datasets/visensor_simulator/meshes_lagout_45/"; 77 | 78 | OkvisDataReader okvisDataReader; 79 | if (!okvisDataReader.Init(datasetFeatures, datasetOptWindow)) 80 | { 81 | std::cerr << "could not find the files"; 82 | return 1; 83 | } 84 | 85 | mesh_based_mapping::VecPoint3f in_landmarks_3d;//in camera frame 86 | mesh_based_mapping::VecPoint2f in_landmarks_2d;//in image coords 87 | const mesh_based_mapping::VecPoint3f *out_landmarks_3d; 88 | const mesh_based_mapping::VecTriangle *out_triangles; 89 | const mesh_based_mapping::VecPoint2f *out_landmarks_2d; 90 | 91 | mesh_based_mapping::MeshMapper mapper(0.3, 4, 0.3); 92 | 93 | FeatureLandmarkFrameData okvis_data; 94 | cv::Mat result_map = cv::Mat::zeros(cv::Size(752, 480), CV_32FC1); 95 | //cv::Mat gt_map; 96 | 97 | 98 | #if 1 99 | 100 | while (okvisDataReader.GetNextSample(okvis_data)) 101 | { 102 | // std::cout << okvis_data.timestamp << std::endl; 103 | // continue; 104 | 105 | ConvertSelectOkvisLandmarks(okvis_data, in_landmarks_2d, in_landmarks_3d); 106 | 107 | if (in_landmarks_2d.size() < 30) 108 | continue; 109 | 110 | //mapper.SetPoints(in_landmarks_2d, in_landmarks_3d); 111 | if (mapper.SetPoints(455, 455, 376.5, 240.5, 752, 480, in_landmarks_3d) > 5) 112 | { 113 | mapper.ComputeMesh(); 114 | mapper.GetMesh(out_landmarks_3d, out_landmarks_2d, out_triangles); 115 | 116 | // std::cout << count++ << " " << okvis_data.timestamp << " " << 117 | // in_landmarks_3d.size() << " " << out_triangles->size() << std::endl; 118 | 119 | 120 | 121 | //if (best < out_triangles->size()) { 122 | 123 | // mesh_based_mapping::saveObj("/tmp/mesh_before8.obj", in_landmarks_3d, 124 | // *out_triangles);// 125 | // mesh_based_mapping::saveObj("/tmp/mesh_after8.obj", *out_landmarks_3d, 126 | // *out_triangles); 127 | 128 | 129 | 130 | mesh_based_mapping::RasterMesh( *out_landmarks_2d, *out_landmarks_3d, *out_triangles, result_map ); 131 | 132 | std::string meshFilename = meshesFolder; 133 | meshFilename += boost::lexical_cast(okvis_data.timestamp); 134 | meshFilename += ".yml"; 135 | 136 | cv::FileStorage fs(meshFilename, cv::FileStorage::WRITE); 137 | //fs.open(meshFilename, cv::FileStorage::WRITE); 138 | fs << "mesh" << result_map; 139 | 140 | 141 | //cv::Mat gt_map_float; 142 | //gt_map.convertTo(gt_map_float, CV_32FC1); 143 | //cv::Mat diff = result_map - gt_map_float; 144 | //imagesc("triang", result_map, 4, 9,cv::COLORMAP_BONE); 145 | //imagesc("diff", diff, -1, 1); 146 | //imagesc("abs-diff", cv::abs(diff), 0, 1,cv::COLORMAP_AUTUMN); 147 | } 148 | else 149 | { 150 | std::cout << "not enough points in the field of view" << std::endl; 151 | } 152 | 153 | if ((char)27 == cv::waitKey(100)) 154 | { 155 | break; 156 | } 157 | 158 | } 159 | 160 | #else 161 | 162 | refrOkvisDataReader.GetSample(uint64_t(1456317951390953500), okvis_data); 163 | ConvertSelectOkvisLandmarks(okvis_data, in_landmarks_2d, in_landmarks_3d); 164 | // Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", 165 | // "]"); 166 | 167 | // std::cout << okvis_data.t_ws.getPosition().format(HeavyFmt) << " - " << 168 | // okvis_data.t_ws.getRotation().vector().format(HeavyFmt) << " - " << 169 | // okvis_data.t_ws.getEigenQuaternion().coeffs().format(HeavyFmt); 170 | 171 | // std::cout << count++ << " " << okvis_data.landmarks.size() << " " << 172 | // okvis_data.keypoints.size() << " - " << in_landmarks_2d.size() << " " << 173 | // in_landmarks_3d.size() << std::endl; 174 | 175 | 176 | mapper.SetPoints(in_landmarks_2d, in_landmarks_3d); 177 | // mapper.SetPoints(470,470,376,247,752,480, in_landmarks_3d); 178 | mapper.ComputeMesh(); 179 | mapper.GetMesh(out_landmarks_3d, out_landmarks_2d, out_triangles); 180 | std::cout << okvis_data.timestamp << " " << 181 | in_landmarks_3d.size() << " " << out_triangles->size() << std::endl; 182 | 183 | mesh_based_mapping::saveObj("/tmp/mesh_before4.obj", in_landmarks_3d, 184 | *out_triangles); 185 | mesh_based_mapping::saveObj("/tmp/mesh_after4.obj", *out_landmarks_3d, 186 | *out_triangles); 187 | 188 | #endif 189 | 190 | 191 | 192 | return 0; 193 | } 194 | -------------------------------------------------------------------------------- /examples/simple_example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | ADD_DEFINITIONS (-march=native -std=c++11 ) 3 | 4 | 5 | 6 | include_directories(${CMAKE_SOURCE_DIR}/third_party/okvis_kinematics/include) 7 | add_executable(simple_example src/main.cpp ${CMAKE_SOURCE_DIR}/include/mesh_based_mapping/mesh_based_mapping.hpp) 8 | 9 | target_link_libraries(simple_example 10 | ${OpenCV_LIBS} 11 | v4rl_mesh_based_mapping_lib 12 | ${CMAKE_SOURCE_DIR}/third_party/fade_v1.46a/${FADE_LIB_VERSION}/libfade2d.so ) 13 | 14 | -------------------------------------------------------------------------------- /examples/simple_example/src/main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | const double pi = std::acos(-1); 6 | 7 | void buildHemiSphere(double ro_step, double phi_step, 8 | double radius, double central_hole, 9 | std::vector> 10 | &landmarks_3d) { 11 | 12 | float x, y, z; 13 | 14 | for (double ro = central_hole; ro < pi / 2.0; ro += ro_step) { 15 | z = radius * std::cos(ro); 16 | 17 | for (double phi = -pi; phi < pi; phi += phi_step) { 18 | x = radius * std::sin(ro) * std::cos(phi); 19 | y = radius * std::sin(ro) * std::sin(phi); 20 | landmarks_3d.push_back(Eigen::Vector3f(x, y, z)); 21 | } 22 | } 23 | } 24 | 25 | int main(int, char **) { 26 | 27 | 28 | const double focalU = 450; 29 | const double focalV = 450; 30 | const double centerU = 400.5; 31 | const double centerV = 400.5; 32 | const double dimU = 800; 33 | const double dimV = 800; 34 | mesh_based_mapping::VecPoint3f landmarks_3d;//in camera frame 35 | 36 | buildHemiSphere(0.01, 0.1, 10, 0, landmarks_3d); 37 | 38 | const mesh_based_mapping::VecPoint3f *landmarks_3d_filtered; 39 | const mesh_based_mapping::VecTriangle *triangles; 40 | const mesh_based_mapping::VecPoint2f *landmarks_2d; 41 | 42 | mesh_based_mapping::MeshMapper mapper; 43 | mapper.SetPoints(focalU, focalV, centerU, centerV, dimU,dimV, landmarks_3d); 44 | mapper.ComputeMesh(); 45 | mapper.GetMesh(landmarks_3d_filtered,landmarks_2d,triangles); 46 | 47 | 48 | mesh_based_mapping::saveObj("/tmp/mesh_before3.obj", landmarks_3d, *triangles); 49 | 50 | mapper.SaveObj("/tmp/mesh_after3.obj",dimU,dimV); 51 | 52 | 53 | 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /examples/simple_example/src/obj.data: -------------------------------------------------------------------------------- 1 | const char* point_cloud = R"(v -159.262 11.4339 13.6523 2 | v -159.469 11.3725 11.9626 3 | v -160.103 13.0951 15.1132 4 | v -159.402 11.4363 13.4745 5 | v -161.217 12.9697 14.9741 6 | v -160.626 13.0613 15.2765 7 | v -160.195 13.0594 15.3072 8 | v -161.763 12.8907 14.5062 9 | v -161.103 13.011 14.427 10 | v -162.341 12.9695 14.1501 11 | v -161.715 13.1584 13.7712 12 | v -161.517 13.2977 13.0762 13 | v -161.775 13.2978 13.2096 14 | v -162.191 12.8992 14.2917 15 | v -161.853 12.9411 14.6746 16 | v -161.014 13.291 11.7126 17 | v -161.786 12.9704 14.7984 18 | v -161.404 12.9161 14.7334 19 | v -161.599 13.3748 12.8724 20 | v -161.606 13.2497 13.3682 21 | v -161.465 13.5355 12.3246 22 | v -161.921 13.1293 13.6194 23 | v -161.356 13.3376 12.1265 24 | v -162.325 12.8991 14.4513 25 | v -161.652 13.0415 14.0353 26 | v -160.962 13.0542 13.2876 27 | v -163.04 14.0453 15.2628 28 | v -160.66 13.0389 15.0367 29 | v -160.656 13.0549 14.7178 30 | v -160.345 13.1288 15.2651 31 | v -162.24 12.874 14.5098 32 | v -162.243 12.9867 13.9408 33 | v -159.956 12.647 14.2153 34 | v -160.027 11.9441 10.4414 35 | v -162.397 12.8754 14.1464 36 | v -162.423 12.8885 14.0456 37 | )"; 38 | -------------------------------------------------------------------------------- /include/mesh_based_mapping/mesh_based_mapping.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * Mesh Mapping- Code reference for the paper Lucas and Chli - IROS 2016 3 | * Copyright (c) 2017, Vision for Robotics Lab / ETH Zurich 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | * 18 | * Created on: Jan 1, 2017 19 | * Author: Lucas Teixeira (lteixeira@mavt.ethz.ch) 20 | * 21 | * 22 | *********************************************************************************/ 23 | 24 | 25 | #ifndef MESH_BASED_MAPPING_MESH_MAPPING_H_ 26 | #define MESH_BASED_MAPPING_MESH_MAPPING_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace GEOM_FADE2D { 33 | class Point2; 34 | class Triangle2; 35 | class Fade_2D; 36 | } 37 | 38 | namespace mesh_based_mapping { 39 | 40 | typedef std::vector > 41 | VecPoint3f; 42 | typedef std::vector > 43 | VecPoint2f; 44 | typedef std::vector > 45 | VecTriangle; 46 | 47 | class MeshMapper { 48 | public: 49 | MeshMapper(double laplace_alpha = 0.1, 50 | unsigned int smoothing_iteration = 3, 51 | double max_delta = 0.2); 52 | 53 | ~MeshMapper(); 54 | 55 | 56 | int SetPoints(double focal_u, double focal_v, 57 | double center_u, double center_v, 58 | uint dim_u, uint dim_v, const VecPoint3f &in_landmarks_3d); 59 | 60 | int SetPoints(const VecPoint2f &in_landmarks_2d, 61 | const VecPoint3f &in_landmarks_3d); 62 | 63 | bool ComputeMesh(); 64 | 65 | bool GetFilteredLandmarks(const VecPoint3f *&out_landmarks_3d); 66 | 67 | bool GetMesh(const VecPoint3f *&out_landmarks_3d, 68 | const VecPoint2f *&out_landmarks_2d, 69 | const VecTriangle 70 | * &out_triangles); //on the landmarks vector could include unused landmaks by the triangles.(noise) 71 | 72 | void Clear(); 73 | 74 | 75 | void SaveObj(std::string filepath, uint dim_u=0, uint dim_v=0); 76 | 77 | private: 78 | 79 | void convert2dLandmarks(const VecPoint2f &in_landmarks_2d); 80 | 81 | void ProjectLandmarks(const double &focalU, const double &focalV, 82 | const double ¢erU, const double ¢erV, const double &dimU, 83 | const double &dimV, const mesh_based_mapping::VecPoint3f &landmarks); 84 | 85 | void filteringAndSmoothing(mesh_based_mapping::VecPoint3f &points3d, 86 | const std::vector &triangles, 87 | std::vector &blacklist, 88 | double laplaceAlpha, unsigned int smoothingIteration, 89 | double maxDelta); 90 | 91 | double laplace_alpha_; 92 | unsigned int smoothing_iteration_; 93 | double max_delta_; 94 | 95 | std::vector *landmarks_2d_; 96 | GEOM_FADE2D::Fade_2D *fade_; //stores most of the information 97 | std::vector triangles_;//pointers to fade_ structure 98 | 99 | VecPoint3f landmarks_3d_; 100 | VecPoint3f filtered_landmarks_3d_output_; 101 | std::vector triangle_blacklist_; 102 | 103 | VecTriangle triangles_output_; 104 | VecPoint2f landmarks_2d_output_; 105 | 106 | }; 107 | 108 | } 109 | 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /include/mesh_based_mapping/utils/file_io.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MESH_BASED_MAPPING_FILE_IO_H_ 2 | #define MESH_BASED_MAPPING_FILE_IO_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace mesh_based_mapping { 8 | 9 | void saveObj(std::string filepath, 10 | const VecPoint3f &landmarks_3d, 11 | const VecTriangle &triangles = VecTriangle()) { 12 | 13 | std::ofstream ofs; 14 | ofs.open(filepath, std::ofstream::out); 15 | 16 | 17 | for (unsigned int i = 0; i < landmarks_3d.size(); i++) { 18 | const Eigen::Vector3f &hPoint = landmarks_3d[i]; 19 | ofs << "v " << hPoint[0] << " " << hPoint[1] << " " << hPoint[2] << std::endl; 20 | } 21 | 22 | for (unsigned int i = 0; i < triangles.size(); i++) { 23 | 24 | const Eigen::Vector3i &t = triangles[i];//obj format starts from 1 instead zero 25 | ofs << "f " << t[2] + 1 << " "; 26 | ofs << t[1] + 1 << " "; 27 | ofs << t[0] + 1 << std::endl; 28 | } 29 | 30 | ofs.close(); 31 | } 32 | 33 | } 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /include/mesh_based_mapping/utils/mesh_based_mapping_okvis_wrapper.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MESH_BASED_MAPPING_FILE_IO_H_ 2 | #define MESH_BASED_MAPPING_FILE_IO_H_ 3 | 4 | #include 5 | #include 6 | #include //TODO replace the mappointvector struc 7 | #define MIN_CAM_DISTANCE 0.05 8 | 9 | namespace mesh_based_mapping { 10 | 11 | 12 | 13 | /** 14 | 15 | */ 16 | 17 | void saveCSV(std::string filename, std::string timestamp, std::string img_path, 18 | std::string mesh_path, okvis::kinematics::Transformation &T_WCRef) { 19 | Eigen::Vector3d p_WS_W = T_WCRef.r(); 20 | Eigen::Quaterniond q_WS = T_WCRef.q(); 21 | 22 | std::fstream csvFile(filename, std::fstream::app | std::fstream::out); 23 | 24 | csvFile << timestamp << "," << img_path << "," << mesh_path << "," << 25 | std::scientific 26 | << std::setprecision(18) << p_WS_W[0] << "," << p_WS_W[1] << "," 27 | << p_WS_W[2] << "," << q_WS.x() << "," << q_WS.y() << "," 28 | << q_WS.z() << "," << q_WS.w() << std::endl; 29 | csvFile.close(); 30 | } 31 | 32 | 33 | /** 34 | 35 | */ 36 | void projectLandmarks(const double focalU, const double focalV, 37 | const double centerU, const double centerV, 38 | const double dimU, const double dimV, 39 | const double minQuality, 40 | okvis::kinematics::Transformation &T_WCRef, 41 | const okvis::MapPointVector &matchedLandmarks, 42 | VecPoint3f &filteredLandmarks, 43 | std::vector &landmarks2D) { 44 | 45 | okvis::kinematics::Transformation T_CRefW = T_WCRef.inverse(); 46 | 47 | for (unsigned int i = 0 ; i < matchedLandmarks.size() ; i++) { 48 | 49 | if (matchedLandmarks[i].point(3) < 1.0e-8) { 50 | continue; 51 | } 52 | 53 | if (matchedLandmarks[i].quality < minQuality) { 54 | continue; 55 | } 56 | 57 | Eigen::Vector4d pt_CRef = T_CRefW * matchedLandmarks[i].point; 58 | 59 | 60 | 61 | pt_CRef(0) = pt_CRef(0) / pt_CRef(3); 62 | pt_CRef(1) = pt_CRef(1) / pt_CRef(3); 63 | pt_CRef(2) = pt_CRef(2) / pt_CRef(3); 64 | 65 | 66 | if (pt_CRef(2) > MIN_CAM_DISTANCE) { 67 | double x = ((pt_CRef(0) / pt_CRef(2)) * focalU) + centerU; 68 | double y = ((pt_CRef(1) / pt_CRef(2)) * focalV) + centerV; 69 | 70 | if (x <= 0 || y <= 0 || x >= dimU || y >= dimV) { 71 | continue; 72 | } 73 | 74 | filteredLandmarks.push_back(Eigen::Vector3f(pt_CRef(0), pt_CRef(1), 75 | pt_CRef(2))); 76 | landmarks2D.push_back(GEOM_FADE2D::Point2(x, y)); 77 | landmarks2D.back().setCustomIndex(filteredLandmarks.size() - 1); 78 | } 79 | } 80 | } 81 | 82 | 83 | 84 | void buildMeshDepthMap(const double focalU, 85 | const double focalV, 86 | const double centerU, 87 | const double centerV, 88 | const double dimU, 89 | const double dimV, 90 | const double minQuality, 91 | const okvis::MapPointVector &matchedLandmarks, 92 | okvis::kinematics::Transformation &T_WCRef, 93 | cv::Mat &image, 94 | uint64_t timestampNsec, 95 | cv::Mat &resultDepthMap, 96 | const double laplaceAlpha = 0.1, 97 | const unsigned int smoothingIteration = 3, 98 | const double maxDelta = 0.2) { 99 | 100 | std::vector points2D; 101 | VecPoint3f points3D; 102 | 103 | projectLandmarks(focalU, focalV, centerU, centerV, 104 | dimU, dimV, minQuality, T_WCRef, 105 | matchedLandmarks, points3D, points2D); 106 | 107 | GEOM_FADE2D::Fade_2D dt; 108 | dt.insert(points2D); 109 | std::vector vAllTriangles; 110 | dt.getTrianglePointers(vAllTriangles); 111 | 112 | std::vector blacklist(vAllTriangles.size(), false); 113 | 114 | #ifdef MM_DEBUG 115 | saveObj("meshProj_before.obj", points3D, points2D, vAllTriangles, blacklist); 116 | #endif 117 | 118 | 119 | filteringAndSmoothing(points3D, vAllTriangles, blacklist, laplaceAlpha, 120 | smoothingIteration, maxDelta); 121 | 122 | 123 | #ifdef MM_DEBUG 124 | saveObj("meshProj_after.obj", points3D, points2D, vAllTriangles, blacklist); 125 | #endif 126 | 127 | //clean the result map 128 | resultDepthMap.setTo(cv::Vec2f(0, 0)); 129 | 130 | //write on the result map 131 | for (uint i = 0; i < vAllTriangles.size() ; i++) { 132 | if (blacklist[i]) { 133 | continue; 134 | } 135 | 136 | GEOM_FADE2D::Triangle2 *itri = vAllTriangles[i]; 137 | std::vector tpts(3); 138 | std::vector zs(3); 139 | 140 | tpts[0] = cv::Point(itri->getCorner(0)->x(), itri->getCorner(0)->y()); 141 | zs[0] = (points3D[itri->getCorner(0)->getCustomIndex()])(2); 142 | 143 | tpts[1] = cv::Point(itri->getCorner(1)->x(), itri->getCorner(1)->y()); 144 | zs[1] = points3D[itri->getCorner(1)->getCustomIndex()](2); 145 | 146 | tpts[2] = cv::Point(itri->getCorner(2)->x(), itri->getCorner(2)->y()); 147 | zs[2] = points3D[itri->getCorner(2)->getCustomIndex()](2); 148 | 149 | rasterTriangle(tpts, zs, itri, resultDepthMap); 150 | } 151 | 152 | #ifdef MM_DEBUG 153 | 154 | for (int i = 0; i < 2; i++) { 155 | cv::Mat labels; 156 | cv::extractChannel(resultDepthMap, labels, i); 157 | // writeCSV("meshProj_nn_" + std::to_string(i) + ".csv", labels); 158 | } 159 | 160 | #endif 161 | ///3DV stuff 162 | /// 163 | VecPoint3f point3D_world_frame; 164 | 165 | for (size_t i = 0; i < points3D.size(); i++) { 166 | Eigen::Vector4d pt_cam; 167 | pt_cam(0) = points3D[i](0); 168 | pt_cam(1) = points3D[i](1); 169 | pt_cam(2) = points3D[i](2); 170 | pt_cam(3) = 1.0; 171 | Eigen::Vector4d pt_world = T_WCRef * pt_cam; 172 | point3D_world_frame.push_back(Eigen::Vector3f(pt_world(0) / pt_world(3), 173 | pt_world(1) / pt_world(3), pt_world(2) / pt_world(3))); 174 | 175 | } 176 | 177 | //TODO this is wrong, because this should be the undistorted image, but I am using the distorted one. 178 | cv::Mat img2; 179 | image.convertTo(img2, CV_32FC1, 1.0 / 255.0); 180 | cv::insertChannel(img2, resultDepthMap, 1); 181 | 182 | 183 | std::string timestamp_string; 184 | std::stringstream timestamp_ss; 185 | 186 | timestamp_ss << timestampNsec; 187 | timestamp_string = timestamp_ss.str(); 188 | 189 | std::string folder_path = 190 | "/tmp/okvis_dataset/";//need to create this folder and add inside the folders data/meshes/ and data/images/ 191 | 192 | std::string csv_path = "data.csv"; 193 | std::string mesh_path = "meshes/" + timestamp_string + ".obj"; 194 | std::string img_path = "images/" + timestamp_string + ".png"; 195 | 196 | saveCSV(folder_path + csv_path, timestamp_string, img_path, mesh_path, T_WCRef); 197 | saveObj(folder_path + "data/" + mesh_path, point3D_world_frame, points2D, 198 | vAllTriangles, blacklist); 199 | cv::imwrite(folder_path + "data/" + img_path, image); 200 | 201 | return; 202 | } 203 | } 204 | 205 | 206 | #endif -------------------------------------------------------------------------------- /include/mesh_based_mapping/utils/triangle_rasterization.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef MESH_BASED_MAPPING_TRIANGLE_RASTERIZATION_HPP_ 3 | #define MESH_BASED_MAPPING_TRIANGLE_RASTERIZATION_HPP_ 4 | 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | 12 | namespace mesh_based_mapping { 13 | 14 | 15 | 16 | /* 17 | * Beginning of the code adapted from scratchapixel 18 | * Copyright (C) 2012 www.scratchapixel.com - GPLv3 19 | */ 20 | 21 | float min3(const float &a, const float &b, const float &c) { 22 | return std::min(a, std::min(b, c)); 23 | } 24 | 25 | float max3(const float &a, const float &b, const float &c) { 26 | return std::max(a, std::max(b, c)); 27 | } 28 | 29 | void maxIndex3(const float &a, const float &b, const float &c, float &maxValue, 30 | int &maxIndex) { 31 | if (a > b) { 32 | if (a > c) { 33 | maxValue = a; 34 | maxIndex = 0; 35 | } else { 36 | maxValue = c; 37 | maxIndex = 2; 38 | } 39 | } else { 40 | if (b > c) { 41 | maxValue = b; 42 | maxIndex = 1; 43 | } else { 44 | maxValue = c; 45 | maxIndex = 2; 46 | } 47 | } 48 | } 49 | 50 | float edgeFunction(const cv::Point2f &a, const cv::Point2f &b, 51 | const cv::Point2f &c) { 52 | return (c.x - a.x) * (b.y - a.y) - (c.y - a.y) * (b.x - a.x); 53 | } 54 | 55 | 56 | void rasterTriangle(const std::vector &points, 57 | const std::vector &zs, 58 | // const GEOM_FADE2D::Triangle2 *itri, 59 | cv::Mat &resultMap) { 60 | 61 | cv::Point2f v0Raster = points[0]; 62 | cv::Point2f v1Raster = points[1]; 63 | cv::Point2f v2Raster = points[2]; 64 | float x0 = std::max(min3(v0Raster.x, v1Raster.x, v2Raster.x),.0f); 65 | float y0 = std::max(min3(v0Raster.y, v1Raster.y, v2Raster.y),.0f); 66 | float x1 = std::min(max3(v0Raster.x, v1Raster.x, v2Raster.x),(float)resultMap.cols); 67 | float y1 = std::min(max3(v0Raster.y, v1Raster.y, v2Raster.y),(float)resultMap.rows); 68 | 69 | 70 | float area = edgeFunction(v0Raster, v1Raster, v2Raster); 71 | 72 | float iz0 = 1.0f / zs[0]; 73 | float iz1 = 1.0f / zs[1]; 74 | float iz2 = 1.0f / zs[2]; 75 | 76 | for (uint32_t y = y0; y <= y1; ++y) { 77 | for (uint32_t x = x0; x <= x1; ++x) { 78 | cv::Point2f pixelSample(x + 0.5f, y + 0.5f); 79 | float w0 = edgeFunction(v1Raster, v2Raster, pixelSample); 80 | float w1 = edgeFunction(v2Raster, v0Raster, pixelSample); 81 | float w2 = edgeFunction(v0Raster, v1Raster, pixelSample); 82 | w0 /= area; 83 | w1 /= area; 84 | w2 /= area; 85 | 86 | if (w0 >= 0 && w1 >= 0 && w2 >= 0) { 87 | float oneOverZ = iz0 * w0 + iz1 * w1 + iz2 * w2; 88 | float z = 1.0f / oneOverZ; 89 | resultMap.at(y, x) = z; 90 | int index; 91 | float maxVal; 92 | maxIndex3(w0, w1, w2, maxVal, index); 93 | // resultMap.at(y, x)[1] = itri->getCorner(index)->getCustomIndex(); 94 | } 95 | } 96 | } 97 | } 98 | 99 | /* 100 | * End of the code adapted from scratchapixel 101 | * Copyright (C) 2012 www.scratchapixel.com - GPLv3 102 | */ 103 | 104 | void RasterMesh(const VecPoint2f &landmarks_2d, 105 | const VecPoint3f &landmarks_3d, 106 | const VecTriangle &triangles, 107 | cv::Mat &result_map) { 108 | 109 | result_map.setTo(0); 110 | 111 | //write on the result map 112 | for (uint i = 0; i < triangles.size() ; i++) { 113 | 114 | const Eigen::Vector3i &itri = triangles[i]; 115 | std::vector tpts(3); 116 | std::vector zs(3); 117 | tpts[0] = cv::Point(landmarks_2d[itri(0)](0), landmarks_2d[itri(0)](1)); 118 | zs[0] = landmarks_3d[itri(0)](2); 119 | 120 | tpts[1] = cv::Point(landmarks_2d[itri(1)](0), landmarks_2d[itri(1)](1)); 121 | zs[1] = landmarks_3d[itri(1)](2); 122 | 123 | tpts[2] = cv::Point(landmarks_2d[itri(2)](0), landmarks_2d[itri(2)](1)); 124 | zs[2] = landmarks_3d[itri(2)](2); 125 | 126 | rasterTriangle(tpts, zs, result_map); 127 | } 128 | 129 | 130 | } 131 | 132 | 133 | } 134 | 135 | 136 | #endif 137 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/Bbox2.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | 18 | #pragma once 19 | #include "Point2.h" 20 | #include "common.h" 21 | #if GEOM_PSEUDO3D==GEOM_TRUE 22 | namespace GEOM_FADE25D { 23 | #elif GEOM_PSEUDO3D==GEOM_FALSE 24 | namespace GEOM_FADE2D { 25 | #else 26 | #error GEOM_PSEUDO3D is not defined 27 | #endif 28 | 29 | class GeomTest; // FWD 30 | 31 | /** \brief Boundingbox 32 | */ 33 | class CLASS_DECLSPEC Bbox2 34 | { 35 | public: 36 | /** \brief Create a bounding box 37 | * Bounds stay uninitialized 38 | */ 39 | 40 | 41 | Bbox2(GeomTest* pGeomTest_=NULL): 42 | minX(DBL_MAX),minY(DBL_MAX), 43 | maxX(DBL_MIN),maxY(DBL_MIN), 44 | bValid(false),pGeomTest(pGeomTest_) 45 | { 46 | } 47 | 48 | /** \brief Create a bounding box 49 | * Bounds initialized to the minimal bounding box of the iterator range of points 50 | */ 51 | 52 | Bbox2( std::vector::const_iterator start_it, 53 | std::vector::const_iterator end_it, 54 | GeomTest* pGeomTest_=NULL) 55 | : 56 | minX(DBL_MAX),minY(DBL_MAX), 57 | maxX(DBL_MIN),maxY(DBL_MIN), 58 | pGeomTest(pGeomTest_) 59 | { 60 | add(start_it,end_it); 61 | } 62 | 63 | /** \brief Are the bounds valid? 64 | */ 65 | 66 | bool isValid() const 67 | { 68 | return minX& vBoxCorners) const; 74 | 75 | 76 | /** \brief Add points 77 | * \return true if the bounding box changes, false otherwise 78 | */ 79 | 80 | bool add(std::vector::const_iterator start_it,std::vector::const_iterator end_it) 81 | { 82 | if(start_it==end_it) return false; 83 | if(bValid) 84 | { 85 | double oldMinX(minX),oldMinY(minY),oldMaxX(maxX),oldMaxY(maxY); 86 | for(;start_it!=end_it;++start_it) treatPointForValidBox(*start_it); 87 | if(oldMinX!=minX || oldMinY!=minY || oldMaxX!=maxX || oldMaxY!=maxY ) return true; 88 | return false; 89 | } 90 | else 91 | { 92 | treatPointForInvalidBox(*start_it); 93 | ++start_it; 94 | for(;start_it!=end_it;++start_it) treatPointForValidBox(*start_it); 95 | return true; 96 | } 97 | } 98 | 99 | /** \brief Add points 100 | * \return true if the bounding box changes, false otherwise 101 | */ 102 | 103 | bool add(size_t numPoints,double * coordinates) 104 | { 105 | #if GEOM_PSEUDO3D==GEOM_TRUE 106 | const int NUMCOMPONENTS(3); 107 | #else 108 | const int NUMCOMPONENTS(2); 109 | #endif 110 | 111 | if(numPoints==0) return false; 112 | double oldMinX(minX),oldMinY(minY),oldMaxX(maxX),oldMaxY(maxY); 113 | double x(coordinates[0]); 114 | double y(coordinates[1]); 115 | if(xmaxX) maxX=x; 117 | if(ymaxY) maxY=y; 119 | 120 | for(size_t i=0;imaxX) maxX=x; 126 | if(ymaxY) maxY=y; 128 | } 129 | bValid=true; 130 | if(oldMinX!=minX || oldMinY!=minY || oldMaxX!=maxX || oldMaxY!=maxY ) return true; 131 | else return false; 132 | } 133 | 134 | /** \brief Add a point 135 | * \return true if the bounding box changes, false otherwise 136 | */ 137 | 138 | bool add(const Point2& p) 139 | { 140 | //std::cout<<"Add point: "<maxX) maxX=b.maxX; 164 | if(b.minYmaxY) maxY=b.maxY; 166 | return *this; 167 | } 168 | 169 | #if GEOM_PSEUDO3D==GEOM_TRUE 170 | /** \brief Get the min point 171 | * \return the point with the three minimum coordinates, the z-coordinate is set to 0 172 | */ 173 | #else 174 | /** \brief Get the min point 175 | * \return the point with the three minimum coordinates 176 | */ 177 | #endif 178 | 179 | Point2 getMinPoint() const 180 | { 181 | #if GEOM_PSEUDO3D==GEOM_TRUE 182 | return Point2(minX,minY,0); 183 | #else 184 | return Point2(minX,minY); 185 | #endif 186 | } 187 | 188 | 189 | #if GEOM_PSEUDO3D==GEOM_TRUE 190 | /** \brief Get the max point 191 | * \return the point with the three maximum coordinates, the z-coordinate is set to 0 192 | */ 193 | #else 194 | /** \brief Get the max point 195 | * \return the point with the three maximum coordinates 196 | */ 197 | #endif 198 | 199 | Point2 getMaxPoint() const 200 | { 201 | #if GEOM_PSEUDO3D==GEOM_TRUE 202 | return Point2(maxX,maxY,0); 203 | #else 204 | return Point2(maxX,maxY); 205 | #endif 206 | } 207 | 208 | /** \brief Get minimum coordinate 209 | * \return the smallest coordinate value of {x,y} 210 | */ 211 | 212 | double getMinCoord() const 213 | { 214 | if(minXmaxY) return maxX; 224 | else return maxY; 225 | } 226 | /** \brief Get x range 227 | * \return maxX-minX 228 | */ 229 | 230 | double getRangeX() const 231 | { 232 | return maxX-minX; 233 | } 234 | /** \brief Get y range 235 | * \return maxY-minY 236 | */ 237 | 238 | double getRangeY() const 239 | { 240 | return maxY-minY; 241 | } 242 | /** \brief Get max range 243 | * \return the largest range of {x,y} 244 | */ 245 | 246 | double getMaxRange() const 247 | { 248 | double range0=getRangeX(); 249 | double range1=getRangeY(); 250 | if(range0>range1) std::swap(range0,range1); 251 | return range1; 252 | } 253 | /** \brief Get minX 254 | * \return minX 255 | */ 256 | double get_minX() {return minX;} 257 | /** \brief Get minY 258 | * \return minY 259 | */ 260 | double get_minY() {return minY;} 261 | /** \brief Get maxX 262 | * \return maxX 263 | */ 264 | double get_maxX() {return maxX;} 265 | /** \brief Get maxY 266 | * \return maxY 267 | */ 268 | double get_maxY() {return maxY;} 269 | 270 | /** \brief Double the box size 271 | * Changes the bounds such that the box grows in each direction by half the range 272 | */ 273 | 274 | void doubleTheBox(); 275 | 276 | protected: 277 | /** \brief Add a point to a valid box 278 | */ 279 | 280 | inline void treatPointForValidBox(const Point2& p) 281 | { 282 | double x,y; 283 | p.xy(x,y); 284 | if(xmaxX) maxX=x; 286 | if(ymaxY) maxY=y; 288 | } 289 | 290 | /** \brief Add a point to an uninitialized 291 | */ 292 | 293 | inline void treatPointForInvalidBox(const Point2& p) 294 | { 295 | p.xy(minX,minY); 296 | p.xy(maxX,maxY); 297 | bValid=true; 298 | } 299 | 300 | 301 | 302 | 303 | protected: 304 | double minX,minY; 305 | double maxX,maxY; 306 | bool bValid; 307 | friend std::ostream &operator<<(std::ostream &stream, Bbox2& pC); 308 | GeomTest* pGeomTest; 309 | }; 310 | 311 | 312 | inline std::ostream &operator<<(std::ostream &stream, Bbox2& pC) 313 | { 314 | stream<<"Bbox2: ("< ("< 23 | 24 | 25 | #include "common.h" 26 | #if GEOM_PSEUDO3D==GEOM_TRUE 27 | namespace GEOM_FADE25D { 28 | #elif GEOM_PSEUDO3D==GEOM_FALSE 29 | namespace GEOM_FADE2D { 30 | #else 31 | #error GEOM_PSEUDO3D is not defined 32 | #endif 33 | 34 | class Dt2; // Forward 35 | class ConstraintSegment2; // Forward 36 | class GeomTest; // Forward 37 | 38 | 39 | /** \brief ConstraintGraph2 is a set of enforced edges 40 | * 41 | * \see \ref createConstraint in the Fade2D class 42 | * 43 | * \image html crop_ignoreBike.jpg "Constraints in a triangulation" 44 | * \image latex crop_ignoreBike.eps "Constraints in a triangulation" width=12cm 45 | */ 46 | class ConstraintGraph2 47 | { 48 | public: 49 | /** \brief Internal use 50 | * 51 | * Constructor for one or more ConstraintSegment2 objects 52 | */ 53 | CLASS_DECLSPEC 54 | ConstraintGraph2( 55 | Dt2* pDt2_, 56 | std::vector& vCSegments, 57 | ConstraintInsertionStrategy 58 | ); 59 | 60 | /** \brief Internal use 61 | * 62 | * Constructor for one or more ConstraintSegment2 objects whose 63 | * direction is given by mPPReverse. Used for bounded zones 64 | */ 65 | CLASS_DECLSPEC 66 | ConstraintGraph2( 67 | Dt2* pDt2_, 68 | std::vector& vCSegments_, 69 | std::map,bool > mPPReverse, 70 | ConstraintInsertionStrategy cis_ 71 | ); 72 | 73 | /** \brief Internal use 74 | * 75 | * Called by the two constructors 76 | */ 77 | void init(std::vector& vCSegments_); 78 | 79 | 80 | /** \brief Does the constraint graph form a closed polygon? 81 | * 82 | * @note This method does not check if it is a simple polygon (one 83 | * without self-intersections). 84 | */ 85 | CLASS_DECLSPEC 86 | bool isPolygon() const; 87 | 88 | /** \brief Are the segments of the constraint graph oriented? 89 | * 90 | * @return true if the constraint graph has been created with 91 | * bOrientedSegments=true or if automatic reorientation was possible 92 | * which is the case for simple polygons. 93 | */ 94 | CLASS_DECLSPEC 95 | bool isOriented() const; 96 | 97 | /** \brief Get the vertices of the constraint segments 98 | * 99 | * Use this method to retrieve the segments of *this in form of a vector 100 | * of vertices. If *this is a closed polygon, then the points are ordered 101 | * and oriented in counterclockwise direction, e.g. (a,b,b,c,c,d,d,a). If 102 | * the *this is not a polygon, then the segments are returned in the 103 | * original direction. 104 | * 105 | * @note If it was necessary to split the constraint segments, then the 106 | * splitted segments are returned. If, in the above example, the constraint 107 | * segment (a,b) crosses some previously inserted point x, then the 108 | * result is (a,x,x,b,b,c,c,d,d,a). 109 | * 110 | */ 111 | CLASS_DECLSPEC 112 | void getPolygonVertices(std::vector& vTriangulationPoints_) ; 113 | 114 | /** \brief Get the constraint insertion strategy 115 | * 116 | * \if SECTION_FADE25D 117 | * @return CIS_CONFORMING_DELAUNAY, CIS_CONFORMING_DELAUNAY_SEGMENT_LEVEL or @n 118 | * CIS_CONSTRAINED_DELAUNAY 119 | * \else 120 | * @return CIS_CONFORMING_DELAUNAY or CIS_CONSTRAINED_DELAUNAY 121 | * \endif 122 | * 123 | */ 124 | CLASS_DECLSPEC 125 | ConstraintInsertionStrategy getInsertionStrategy() const; 126 | 127 | /** \brief Check if an edge is a constraint 128 | * 129 | * Checks if the edge (p0,p1) is a constraint of *this 130 | */ 131 | CLASS_DECLSPEC 132 | bool isConstraint(Point2* p0,Point2* p1) const; 133 | 134 | 135 | 136 | /** \brief Visualization 137 | * 138 | */ 139 | CLASS_DECLSPEC 140 | void show(const std::string& name); 141 | 142 | 143 | 144 | /** \brief Get the original ConstraintSegment2 objects 145 | * 146 | * Get the original, not subdivided ConstraintSegment2 objects. The 147 | * ones which have been splitted are not alive anymore. But they 148 | * have children (for which the same may hold). 149 | * 150 | */ 151 | CLASS_DECLSPEC 152 | void getOriginalConstraintSegments(std::vector& vConstraintSegments_) const; 153 | 154 | /** \brief Update a constraint segment of *this 155 | * Internal method 156 | */ 157 | void updateSplittedConstraintSegment(ConstraintSegment2* pCSeg,bool bDirChange0,bool bDirChange1,ConstraintSegment2* pChild0,ConstraintSegment2* pChild1); 158 | /** 159 | * \return the Delaunay class it belongs to 160 | */ 161 | Dt2* getDt2(); 162 | void getAliveConstraintChain(std::vector& vAliveCSeg) ; // For debugging 163 | protected: 164 | bool isReverse(ConstraintSegment2* pCSeg) const; 165 | bool checkAndSortPolygon(std::vector& vCSegments_); 166 | 167 | 168 | // Data 169 | Dt2* pDt2; 170 | GeomTest* pGeomPredicates; 171 | ConstraintInsertionStrategy cis; 172 | std::vector vCSegParents; 173 | bool bIsPolygon; 174 | std::map > mCSegReverse; 175 | std::map mSplitPointNum; 176 | bool bIsOriented; 177 | }; 178 | 179 | } // (namespace) 180 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/ConstraintSegment2.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | #pragma once 18 | #include 19 | 20 | #include "common.h" 21 | #if GEOM_PSEUDO3D==GEOM_TRUE 22 | namespace GEOM_FADE25D { 23 | #elif GEOM_PSEUDO3D==GEOM_FALSE 24 | namespace GEOM_FADE2D { 25 | #else 26 | #error GEOM_PSEUDO3D is not defined 27 | #endif 28 | 29 | class Point2; // FWD 30 | class ConstraintGraph2; // FWD 31 | 32 | 33 | /** \brief Constraint Insertion Strategy 34 | * determines how a constraint edge shall be inserted: 35 | * 36 | * - CIS_CONSTRAINED_DELAUNAY inserts a segment without subdivision 37 | * unless required (which is the case if existing vertices or constraint 38 | * segments are crossed). 39 | * - CIS_CONFORMING_DELAUNAY subdivides a segment if necessary to 40 | * maintain the empty circle property of the surrounding triangles. 41 | * In general this insertion strategy creates more but better shaped 42 | * triangles than CIS_CONSTRAINED_DELAUNAY. 43 | * \if SECTION_FADE25D 44 | * The height (z-coordinate) of new split points is adapted to existing 45 | * triangles. 46 | * - CIS_CONFORMING_DELAUNAY_SEGMENT_LEVEL is the same as the strategy 47 | * CIS_CONFORMING_DELAUNAY except that split points are not projected 48 | * to an existing surface but their height is interpolated between the 49 | * endpoints of the segment to be inserted. 50 | * \endif 51 | * 52 | * \note In former library versions the terms CIS_IGNORE_DELAUNAY 53 | * and CIS_KEEP_DELAUNAY were used but these were misleading and 54 | * are now deprecated. For backwards compatibility they are kept. 55 | */ 56 | enum ConstraintInsertionStrategy 57 | { 58 | CIS_CONFORMING_DELAUNAY=0, 59 | CIS_CONSTRAINED_DELAUNAY=1, 60 | #if GEOM_PSEUDO3D==GEOM_TRUE 61 | CIS_CONFORMING_DELAUNAY_SEGMENT_LEVEL=2, 62 | #endif 63 | CIS_KEEP_DELAUNAY=0, // Deprecated name 64 | CIS_IGNORE_DELAUNAY=1 // Deprecated name 65 | }; 66 | 67 | /** \brief Constraint segment 68 | */ 69 | class ConstraintSegment2 70 | { 71 | public: 72 | ConstraintSegment2(Point2* p0_,Point2* p1_,ConstraintInsertionStrategy cis_); 73 | ~ConstraintSegment2(); 74 | /** \brief Get the source point 75 | * \return the source point 76 | */ 77 | CLASS_DECLSPEC 78 | Point2* getSrc() const; 79 | /** \brief Get the target point 80 | * \return the source point 81 | */ 82 | CLASS_DECLSPEC 83 | Point2* getTrg() const; 84 | /** 85 | * \return if the object is alive 86 | */ 87 | CLASS_DECLSPEC 88 | bool isAlive() const; 89 | /** 90 | * \return the constraint insertion strategy (CIS) 91 | */ 92 | ConstraintInsertionStrategy getCIS() const; 93 | 94 | /** \brief operator<(..) 95 | * Compares by vertex pointers 96 | */ 97 | 98 | CLASS_DECLSPEC 99 | bool operator<(const ConstraintSegment2& pOther) const; 100 | 101 | /** \brief Split a constraint segment 102 | * 103 | * internal use. 104 | */ 105 | CLASS_DECLSPEC 106 | bool split(Point2* pSplit); 107 | /** \brief Split a constraint segment 108 | * 109 | * internal use 110 | */ 111 | CLASS_DECLSPEC 112 | bool splitAndRemovePrev(Point2* pSplit); 113 | /** \brief Add an owner 114 | * 115 | * Sets a specific ConstraintGraph2 as owner of the current ConstraintSegment2. 116 | * internal use. 117 | */ 118 | void addOwner(ConstraintGraph2* pOwner); 119 | /** \brief Remove an owner 120 | * Removes a specific ConstraintGraph2 as owner of the current ConstraintSegment2. 121 | * Mostly for internal use. 122 | */ 123 | void removeOwner(ConstraintGraph2* pOwner); 124 | CLASS_DECLSPEC 125 | /** \brief Get all children 126 | * Recursively retrieve all children of the current ConstraintSegment2. 127 | */ 128 | void getChildrenRec(std::vector& vChildConstraintSegments); 129 | CLASS_DECLSPEC 130 | /** \brief Get the children and the split point 131 | * Retrieve the two direct children of the current ConstraintSegment2 as well as the split point. 132 | */ 133 | void getChildrenAndSplitPoint(ConstraintSegment2*& pCSeg0,ConstraintSegment2*& pCSeg1,Point2*& pSplitPoint); 134 | CLASS_DECLSPEC 135 | friend std::ostream &operator<<(std::ostream &stream, const ConstraintSegment2& cSeg); 136 | /** \brief Return the number of owners 137 | * Returns the number of ConstraintGraphs where the current ConstraintSegment2 is a member 138 | */ 139 | CLASS_DECLSPEC 140 | size_t getNumberOfOwners() const; 141 | 142 | 143 | /** \brief Set adjacent area 144 | * A customer specific method, not thought for public use. If the 145 | * current constraint segment is a border segment, then the area of 146 | * the (non-existing) outside triangle can manually be deposited 147 | * here. This value is used by the extended meshing method when a 148 | * grow factor is given or ignored if not set. 149 | * \note Only use if you know what you are doing. 150 | */ 151 | CLASS_DECLSPEC 152 | void setAdjacentArea(double adjacentArea_); 153 | 154 | /** \brief Get adjacent area 155 | * A customer specific method, not thought for public use. Returns 156 | * a previously via setAdjacentArea(double adjacentArea_) deposited 157 | * value. If not explicitly set, DBL_MAX is returned. 158 | */ 159 | CLASS_DECLSPEC 160 | double getAdjacentArea() const; 161 | 162 | 163 | protected: 164 | Point2 *p0,*p1; 165 | ConstraintInsertionStrategy cis; 166 | std::set sOwners; 167 | std::vector vChildren; 168 | double adjacentArea; 169 | }; 170 | 171 | } // NAMESPACE 172 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/Edge2.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | 18 | #pragma once 19 | 20 | #include "common.h" 21 | #if GEOM_PSEUDO3D==GEOM_TRUE 22 | namespace GEOM_FADE25D { 23 | #elif GEOM_PSEUDO3D==GEOM_FALSE 24 | namespace GEOM_FADE2D { 25 | #else 26 | #error GEOM_PSEUDO3D is not defined 27 | #endif 28 | 29 | class Triangle2; 30 | class Point2; 31 | 32 | class Edge2 33 | { 34 | public: 35 | Edge2(); 36 | Edge2(const Edge2& e_); 37 | Edge2(Triangle2* c_,int oppIdx_); 38 | ~Edge2(); 39 | 40 | // Combinatorial operator< 41 | bool operator<(const Edge2& e) const 42 | { 43 | if(ce.c) return false; 45 | if(oppIdxe1.getLength2D()) return true; 90 | else return false; 91 | } 92 | }; 93 | 94 | #if GEOM_PSEUDO3D==GEOM_TRUE 95 | struct Func_ltEdge25D 96 | { 97 | bool operator()(const Edge2& e0,const Edge2& e1) const 98 | { 99 | if(e0.getLength25D() inline void unusedParameter(const T&){} // Avoids compiler warnings 17 | 18 | 19 | 20 | /** \brief Parameters for the mesh generator 21 | * 22 | * This class serves as container for mesh generator parameters. Client 23 | * code can provide a class which derives from MeshGenParams an which 24 | * provides custom implementations of the getMaxTriangleArea(Triangle* pT) 25 | * method or the getMaxEdgeLength(Triangle* pT) method in order to 26 | * gain control over the local density of the generated mesh. When the 27 | * meshing algorithm decides if a certain triangle T must be refined, 28 | * then it calls these functions. 29 | */ 30 | class CLASS_DECLSPEC MeshGenParams 31 | { 32 | public: 33 | MeshGenParams(Zone2* pZone_): 34 | #if GEOM_PSEUDO3D==GEOM_TRUE 35 | // *** The following two parameters exist only in Fade2.5D *** 36 | pHeightGuideTriangulation(NULL), 37 | maxHeightError(DBL_MAX), 38 | #endif 39 | pZone(pZone_), 40 | minAngleDegree(20.0), 41 | minEdgeLength(1e-3), 42 | maxEdgeLength(DBL_MAX), 43 | maxTriangleArea(DBL_MAX), 44 | bAllowConstraintSplitting(true), 45 | growFactor(DBL_MAX), 46 | growFactorMinArea(1e-3), 47 | capAspectLimit(10.0), 48 | gridLength(0.0) 49 | { 50 | if( pZone->getZoneLocation()!=ZL_INSIDE && 51 | pZone->getZoneLocation()!=ZL_BOUNDED ) 52 | { 53 | std::cerr<<"\n\n\nMeshGenParams::MeshGenParams(..), ERROR: \n\tbehavior since Fade 1.39: The zoneLocation must be ZL_INSIDE or ZL_BOUNDED. Please convert the zone using pZone->convertToBoundedZone(..)\n\n"<5.0, Default: growFactor=DBL_MAX 179 | */ 180 | double growFactor; 181 | 182 | /** \brief growFactorMinArea 183 | * 184 | * The growFactor value is ignored for triangles with a smaller area 185 | * than growFactorMinArea. This value prevents generation of hundreds 186 | * of tiny triangles around one that is unusually small. Default: 0.001 187 | */ 188 | double growFactorMinArea; 189 | 190 | /** \brief capAspectLimit 191 | * 192 | * Limits the quotient edgeLength / height. Default value: 10.0 193 | */ 194 | double capAspectLimit; 195 | 196 | 197 | /** \brief gridVector 198 | * 199 | * When pre-meshing with grid points is used then the grid will be 200 | * aligned along this vector. By default %s gridVector is axis aligned. 201 | */ 202 | Vector2 gridVector; 203 | 204 | /** \brief gridLength 205 | * 206 | * Set %gridLength > 0 to insert grid points before the mesh generator 207 | * runs. By default % gridLength=0 (off). 208 | * 209 | * @warning Small values can produce a huge number of triangles 210 | */ 211 | double gridLength; 212 | 213 | /** \brief Commad 214 | * 215 | * A command string, not for public use. 216 | */ 217 | // std::string command; 218 | }; 219 | 220 | 221 | } // NAMESPACE 222 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/MsgBase.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | 5 | #if GEOM_PSEUDO3D==GEOM_TRUE 6 | namespace GEOM_FADE25D { 7 | #elif GEOM_PSEUDO3D==GEOM_FALSE 8 | namespace GEOM_FADE2D { 9 | #else 10 | #error GEOM_PSEUDO3D is not defined 11 | #endif 12 | 13 | enum MsgType 14 | { 15 | MSG_PROGRESS, 16 | MSG_WARNING 17 | }; 18 | 19 | /** \brief %MsgBase 20 | * 21 | * MsgBase is a base class from which message subscriber classes (for 22 | * example widgets, progress bars, ...) can be derived which then 23 | * receive messages (progress, warnings, ...) from Fade. 24 | * 25 | */ 26 | class CLASS_DECLSPEC MsgBase 27 | { 28 | public: 29 | MsgBase(){}; 30 | virtual ~MsgBase(){} 31 | 32 | 33 | /** \brief update 34 | * 35 | * This method must be defined in derived message subscriber classes. It 36 | * is automatically called everytime Fade has a message of type \e msgType. 37 | * 38 | */ 39 | virtual void update(MsgType msgType,const std::string& s,double d)=0; 40 | }; 41 | 42 | 43 | } // Namespace 44 | 45 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/Performance.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | 18 | #pragma once 19 | #include 20 | 21 | 22 | #include "common.h" 23 | #if GEOM_PSEUDO3D==GEOM_TRUE 24 | namespace GEOM_FADE25D { 25 | #elif GEOM_PSEUDO3D==GEOM_FALSE 26 | namespace GEOM_FADE2D { 27 | #else 28 | #error GEOM_PSEUDO3D is not defined 29 | #endif 30 | 31 | 32 | /** \brief Timer 33 | * 34 | * Call the timer function with a certain string to start time 35 | * measurement. Call it a second time with the same string to 36 | * finish the time measurement. 37 | * 38 | * @return -1 when the timer is started or the elapsed time in 39 | * seconds when the timer is stopped. 40 | */ 41 | CLASS_DECLSPEC 42 | double timer(const std::string& str); 43 | 44 | } // (namespace) 45 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/Publisher.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "common.h" 3 | #include "MsgBase.h" 4 | 5 | #if GEOM_PSEUDO3D==GEOM_TRUE 6 | namespace GEOM_FADE25D { 7 | #elif GEOM_PSEUDO3D==GEOM_FALSE 8 | namespace GEOM_FADE2D { 9 | #else 10 | #error GEOM_PSEUDO3D is not defined 11 | #endif 12 | 13 | typedef std::multimap MMTypMsg; 14 | typedef MMTypMsg::iterator MMTypMsgIt; 15 | class Publisher 16 | { 17 | public: 18 | Publisher():progress_todo(0),progress_done(0),progress_broadcast_interval(0),bEndPublished(false) 19 | { 20 | } 21 | void startProgress(const std::string& name,size_t todo) 22 | { 23 | if(mmSubscribers.empty()) return; 24 | progress_todo=todo; 25 | progress_done=0; 26 | progress_name=name; 27 | progress_broadcast_interval=progress_todo/100; 28 | if(progress_broadcast_interval<1) progress_broadcast_interval=1; 29 | bEndPublished=false; 30 | } 31 | void endProgress() 32 | { 33 | if(mmSubscribers.empty()) return; 34 | progress_done=progress_todo; 35 | if(!bEndPublished) broadcast(MSG_PROGRESS,progress_name,1.0); 36 | progress_name.clear(); 37 | progress_todo=0; 38 | progress_done=0; 39 | } 40 | void incProgress(size_t increment=1) 41 | { 42 | if(mmSubscribers.empty() || progress_todo==0 || bEndPublished) return; 43 | progress_done+=increment; 44 | if(progress_done%progress_broadcast_interval==0) 45 | { 46 | double prog(double(progress_done)/double(progress_todo)); 47 | if(prog>=1.0) 48 | { 49 | prog=1.0; 50 | bEndPublished=true; 51 | } 52 | broadcast(MSG_PROGRESS,progress_name,prog); 53 | } 54 | } 55 | 56 | void incTodo(size_t increment=1) 57 | { 58 | if(mmSubscribers.empty()) return; 59 | progress_todo+=increment; 60 | } 61 | 62 | void broadcast(MsgType msgType,const std::string& s,double d) 63 | { 64 | std::pair result(mmSubscribers.equal_range(msgType)); 65 | for(;result.first!=result.second;++result.first) 66 | { 67 | MsgBase* pCurrentMsgBase(result.first->second); 68 | pCurrentMsgBase->update(msgType,s,d); 69 | } 70 | } 71 | 72 | void subscribe(MsgType msgType,MsgBase* pMsg) 73 | { 74 | std::pair result(mmSubscribers.equal_range(msgType)); 75 | for(;result.first!=result.second;++result.first) 76 | { 77 | MsgBase* pCurrentMsgBase(result.first->second); 78 | if(pCurrentMsgBase==pMsg) 79 | { 80 | return; 81 | } 82 | } 83 | mmSubscribers.insert(std::make_pair(msgType,pMsg)); 84 | } 85 | void unsubscribe(MsgType msgType,MsgBase* pMsg) 86 | { 87 | std::pair result(mmSubscribers.equal_range(msgType)); 88 | for(;result.first!=result.second;++result.first) 89 | { 90 | MsgBase* pCurrentMsgBase(result.first->second); 91 | if(pCurrentMsgBase==pMsg) 92 | { 93 | mmSubscribers.erase(result.first); 94 | return; 95 | } 96 | } 97 | std::cerr<<"Publisher::unsubscribe(), warning, no such subscriber found"<getSrc()getSrc()) return true; 113 | if(seg0->getSrc()>seg1->getSrc()) return false; 114 | if(seg0->getTrg()getTrg()) return true; 115 | if(seg0->getTrg()>seg1->getTrg()) return false; 116 | return false; 117 | } 118 | }; 119 | /** 120 | \endcond 121 | */ 122 | } // (namespace) 123 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/SegmentChecker.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #pragma once 4 | #include 5 | #include "common.h" 6 | #include "Segment2.h" 7 | #include "MsgBase.h" 8 | 9 | 10 | /** The Segment intersection type enumerates the way two line segments intersect each other 11 | */ 12 | enum SegmentIntersectionType 13 | { 14 | SIT_UNINITIALIZED, /**< Invalid value */ 15 | SIT_NONE, /**< No intersection */ 16 | SIT_SEGMENT, /**< The intersection is a non-degenerate segment (collinear intersection) */ 17 | SIT_POINT, /**< The intersection is a single point differnt from the endpoints */ 18 | SIT_ENDPOINT /**< The two segments share a common endpoint which is the only intersection */ 19 | }; 20 | 21 | #if GEOM_PSEUDO3D==GEOM_TRUE 22 | namespace GEOM_FADE25D { 23 | #elif GEOM_PSEUDO3D==GEOM_FALSE 24 | namespace GEOM_FADE2D { 25 | #else 26 | #error GEOM_PSEUDO3D is not defined 27 | #endif 28 | 29 | struct SegmentCheckerData; // FWD, pImpl to ease DLL export 30 | 31 | 32 | /** \brief SegmentChecker identifies intersecting line segments 33 | * 34 | * SegmentChecker takes a bunch of line segments and fully automatically 35 | * identifies illegal segment intersections. The intersection points can 36 | * be computed in 2D and in 2.5D. Further this class offers visualization 37 | * methods. Due to the underlying datastructure the search algorithm 38 | * scales very well to large inputs. 39 | * 40 | * \image html identify_intersecting_segments.png "Polylines: Intersecting segments are automatically found" 41 | * \image latex identify_intersecting_segments.eps "Polylines: Intersecting segments are automatically found" width=20cm 42 | * 43 | * 44 | */ 45 | class CLASS_DECLSPEC SegmentChecker 46 | { 47 | public: 48 | // *** CTOR and DTOR *** 49 | 50 | /** 51 | * @param vSegments_ contains the segments to be checked 52 | */ 53 | SegmentChecker(const std::vector& vSegments_); 54 | ~SegmentChecker(); 55 | 56 | /** 57 | * 58 | * Returns the i-th segment 59 | * 60 | * @param i is the index of the segment to be returned 61 | */ 62 | Segment2* getSegment(size_t i) const; 63 | /** Returns the number of segments contained in this SegmentChecker object 64 | */ 65 | size_t getNumberOfSegments() const; 66 | /** Returns the index of a segment 67 | * 68 | * @param pSeg is the segment whose index is returned 69 | */ 70 | int getIndex(Segment2* pSeg) const; 71 | /** Register a progress bar object 72 | * 73 | * The SegmentChecker does its job typically in fractions of a second. 74 | * But inputs may contain a quadratic number of intersections and such 75 | * tasks take a while. Therefore a user defined message object (your 76 | * own progress-bar class) can be registered in order to get progress 77 | * updates. This step is optional. 78 | * 79 | * @param msgType is the message type. For progress information 80 | * the type is always MSG_PROGRESS 81 | * @param pMsg is a user defined progress bar which derives from 82 | * Fade's MsgBase. 83 | * 84 | * 85 | */ 86 | void subscribe(MsgType msgType,MsgBase* pMsg); 87 | /** Unregister a progress bar object 88 | * 89 | * @param msgType is the message type. For progress information 90 | * the type is always MSG_PROGRESS 91 | * @param pMsg is a user defined class which derives from Fade's MsgBase 92 | */ 93 | void unsubscribe(MsgType msgType,MsgBase* pMsg); 94 | 95 | // *** Visualization and debug *** 96 | /** Write all segments, with and without intersection, to a postscript file 97 | * 98 | * @param name is the output filename 99 | * 100 | * \image html show_line_segments.png "Line segments written to a postscript file" 101 | * \image latex show_line_segments.eps "Line segments written to a postscript file" width=20cm 102 | */ 103 | void showSegments(const std::string& name) const; 104 | 105 | 106 | /** Write a postscript file, highlight illegal segments 107 | * 108 | * @param bAlsoEndPointIntersections specifies if intersections at endpoints are also illegal 109 | * @param name is the output filename 110 | * 111 | * \image html show_illegal_segments_count.png "Visualization of polyline intersections" 112 | * \image latex show_illegal_segments_count.eps "Visualization of polyline intersections" width=20cm 113 | */ 114 | void showIllegalSegments(bool bAlsoEndPointIntersections,const std::string& name) const; 115 | 116 | // *** Intersectors *** 117 | /** Get illegal segments 118 | * 119 | * Returns segments which are involved in intersections. Intersections 120 | * at endpoints are only reported when \p bAlsoEndPointIntersections is true. 121 | * 122 | * \image html segment_intersection_types.png "Segment intersections: (1) Non-collinear, (2) collinear, (3) duplicate segments, (4) endpoint intersection" 123 | * 124 | * @param bAlsoEndPointIntersections specifies if intersections at endpoints shall be detected 125 | * @param [out] vIllegalSegmentsOut is the output vector 126 | * 127 | * 128 | */ 129 | void getIllegalSegments(bool bAlsoEndPointIntersections,std::vector& vIllegalSegmentsOut) const; 130 | 131 | /** Get the intersection type of two segments 132 | * 133 | * @param pSeg1,pSeg2 are the segments to be checked 134 | * 135 | * @return SIT_NONE (no intersection),\n SIT_SEGMENT (collinear intersection),\n SIT_POINT (intersection somewhere between the endpoints) or\n SIT_ENDPOINT (endpoint intersection) 136 | * 137 | * 138 | * @note \p pSeg1 and \p pSeg2 do not need to be from the set that has 139 | * been used to initialize the present object 140 | */ 141 | SegmentIntersectionType getIntersectionType(Segment2* pSeg1,Segment2* pSeg2) const; 142 | 143 | 144 | /** Return segments that intersect a certain segment along with their intersection type 145 | * 146 | * @param pTestSegment is the segment to be analyzed 147 | * @param bAlsoEndPointIntersections specifies if intersections of 148 | * type SIT_ENDPOINT shall also be reported. 149 | * @param [out] vIntersectorsOut is the output vector. It returns segments 150 | * which intersect \p pTestSegment along with their intersection type 151 | */ 152 | void getIntersectors( Segment2* pTestSegment, 153 | bool bAlsoEndPointIntersections, 154 | std::vector >& vIntersectorsOut) const; 155 | 156 | // Intersections 157 | #if GEOM_PSEUDO3D==GEOM_TRUE 158 | /** Compute the intersection point(s) of two segments 159 | * 160 | * Use 161 | * \link getIntersectionType() \endlink to determine the segment 162 | * intersection type \p sit. 163 | * 164 | * @param sit is the segment intersection type (SIT_POINT or SIT_ENDPOINT for the present method) 165 | * @param seg0,seg1 are the intersecting segments 166 | * @param [out] ispOut0 output intersection point at \p seg0 167 | * @param [out] ispOut1 output intersection point at \p seg1 168 | * 169 | * The two output intersection points \p ispOut0 and \p ispOut1 170 | * have same (x,y) coordinates but possibly different heights z. 171 | * 172 | * @note It is not required that \p pSeg1 and \p pSeg2 are from the 173 | * set of segments that have been used to initialize the SegmentChecker. 174 | */ 175 | void getIntersectionPoint( SegmentIntersectionType sit, 176 | const Segment2& seg0, 177 | const Segment2& seg1, 178 | Point2& ispOut0, 179 | Point2& ispOut1 180 | ) const; 181 | /** Compute the intersection segment(s) of two collinear intersecting segments 182 | * 183 | * @param seg0,seg1 are intersecting segments such that their SegmentIntersectionType is SIT_SEGMENT 184 | * @param [out] issOut0 intersection segment at \p seg0 185 | * @param [out] issOut1 intersection segment at \p seg1 186 | * 187 | * The two output segments have same (x,y) coordinates but possibly 188 | * diffent heights z. 189 | * 190 | * @note \p pSeg1 and \p pSeg2 do not need to be from the set that has 191 | * been used to initialize the SegmentChecker. 192 | 193 | */ 194 | 195 | void getIntersectionSegment(const Segment2& seg0, 196 | const Segment2& seg1, 197 | Segment2& issOut0, 198 | Segment2& issOut1 199 | ) const; 200 | #else 201 | /** Compute the intersection point of two segments 202 | * 203 | * Use \link getIntersectionType() \endlink to determine the segment 204 | * intersection type \p sit. 205 | 206 | * @param typ is the intersection type (SIT_POINT or SIT_ENDPOINT for the present method) 207 | * @param seg0,seg1 are the intersecting segments 208 | * @param [out] ispOut is the output intersection point. 209 | * 210 | * @note \p pSeg1 and \p pSeg2 do not need to be from the set that has 211 | * been used to initialize the SegmentChecker. 212 | * 213 | */ 214 | void getIntersectionPoint( SegmentIntersectionType typ, 215 | const Segment2& seg0, 216 | const Segment2& seg1, 217 | Point2& ispOut 218 | ) const; 219 | /** Computes the intersection segment of two collinear intersecting segments 220 | * 221 | * @param seg0,seg1 are intersecting segments such that their SegmentIntersectionType is SIT_SEGMENT 222 | * @param [out] issOut is the computed intersection of \p seg0 and \p seg1 223 | * 224 | * @note \p pSeg1 and \p pSeg2 do not need to be from the set that has 225 | * been used to initialize the present object 226 | 227 | */ 228 | 229 | void getIntersectionSegment(const Segment2& seg0, 230 | const Segment2& seg1, 231 | Segment2& issOut 232 | ) const; 233 | #endif 234 | 235 | /** Return the intersection type as a human readable string. This is a convenience function 236 | * 237 | * @param sit is an intersection type to be converted to a string 238 | * 239 | */ 240 | 241 | std::string getIntersectionTypeString(SegmentIntersectionType sit) const; 242 | 243 | 244 | private: 245 | 246 | /** 247 | \cond HIDDEN_SYMBOLS 248 | */ 249 | SegmentIntersectionType getIntersectionType_degeneratePart( 250 | Segment2*& pSeg1, 251 | Segment2*& pSeg2, 252 | Point2& p1, 253 | Point2& p2, 254 | Point2& p3, 255 | Point2& p4 256 | ) const; 257 | void addSegments(const std::vector& vSegments_); 258 | 259 | // Data 260 | SegmentCheckerData* pSCD; 261 | 262 | 263 | /** 264 | \endcond 265 | */ 266 | 267 | }; 268 | 269 | } // Namespace 270 | 271 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/TriangleAroundVertexIterator.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | 18 | #pragma once 19 | #include "common.h" 20 | #include "Point2.h" 21 | #include "Triangle2.h" 22 | 23 | 24 | #include "common.h" 25 | #if GEOM_PSEUDO3D==GEOM_TRUE 26 | namespace GEOM_FADE25D { 27 | #elif GEOM_PSEUDO3D==GEOM_FALSE 28 | namespace GEOM_FADE2D { 29 | #else 30 | #error GEOM_PSEUDO3D is not defined 31 | #endif 32 | 33 | 34 | inline 35 | int inc1( int num) 36 | { 37 | ++num; 38 | if(num>2) return 0; 39 | return num; 40 | } 41 | 42 | inline 43 | int inc2( int num) 44 | { 45 | --num; 46 | if(num<0) return 2; 47 | return num; 48 | } 49 | /** \brief Iterator for all triangles around a given vertex 50 | * 51 | * Iterates over all triangles incident to a given vertex in a circular manner. Thereby, 52 | * counterclockwise is the positive direction. 53 | * 54 | * \image html umbrella.jpg "Left: the iterator visits the triangles around a vertex. Right: The iterator 'jumps' over the border edges of the triangulation" 55 | * \image latex umbrella.eps "Left: the iterator visits the triangles around a vertex. Right: The iterator 'jumps' over the border edges of the triangulation" width=10cm 56 | * 57 | */ 58 | class CLASS_DECLSPEC TriangleAroundVertexIterator 59 | { 60 | public: 61 | /** \brief Constructor 62 | * 63 | * @param pPnt_ is the vertex whose incident triangles can be visited with the iterator 64 | * 65 | * The iterator will start at an arbitrary triangle 66 | */ 67 | TriangleAroundVertexIterator(Point2* pPnt_):pPnt(pPnt_),pTr(pPnt_->getIncidentTriangle()),pSavedTr(NULL) 68 | { 69 | if(pTr==NULL) 70 | { 71 | std::cerr<<"TriangleAroundVertexIterator::TriangleAroundVertexIterator(), created from an invalid point"<getIntraTriangleIndex(pPnt)); 111 | std::swap(pSavedTr,pTr); 112 | pTr=pSavedTr->getOppositeTriangle(ccwIdx); 113 | 114 | return *this; 115 | } 116 | 117 | /** \brief Proceed to the previous triangle (the one in clockwise order) 118 | * 119 | * @warning At the border of a triangulation, two border edges are incident to the 120 | * center vertex. Consequently, the neighbor triangles are NULL there. If 121 | * operator--() leads the iterator off the triangulation then the iterator 122 | * will point to NULL. Another call to operator--() will set the iterator to 123 | * the next triangle in clockwise order. 124 | */ 125 | TriangleAroundVertexIterator& operator--() 126 | { 127 | if(pTr==NULL) 128 | { 129 | loop(); 130 | return *this; 131 | } 132 | int cwIdx=inc2(pTr->getIntraTriangleIndex(pPnt)); 133 | std::swap(pSavedTr,pTr); 134 | pTr=pSavedTr->getOppositeTriangle(cwIdx); 135 | return *this; 136 | } 137 | /** \brief Check if the center points and the current triangles of the iterators are the same 138 | * 139 | */ 140 | bool operator==(const TriangleAroundVertexIterator& rhs) 141 | { 142 | return (pPnt==rhs.pPnt && pTr==rhs.pTr); 143 | } 144 | /** \brief Check if the center points or the current triangles of the iterators are different 145 | * 146 | */ 147 | bool operator!=(const TriangleAroundVertexIterator& rhs) 148 | { 149 | return (pPnt!=rhs.pPnt || pTr!=rhs.pTr); 150 | } 151 | /** \brief Returns a pointer to the current triangle (or NULL) 152 | * 153 | * Dereferencing the iterator yields a pointer to the triangle to which the iterator points. 154 | * @warning This method might yield NULL at the border of a triangulation. 155 | */ 156 | Triangle2* operator*() 157 | { 158 | return pTr; 159 | } 160 | /** \brief Preview next triangle (CCW direction) 161 | * 162 | * @return the next triangle (the one in CCW direction) without changing 163 | * the current position. 164 | * @warning This method might yield NULL at the border of a triangulation. 165 | */ 166 | Triangle2* previewNextTriangle() 167 | { 168 | TriangleAroundVertexIterator tmp(*this); 169 | ++tmp; 170 | return *tmp; 171 | } 172 | /** \brief Preview previous triangle (CW direction) 173 | * 174 | * @return the previous triangle (the one in CW direction) without 175 | * changing the current position. 176 | * @warning This method might yield NULL at the border of a triangulation. 177 | */ 178 | Triangle2* previewPrevTriangle() 179 | { 180 | TriangleAroundVertexIterator tmp(*this); 181 | --tmp; 182 | return *tmp; 183 | } 184 | 185 | 186 | 187 | protected: 188 | Point2* pPnt; 189 | Triangle2 *pTr,*pSavedTr; 190 | 191 | void loop() 192 | { 193 | assert(pTr==NULL && pSavedTr!=NULL); 194 | enum DIRECTION{DIRECTION_NONE,DIRECTION_BACK,DIRECTION_FWD}; 195 | DIRECTION direction(DIRECTION_NONE); 196 | 197 | int axisIndex=pSavedTr->getIntraTriangleIndex(pPnt); 198 | 199 | if(pSavedTr->getOppositeTriangle(inc2(axisIndex))!=NULL) 200 | { 201 | direction=DIRECTION_BACK; 202 | } 203 | if(pSavedTr->getOppositeTriangle(inc1(axisIndex))!=NULL) 204 | { 205 | assert(direction==DIRECTION_NONE); 206 | direction=DIRECTION_FWD; 207 | } 208 | pTr=pSavedTr; 209 | if(direction==DIRECTION_FWD) while(*operator++()!=NULL); // fast forward 210 | if(direction==DIRECTION_BACK) while(*operator--()!=NULL); // rewind 211 | pTr=pSavedTr; 212 | } 213 | }; 214 | 215 | 216 | } // (namespace) 217 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/Vector2.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | 18 | #pragma once 19 | #include "common.h" 20 | 21 | 22 | #if GEOM_PSEUDO3D==GEOM_TRUE 23 | namespace GEOM_FADE25D { 24 | #elif GEOM_PSEUDO3D==GEOM_FALSE 25 | namespace GEOM_FADE2D { 26 | #else 27 | #error GEOM_PSEUDO3D is not defined 28 | #endif 29 | 30 | class CLASS_DECLSPEC Vector2; 31 | 32 | /** \brief Vector 33 | * 34 | * This class represents a vector in 2D 35 | */ 36 | class Vector2 37 | { 38 | public: 39 | 40 | #if GEOM_PSEUDO3D==GEOM_TRUE 41 | /** \brief Constructor 42 | * 43 | */ 44 | 45 | Vector2(const double x_,const double y_,const double z_); 46 | 47 | /** \brief Default constructor 48 | * 49 | * The vector is initialized to (0,0,0) 50 | */ 51 | 52 | Vector2(); 53 | 54 | /** \brief Copy constructor 55 | * 56 | * Create a copy of vector v_ 57 | */ 58 | 59 | Vector2(const Vector2& v_); 60 | 61 | #else 62 | 63 | /** \brief Constructor 64 | * 65 | */ 66 | 67 | Vector2(const double x_,const double y_); 68 | /** \brief Default constructor 69 | * 70 | * The vector is initialized to (0,0) 71 | */ 72 | 73 | Vector2(); 74 | /** \brief Copy constructor 75 | * 76 | * Create a copy of vector v_ 77 | */ 78 | 79 | Vector2(const Vector2& v_); 80 | #endif 81 | 82 | 83 | 84 | #if GEOM_PSEUDO3D==GEOM_TRUE 85 | /** \brief Get an orthogonal vector (CCW direction) 86 | * 87 | * @note: Only (x,y) coordinates are computed, z=0 88 | */ 89 | #else 90 | /** \brief Get an orthogonal vector (CCW direction) 91 | */ 92 | #endif 93 | 94 | Vector2 orthogonalVector() const; 95 | 96 | /** \brief Get the x-value 97 | */ 98 | 99 | double x() const; 100 | 101 | /** \brief Get the y-value 102 | */ 103 | 104 | double y() const; 105 | 106 | #if GEOM_PSEUDO3D==GEOM_TRUE 107 | /** \brief Get the z-value. 108 | */ 109 | 110 | double z() const; 111 | #endif 112 | 113 | #if GEOM_PSEUDO3D==GEOM_TRUE 114 | /** \brief Set the values 115 | */ 116 | void set(const double x_,const double y_,const double z_); 117 | 118 | #else 119 | /** \brief Set the values 120 | */ 121 | void set(const double x_,const double y_); 122 | #endif 123 | 124 | 125 | /** \brief Get the length of the vector 126 | */ 127 | 128 | double length() const; 129 | 130 | /** \brief Scalar product 131 | */ 132 | #if GEOM_PSEUDO3D==GEOM_TRUE 133 | 134 | double operator*(const Vector2& other) const; 135 | #else 136 | 137 | double operator*(const Vector2& other) const; 138 | #endif 139 | 140 | 141 | /** \brief Multiply by a scalar value 142 | */ 143 | #if GEOM_PSEUDO3D==GEOM_TRUE 144 | 145 | Vector2 operator*(double val) const; 146 | #else 147 | 148 | Vector2 operator*(double val) const; 149 | #endif 150 | 151 | /** \brief Divide by a scalar value 152 | */ 153 | #if GEOM_PSEUDO3D==GEOM_TRUE 154 | 155 | Vector2 operator/(double val) const; 156 | #else 157 | 158 | Vector2 operator/(double val) const; 159 | #endif 160 | 161 | 162 | protected: 163 | double valX; 164 | double valY; 165 | #if GEOM_PSEUDO3D==GEOM_TRUE 166 | double valZ; 167 | #endif 168 | }; 169 | 170 | 171 | 172 | 173 | // Free functions 174 | 175 | CLASS_DECLSPEC 176 | inline std::ostream &operator<<(std::ostream &stream, const Vector2& vec) 177 | { 178 | #if GEOM_PSEUDO3D==GEOM_TRUE 179 | stream << "Vector2: "<0) 211 | { 212 | return Vector2(other.x()/len,other.y()/len,other.z()/len); 213 | } 214 | else 215 | { 216 | std::cout<<"warning: normalize(const Vector2& other), Null length vector!"<0) 221 | { 222 | 223 | return Vector2(other.x()/len,other.y()/len); 224 | } 225 | else 226 | { 227 | std::cout<<"warning: normalize(const Vector2& other), Null length vector!"<& vPoints,const Color& c); 70 | 71 | /** \brief Add a vector of Point2* to the visualization 72 | */ 73 | CLASS_DECLSPEC 74 | void addObject(const std::vector& vPoints,const Color& c); 75 | 76 | 77 | /** \brief Add a vector of Segment2 objects to the visualization 78 | */ 79 | CLASS_DECLSPEC 80 | void addObject(const std::vector& vSegments,const Color& c); 81 | 82 | /** \brief Add a vector of Triangle2 objects to the visualization 83 | */ 84 | CLASS_DECLSPEC 85 | void addObject(const std::vector& vT,const Color& c); 86 | 87 | /** \brief Add a Circle2 object to the visualization 88 | */ 89 | CLASS_DECLSPEC 90 | void addObject(const Circle2& circ,const Color& c); 91 | /** \brief Add a Point2 object to the visualization 92 | */ 93 | CLASS_DECLSPEC 94 | void addObject(const Point2& pnt,const Color& c); 95 | /** \brief Add a Triangle2 object to the visualization 96 | */ 97 | CLASS_DECLSPEC 98 | void addObject(const Triangle2& tri,const Color& c); 99 | 100 | /** \brief Add a Triangle2* vector to the visualization 101 | */ 102 | CLASS_DECLSPEC 103 | void addObject(const std::vector vT,const Color& c); 104 | 105 | /** \brief Add a Label object to the visualization 106 | */ 107 | CLASS_DECLSPEC 108 | void addObject(const Label& lab,const Color& c); 109 | 110 | /** \brief Add a header line to the visualization 111 | */ 112 | CLASS_DECLSPEC 113 | void addHeaderLine(const std::string& s); 114 | 115 | 116 | /** \brief Finish and write the postscript file 117 | * 118 | * @note This method \e must be called at the end when all the objects have been added. 119 | */ 120 | CLASS_DECLSPEC 121 | void writeFile(); 122 | 123 | protected: 124 | std::ofstream outFile; 125 | std::vector > vSegments; 126 | std::vector > vCircles; 127 | std::vector > vPoints; 128 | std::vector > vTriangles; 129 | std::vector > vLabels; 130 | void writeHeaderLines(); 131 | 132 | int updateCtr; 133 | Bbox2 bbox; 134 | bool bFill; 135 | 136 | Point2 scaledPoint(const Point2 &p); 137 | double scaledDouble(const double &d); 138 | void changeColor(float r,float g,float b,float linewidth,bool bFill); 139 | void changeColor(const Color& c); 140 | void writeHeader(std::string title); 141 | void writeFooter(); 142 | void writeLabel(Label l); 143 | void writeLine(const Point2& pSource,const Point2& pTarget); 144 | void writeTriangle(const Triangle2* pT,bool bFill_); 145 | void writePoint(Point2& p1_,float size); 146 | void writeCircle(const Point2& p1_,double radius); 147 | void periodicStroke(); 148 | void setRange(); 149 | Color lastColor; 150 | std::string filename; 151 | std::vector vHeaderLines; 152 | }; 153 | 154 | 155 | } // (namespace) 156 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/common.h: -------------------------------------------------------------------------------- 1 | // (c) 2010 Geom e.U. Bernhard Kornberger, Graz/Austria. All rights reserved. 2 | // 3 | // This file is part of the Fade2D library. You can use it for your personal 4 | // non-commercial research. Licensees holding a commercial license may use this 5 | // file in accordance with the Commercial License Agreement provided 6 | // with the Software. 7 | // 8 | // This software is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING 9 | // THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. 10 | // 11 | // Please contact the author if any conditions of this licensing are not clear 12 | // to you. 13 | // 14 | // Author: Bernhard Kornberger, bkorn (at) geom.at 15 | // http://www.geom.at 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | // When the library is built, a static local version of this file is 33 | // used. When the client software uses this file, it is generated by 34 | // CMake. 35 | #include "fadeVersion.h" 36 | 37 | 38 | // DLL IMPORT/EXPORT MACRO 39 | #if defined (_WIN32) 40 | // ** WINDOWS ** 41 | #if defined(FADE2D_EXPORT) 42 | #define CLASS_DECLSPEC __declspec(dllexport) 43 | #else 44 | #define CLASS_DECLSPEC __declspec(dllimport) 45 | #endif 46 | #else 47 | // ** LINUX and other ** 48 | #define CLASS_DECLSPEC 49 | #endif 50 | 51 | 52 | 53 | //#define GEOM_OMP_ACTIVE GEOM_TRUE 54 | #define GEOM_OMP_ACTIVE GEOM_FALSE 55 | 56 | 57 | //#define GEOM_BENCHCOUNTING 58 | #ifdef GEOM_BENCHCOUNTING 59 | #define BC(x) benchCounter(x); 60 | #else 61 | #define BC(x) 62 | #endif 63 | 64 | 65 | 66 | // NAMESPACE 67 | #if GEOM_PSEUDO3D==GEOM_TRUE 68 | namespace GEOM_FADE25D { 69 | #elif GEOM_PSEUDO3D==GEOM_FALSE 70 | namespace GEOM_FADE2D { 71 | #else 72 | #error GEOM_PSEUDO3D is not defined 73 | #endif 74 | 75 | 76 | 77 | /** \brief Empty class. Used as marker for experimental features 78 | * 79 | * New features are necessarily part of the API. To protect users 80 | * from code which may change or even disappear in future releases 81 | * this class is used as tag. By using it you say "I know what I'm 82 | * doing". 83 | */ 84 | 85 | class CLASS_DECLSPEC AcceptExperimentalFeature{}; 86 | 87 | enum ZoneLocation 88 | { 89 | ZL_GLOBAL, 90 | ZL_INSIDE, 91 | ZL_OUTSIDE, 92 | ZL_GROW, 93 | ZL_RESULT_UNION, 94 | ZL_RESULT_INTERSECTION, 95 | ZL_RESULT_DIFFERENCE, 96 | ZL_RESULT_SYM_DIFFERENCE, 97 | ZL_BOUNDED 98 | }; 99 | 100 | enum Orientation2 101 | { 102 | ORIENTATION2_COLLINEAR, 103 | ORIENTATION2_CCW, 104 | ORIENTATION2_CW 105 | }; 106 | 107 | template struct func_ltDerefPtr 108 | { 109 | bool operator()(T_PTR p0,T_PTR p1) const {return *p0<*p1;} 110 | }; 111 | 112 | 113 | 114 | 115 | struct FadeException: public std::exception 116 | { 117 | virtual const char* what() const throw() 118 | { 119 | return "\n Fade BUG: This should never happen, sorry. Please report this \n incident and it will be fixed quickly: bkorn@geom.at"; 120 | } 121 | }; 122 | struct FadeLicenseException: public std::exception 123 | { 124 | virtual const char* what() const throw() 125 | { 126 | return "Fade: Your program used an unlicensed feature, sorry."; 127 | } 128 | }; 129 | 130 | 131 | 132 | 133 | 134 | } // (namespace) 135 | 136 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/fadeVersion.h: -------------------------------------------------------------------------------- 1 | #ifdef FADE2D_EXPORT 2 | #error FADE2D_EXPORT is defined 3 | #endif 4 | 5 | #pragma once 6 | #define GEOM_TRUE 100 7 | #define GEOM_FALSE 99 8 | #undef GEOM_PSEUDO3D 9 | #define GEOM_PSEUDO3D GEOM_FALSE 10 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/include_fade2d/testDataGenerators.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "Point2.h" 3 | #include "Segment2.h" 4 | #include 5 | 6 | 7 | 8 | 9 | #if GEOM_PSEUDO3D==GEOM_TRUE 10 | namespace GEOM_FADE25D { 11 | #elif GEOM_PSEUDO3D==GEOM_FALSE 12 | namespace GEOM_FADE2D { 13 | #else 14 | #error GEOM_PSEUDO3D is not defined 15 | #endif 16 | 17 | 18 | /** \defgroup dataGenerators Test Data Generators 19 | * 20 | * \image html random_objects.png "Random objects (points, lines, polygons, polylines)" 21 | * 22 | * # Generate random polygons and other test objects 23 | * Theory, careful programming and automated software stress tests. 24 | * Neither of them can replace the other one. Testing with random data 25 | * helps to discover errors early. Fade provides random object 26 | * generators for your automated software stress tests: 27 | * 28 | * - Random simple polygons 29 | * - Random segments 30 | * - Random point clouds 31 | * - Random numbers. 32 | * - Polylines from sine functions 33 | * 34 | * 35 | * If you discover an error in your software you must be able to reproduce 36 | * the input data that has triggered your bug. For this reason the random 37 | * object generators take a seed value to initialize the internal random 38 | * number generators. A certain seed value always leads to the same 39 | * sequence of objects. Only when the special seed value 0 is used then 40 | * the random number generators are initialized from the system time. 41 | * 42 | * @{ 43 | */ 44 | 45 | /** @brief Generate random numbers 46 | * 47 | * @param num Number of random numbers to be generated 48 | * @param min Lower bound 49 | * @param max Upper bound 50 | * @param [out] vRandomNumbersOut is the output vector 51 | * @param seed initializes the random number generator RNG 52 | * (default: 0...mapped to a random seed, other values: constant initialization) 53 | * 54 | * \note Reproducable random numbers are often desirable when software 55 | * is tested with random geometric constructions. Thus each seed value 56 | * different from \e 0 leads to its own, reproducible, output sequence. 57 | * In contrast the \p seed value \e 0 is mapped to random initialization 58 | * of the RNG. In this case the RNG will produce a different output 59 | * sequence each time it is called. 60 | */ 61 | CLASS_DECLSPEC 62 | void generateRandomNumbers( size_t num, 63 | double min, 64 | double max, 65 | std::vector& vRandomNumbersOut, 66 | unsigned int seed=0 67 | ); 68 | 69 | /** @brief Generate random points 70 | * 71 | * @param numRandomPoints Number of points to be generated 72 | * @param min Lower bound (x,y) 73 | * @param max Upper bound (x,y) 74 | * @param [out] vRandomPointsOut is the output vector 75 | * @param seed initializes the random number generator RNG 76 | * (default: 0...mapped to a random seed, other values: constant initialization) 77 | * 78 | * 79 | * 80 | * \image html randomPoints.png "Point generator" 81 | * \image latex randomPoints.eps "Point generator" width=15cm 82 | * 83 | */ 84 | CLASS_DECLSPEC 85 | void generateRandomPoints( size_t numRandomPoints, 86 | double min, 87 | double max, 88 | std::vector& vRandomPointsOut, 89 | unsigned int seed=0 90 | ); 91 | 92 | /** @brief Generate a random simple polygon 93 | * 94 | * @param numSegments Number of segments to be generated 95 | * @param min Lower bound (x,y) 96 | * @param max Upper bound (x,y) 97 | * @param [out] vPolygonOut is the output vector 98 | * @param seed initializes the random number generator RNG 99 | * (default: 0...mapped to a random seed, other values: constant initialization) 100 | * 101 | * 102 | * \image html randomPolygon.png "Polygon generator: Random simple polygon" 103 | * \image latex randomPolygon.eps "Polygon generator: Random simple polygon" width=15cm 104 | */ 105 | CLASS_DECLSPEC 106 | void generateRandomPolygon( size_t numSegments, 107 | double min, 108 | double max, 109 | std::vector& vPolygonOut, 110 | unsigned int seed=0 111 | ); 112 | 113 | /** @brief Generate random line segments 114 | * 115 | * @param numSegments Number of segments to be generated 116 | * @param min Lower bound (x,y) 117 | * @param max Upper bound (x,y) 118 | * @param maxLen Maximal segment length 119 | * @param [out] vSegmentsOut is the output vector 120 | * @param seed initializes the random number generator RNG 121 | * (default: 0...mapped to a random seed, other values: constant initialization) 122 | * 123 | * 124 | * 125 | * \image html randomSegments.png "Segment generator: Random line segments" 126 | * \image latex randomSegments.eps "Segment generator: Random line segments" width=15cm 127 | */ 128 | CLASS_DECLSPEC 129 | void generateRandomSegments(size_t numSegments, 130 | double min, 131 | double max, 132 | double maxLen, 133 | std::vector& vSegmentsOut, 134 | unsigned int seed); 135 | 136 | /** @brief Generate segments from a sine function 137 | * 138 | * @param numSegments Number of segments to be generated 139 | * @param numPeriods Number of periods of the sine function 140 | * @param xOffset Offset of the output x-coordinates 141 | * @param yOffset Offset of the output y-coordinates 142 | * @param xFactor Factor to scale the sine function in x direction 143 | * @param yFactor Factor to scale the sine function in y direction 144 | * @param bSwapXY Swap the x and y coordinate of the function 145 | * @param [out] vSineSegmentsOut is the output vector 146 | * 147 | * * \image html sinePolyline.png "Polyline generator: Polylines from sine functions" 148 | * \image latex sinePolyline.eps "Polyline generator: Polylines from sine functions" width=15cm 149 | */ 150 | CLASS_DECLSPEC 151 | void generateSineSegments( 152 | int numSegments, 153 | int numPeriods, 154 | double xOffset, 155 | double yOffset, 156 | double xFactor, 157 | double yFactor, // 0 causes absolute values 158 | bool bSwapXY, 159 | std::vector& vSineSegmentsOut 160 | ); 161 | 162 | 163 | 164 | /** @}*/ 165 | } // Namespace 166 | -------------------------------------------------------------------------------- /third_party/fade_v1.46a/lib_ubuntu14.04_i686/libfade2d.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VIS4ROB-lab/v4rl_mesh_based_mapping_lib/4c962907e5574a1f6830b57a22b37b5139c110b5/third_party/fade_v1.46a/lib_ubuntu14.04_i686/libfade2d.so -------------------------------------------------------------------------------- /third_party/fade_v1.46a/lib_ubuntu14.04_x86_64/libfade2d.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VIS4ROB-lab/v4rl_mesh_based_mapping_lib/4c962907e5574a1f6830b57a22b37b5139c110b5/third_party/fade_v1.46a/lib_ubuntu14.04_x86_64/libfade2d.so -------------------------------------------------------------------------------- /third_party/fade_v1.46a/lib_ubuntu16.10_x86_64/libfade2d.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VIS4ROB-lab/v4rl_mesh_based_mapping_lib/4c962907e5574a1f6830b57a22b37b5139c110b5/third_party/fade_v1.46a/lib_ubuntu16.10_x86_64/libfade2d.so -------------------------------------------------------------------------------- /third_party/fade_v1.46a/readme.txt: -------------------------------------------------------------------------------- 1 | Fade2D, v1.46a, Januray 14th, 2016 2 | ---------------------------------- 3 | 4 | Note: This Fade version is the same as v1.46 but support for Raspberry PI 5 | has been added. Raspberry PI users: Please give feedback on RPI support, 6 | do you have everything you need now for this platform? 7 | 8 | 9 | 10 | Welcome to the Fade2D Library! 11 | 12 | Any feedback is appreciated: 13 | 14 | Geom Software 15 | Bernhard Kornberger 16 | C++ Freelancer 17 | bkorn@geom.at 18 | 19 | 20 | The present download contains all library versions needed for development 21 | under 22 | .) Windows (VS2008, VS2010, VS2012, VS2013, VS2015) 23 | .) Linux (gcc) 24 | .) Apple (clang) 25 | 26 | 27 | Getting started 28 | =============== 29 | 30 | Linux and Apple users: 31 | ---------------------- 32 | 33 | cd examples_2D (...or examples_25D) 34 | Edit the Makefile -> uncomment the DISTRO string that matches your system (Apple, Ubuntu, Fedora, ...) 35 | make 36 | ./allExamples_2D (...or ./examples_25D) 37 | 38 | 39 | Windows users: 40 | -------------- 41 | 42 | Open the solution file 43 | 44 | examples_2D/vs20xx_exampleProject/examples_2D.sln ...or 45 | examples_25D/vs20xx_exampleProject/examples_25D.sln 46 | 47 | ...and compile. Then open a command line window (WIN+r, cmd.exe), 48 | change to fadeRelease/x64 (or fadeRelease/Win32) and run the 49 | executable there. 50 | 51 | 52 | For more information please visit http://www.geom.at/fade2d/html 53 | 54 | 55 | 56 | 57 | Directories 58 | =========== 59 | 60 | .) include_fade2d and include_fade25d 61 | Header files of the two libraries. 62 | 63 | .) Win32 (x64) 64 | This directory contains the *.dll and *.lib files for Windows 32-bit (64-bit) 65 | and it is the target directory for the executables compiled with Visual Studio. 66 | 67 | .) lib_${DISTRO}_${ARCHITECTURE} 68 | The shared libraries (*.so) for Linux developers. They work for a wide range 69 | of Linux distributions. Commercial users who need support for a certain Linux 70 | distribution, please get in contact with the author. 71 | 72 | .) examples_2D 73 | Source code of all examples using Fade2D 74 | 75 | .) examples_25D 76 | Source code of all examples using Fade2.5D 77 | 78 | .) doc 79 | Documentation 80 | 81 | 82 | Student license 83 | =============== 84 | 85 | Fade is free of charge for personal non-commercial scientific research. But we 86 | ask you to put a link to Fade on your research homepage or project page and to 87 | cite Fade2D in scientific publications using it. Fade is not free software. Do 88 | not integrate it in a free software project without explicit permission. 89 | 90 | Limits of the student version: 91 | 2D points: 1 million 92 | 2.5D points: 50000 93 | Meshing: 50000 output triangles 94 | 95 | 96 | Commercial license 97 | ================== 98 | 99 | All other applications, including commercial in-house usage, require a commercial 100 | license which has the advantage of maintenance, error corrections and personal 101 | support. The commercial license of Fade cosists of a base component and three 102 | optional components: 103 | 104 | * The Fade2D base component (mandatory): It computes 2D Delaunay triangulations 105 | and constrained Delaunay triangulations, i.e. it supports insertion of segments. 106 | * The meshing generator component (optional): Creates high quality triangles 107 | inside a specified area. The methods refine() and refineAdvanced() refer to 108 | this component. 109 | * The Fade2.5D extension (optional): For terrains and other height fields: 110 | Points have (x,y,z)-coordinates, ISO lines (contours in a terrain having the 111 | same height) can be computed and the height values of arbitrary (x,y) pairs 112 | can be queried quickly. The above mentioned meshing extension can take 113 | advantage of the height values when a height field shall be filled with 114 | high quality triangles. 115 | * The SegmentChecker component with a fast underlying data structure takes 116 | a bunch of line segments and identifies intersecting ones. Visualization 117 | of problem segments and computation of intersection points is supported. 118 | 119 | Author: Geom e.U., Graz / Austria 120 | Bernhard Kornberger, bkorn@geom.at 121 | C++-Freelancer, http://www.geom.at 122 | 123 | 124 | 125 | 126 | In no case can Geom Graz/Austria be made responsible for damages of any kind that 127 | arise in connection with the use or non-usability of the software or information 128 | provided on our internet pages. If you can't accept these terms, you are not 129 | allowed to use our software. Using Fade for military research and applications 130 | is not accepted. 131 | 132 | 133 | 3rd party libraries 134 | ------------------- 135 | 136 | The Fade 2D library uses the GNU Multiple Precision Arithmetic Library (see 137 | http://gmplib.org/ for details and the source code), released under the GNU 138 | Lesser General Public License (https://www.gnu.org/copyleft/lesser.html). 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/angle-axis.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_ANGLE_AXIS_H_ 26 | #define KINDR_MIN_ROTATION_ANGLE_AXIS_H_ 27 | 28 | #include 29 | 30 | namespace kindr { 31 | namespace minimal { 32 | 33 | template 34 | class RotationQuaternionTemplate; 35 | 36 | /// \class AngleAxis 37 | /// \brief a minimal implementation of an angle and axis representation of 38 | /// rotation 39 | /// This rotation takes vectors from frame B to frame A, written 40 | /// as \f${}_{A}\mathbf{v} = \mathbf{C}_{AB} {}_{B}\mathbf{v}\f$ 41 | /// 42 | /// The angle is assumed to be in radians everywhere 43 | /// 44 | /// In code, we write: 45 | /// 46 | /// \code{.cpp} 47 | /// A_v = C_A_B.rotate(B_v); 48 | /// \endcode 49 | /// 50 | template 51 | class AngleAxisTemplate { 52 | public: 53 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 54 | 55 | typedef Eigen::Matrix Vector3; 56 | 57 | typedef Eigen::Matrix Vector4; 58 | 59 | typedef Eigen::AngleAxis Implementation; 60 | 61 | typedef Eigen::Matrix RotationMatrix; 62 | 63 | /// \brief initialize to identity. 64 | AngleAxisTemplate(); 65 | 66 | /// \brief initialize from the angle and rotation axis (angle first). 67 | AngleAxisTemplate(Scalar angle, Scalar v1, Scalar v2, Scalar v3); 68 | 69 | /// \brief initialize from the angle and rotation axis. 70 | AngleAxisTemplate(Scalar angle, const Vector3& axis); 71 | 72 | /// \brief initialize from an Eigen angleAxis. 73 | AngleAxisTemplate(const Implementation& angleAxis); 74 | 75 | /// \brief initialize from a rotation matrix. 76 | AngleAxisTemplate(const RotationMatrix& matrix); 77 | 78 | /// \brief initialize from an Eigen quaternion. 79 | AngleAxisTemplate(const RotationQuaternionTemplate& quat); 80 | 81 | /// \brief initialize from a angle-scaled axis vector. 82 | AngleAxisTemplate(const Vector3& angleAxis); 83 | 84 | ~AngleAxisTemplate(); 85 | 86 | /// \brief Returns the rotation angle. 87 | Scalar angle() const; 88 | 89 | /// \brief Sets the rotation angle. 90 | void setAngle(Scalar angle); 91 | 92 | /// \brief Returns the rotation axis. 93 | const Vector3& axis() const; 94 | 95 | /// \brief Sets the rotation axis. 96 | void setAxis(const Vector3& axis); 97 | 98 | /// \brief Sets the rotation axis. 99 | void setAxis(Scalar v1, Scalar v2, Scalar v3); 100 | 101 | /// \brief get the components of the angle/axis as a vector (angle first). 102 | Vector4 vector() const; 103 | 104 | /// \brief get a copy of the representation that is unique. 105 | AngleAxisTemplate getUnique() const; 106 | 107 | /// \brief set the angle/axis to its unique representation. 108 | AngleAxisTemplate& setUnique(); 109 | 110 | /// \brief set the rotation to identity. 111 | AngleAxisTemplate& setIdentity(); 112 | 113 | /// \brief get a copy of the rotation inverted. 114 | AngleAxisTemplate inverse() const; 115 | 116 | /// \deprecated use inverse() instead. 117 | AngleAxisTemplate inverted() const __attribute__((deprecated)); 118 | 119 | /// \brief rotate a vector, v. 120 | Vector3 rotate(const Vector3& v) const; 121 | 122 | /// \brief rotate a vector, v. 123 | Vector4 rotate4(const Vector4& v) const; 124 | 125 | /// \brief rotate a vector, v. 126 | Vector3 inverseRotate(const Vector3& v) const; 127 | 128 | /// \brief rotate a vector, v. 129 | Vector4 inverseRotate4(const Vector4& v) const; 130 | 131 | /// \brief cast to the implementation type. 132 | Implementation& toImplementation(); 133 | 134 | /// \brief cast to the implementation type. 135 | const Implementation& toImplementation() const; 136 | 137 | /// \brief get the angle between this and the other rotation. 138 | Scalar getDisparityAngle(const AngleAxisTemplate& rhs) const; 139 | 140 | /// \brief enforce the unit length constraint. 141 | AngleAxisTemplate& normalize(); 142 | 143 | /// \brief compose two rotations. 144 | AngleAxisTemplate operator*( 145 | const AngleAxisTemplate& rhs) const; 146 | 147 | /// \brief assignment operator. 148 | AngleAxisTemplate& operator=(const AngleAxisTemplate& rhs); 149 | 150 | /// \brief get the rotation matrix. 151 | RotationMatrix getRotationMatrix() const; 152 | 153 | private: 154 | Implementation C_A_B_; 155 | }; 156 | 157 | typedef AngleAxisTemplate AngleAxis; 158 | 159 | template 160 | std::ostream& operator<<(std::ostream& out, 161 | const AngleAxisTemplate& rhs); 162 | 163 | } // namespace minimal 164 | } // namespace kindr 165 | 166 | #include 167 | 168 | #endif /* KINDR_MIN_ROTATION_ANGLE_AXIS_HPP */ 169 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/common.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef MINKINDR_MINIMAL_COMMON_H 26 | #define MINKINDR_MINIMAL_COMMON_H 27 | 28 | #include 29 | #include 30 | 31 | namespace kindr { 32 | namespace minimal { 33 | 34 | inline void skewMatrix(const Eigen::Vector3d& v, Eigen::Matrix3d * skew) { 35 | CHECK_NOTNULL(skew); 36 | skew->setZero(); 37 | (*skew)(0,1) = -v[2]; 38 | (*skew)(1,0) = v[2]; 39 | (*skew)(0,2) = v[1]; 40 | (*skew)(2,0) = -v[1]; 41 | (*skew)(1,2) = -v[0]; 42 | (*skew)(2,1) = v[0]; 43 | } 44 | 45 | inline void skewMatrix(const Eigen::Vector3d& v, Eigen::Map * skew) { 46 | CHECK_NOTNULL(skew); 47 | skew->setZero(); 48 | (*skew)(0,1) = -v[2]; 49 | (*skew)(1,0) = v[2]; 50 | (*skew)(0,2) = v[1]; 51 | (*skew)(2,0) = -v[1]; 52 | (*skew)(1,2) = -v[0]; 53 | (*skew)(2,1) = v[0]; 54 | } 55 | 56 | inline Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& v) { 57 | Eigen::Matrix3d skew; 58 | skewMatrix(v, &skew); 59 | return skew; 60 | } 61 | 62 | } // namespace minimal 63 | } // namespace kindr 64 | 65 | #endif // MINKINDR_MINIMAL_COMMON_H 66 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/implementation/angle-axis-inl.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_ANGLE_AXIS_INL_H_ 26 | #define KINDR_MIN_ROTATION_ANGLE_AXIS_INL_H_ 27 | #include 28 | #include 29 | #include 30 | 31 | namespace kindr { 32 | namespace minimal { 33 | 34 | template 35 | AngleAxisTemplate::AngleAxisTemplate() : 36 | C_A_B_(Implementation::Identity()) { 37 | } 38 | 39 | template 40 | AngleAxisTemplate::AngleAxisTemplate( 41 | Scalar w, Scalar x, Scalar y, Scalar z) : C_A_B_(w, Vector3(x,y,z)) { 42 | CHECK_NEAR(Vector3(x,y,z).squaredNorm(), static_cast(1.0), 43 | static_cast(1e-4)); 44 | } 45 | 46 | template 47 | AngleAxisTemplate::AngleAxisTemplate( 48 | Scalar angle, const typename AngleAxisTemplate::Vector3& axis) : 49 | C_A_B_(angle, axis){ 50 | CHECK_NEAR(axis.squaredNorm(), static_cast(1.0), 51 | static_cast(1e-4)); 52 | } 53 | 54 | template 55 | AngleAxisTemplate::AngleAxisTemplate(const Implementation& angleAxis) : 56 | C_A_B_(angleAxis) { 57 | } 58 | 59 | template 60 | AngleAxisTemplate::AngleAxisTemplate(const RotationMatrix& matrix) : 61 | C_A_B_(matrix) { 62 | // \todo furgalep Check that the matrix was good... 63 | } 64 | 65 | template 66 | AngleAxisTemplate::AngleAxisTemplate(const Vector3& angleAxis) { 67 | Scalar norm = angleAxis.norm(); 68 | CHECK_GT(norm, 1e-8); 69 | Vector3 axis = angleAxis; 70 | axis = axis / norm; 71 | 72 | C_A_B_ = Implementation(norm, axis); 73 | } 74 | 75 | template 76 | AngleAxisTemplate::AngleAxisTemplate( 77 | const RotationQuaternionTemplate& quat) : 78 | C_A_B_(quat.toImplementation()) { 79 | } 80 | 81 | template 82 | AngleAxisTemplate::~AngleAxisTemplate() { } 83 | 84 | template 85 | AngleAxisTemplate& AngleAxisTemplate::operator=( 86 | const AngleAxisTemplate& rhs) { 87 | if(this != &rhs) { 88 | C_A_B_ = rhs.C_A_B_; 89 | } 90 | return *this; 91 | } 92 | 93 | template 94 | Scalar AngleAxisTemplate::angle() const{ 95 | return C_A_B_.angle(); 96 | } 97 | 98 | template 99 | void AngleAxisTemplate::setAngle(Scalar angle){ 100 | C_A_B_.angle() = angle; 101 | } 102 | 103 | template 104 | const typename AngleAxisTemplate::Vector3& 105 | AngleAxisTemplate::axis() const{ 106 | return C_A_B_.axis(); 107 | } 108 | 109 | template 110 | void AngleAxisTemplate::setAxis(const Vector3& axis){ 111 | CHECK_NEAR(axis.squaredNorm(), static_cast(1.0), 112 | static_cast(1e-4)); 113 | C_A_B_.axis() = axis; 114 | } 115 | 116 | template 117 | void AngleAxisTemplate::setAxis(Scalar v1, Scalar v2, Scalar v3){ 118 | C_A_B_.axis() = Vector3(v1,v2,v3); 119 | CHECK_NEAR(C_A_B_.axis().squaredNorm(), static_cast(1.0), 120 | static_cast(1e-4)); 121 | } 122 | 123 | template 124 | typename AngleAxisTemplate::Vector4 125 | AngleAxisTemplate::vector() const{ 126 | Vector4 vector; 127 | vector(0) = angle(); 128 | vector.template tail<3>() = C_A_B_.axis(); 129 | return vector; 130 | } 131 | 132 | template 133 | AngleAxisTemplate AngleAxisTemplate::getUnique() const { 134 | // first wraps angle into [-pi,pi) 135 | AngleAxisTemplate aa(fmod(angle()+M_PI, 2*M_PI)-M_PI, C_A_B_.axis()); 136 | if(aa.angle() > 0) { 137 | return aa; 138 | } else if(aa.angle() < 0) { 139 | if(aa.angle() != -M_PI) { 140 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 141 | } else { // angle == -pi, so axis must be viewed further, because -pi,axis 142 | // does the same as -pi,-axis. 143 | if(aa.axis()[0] < 0) { 144 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 145 | } else if(aa.axis()[0] > 0) { 146 | return AngleAxisTemplate(-aa.angle(), aa.axis()); 147 | } else { // v1 == 0 148 | if(aa.axis()[1] < 0) { 149 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 150 | } else if(aa.axis()[1] > 0) { 151 | return AngleAxisTemplate(-aa.angle(), aa.axis()); 152 | } else { // v2 == 0 153 | if(aa.axis()[2] < 0) { // v3 must be -1 or 1 154 | return AngleAxisTemplate(-aa.angle(), -aa.axis()); 155 | } else { 156 | return AngleAxisTemplate(-aa.angle(), aa.axis()); 157 | } 158 | } 159 | } 160 | } 161 | } else { // angle == 0 162 | return AngleAxisTemplate(); 163 | } 164 | } 165 | 166 | template 167 | AngleAxisTemplate& AngleAxisTemplate::setUnique() { 168 | *this = getUnique(); 169 | return *this; 170 | } 171 | 172 | template 173 | AngleAxisTemplate& AngleAxisTemplate::setIdentity() { 174 | C_A_B_ = C_A_B_.Identity(); 175 | return *this; 176 | } 177 | 178 | template 179 | AngleAxisTemplate AngleAxisTemplate::inverse() const { 180 | return AngleAxisTemplate(C_A_B_.inverse()); 181 | } 182 | 183 | template 184 | AngleAxisTemplate AngleAxisTemplate::inverted() const { 185 | return inverse(); 186 | } 187 | 188 | template 189 | typename AngleAxisTemplate::Vector3 AngleAxisTemplate::rotate( 190 | const AngleAxisTemplate::Vector3& v) const { 191 | return C_A_B_*v; 192 | } 193 | 194 | template 195 | typename AngleAxisTemplate::Vector4 196 | AngleAxisTemplate::rotate4( 197 | const AngleAxisTemplate::Vector4& v) const { 198 | AngleAxisTemplate::Vector4 vprime; 199 | vprime[3] = v[3]; 200 | vprime.template head<3>() = C_A_B_ * v.template head<3>(); 201 | return vprime; 202 | } 203 | 204 | template 205 | typename AngleAxisTemplate::Vector3 206 | AngleAxisTemplate::inverseRotate( 207 | const AngleAxisTemplate::Vector3& v) const { 208 | return C_A_B_.inverse() * v; 209 | } 210 | 211 | template 212 | typename AngleAxisTemplate::Vector4 213 | AngleAxisTemplate::inverseRotate4( 214 | const typename AngleAxisTemplate::Vector4& v) const { 215 | Eigen::Vector4d vprime; 216 | vprime[3] = v[3]; 217 | vprime.template head<3>() = C_A_B_.inverse() * v.template head<3>(); 218 | return vprime; 219 | } 220 | 221 | template 222 | typename AngleAxisTemplate::Implementation& 223 | AngleAxisTemplate::toImplementation() { 224 | return C_A_B_; 225 | } 226 | 227 | template 228 | const typename AngleAxisTemplate::Implementation& 229 | AngleAxisTemplate::toImplementation() const { 230 | return C_A_B_; 231 | } 232 | 233 | template 234 | AngleAxisTemplate& AngleAxisTemplate::normalize() { 235 | C_A_B_.axis().normalize(); 236 | return *this; 237 | } 238 | 239 | template 240 | AngleAxisTemplate AngleAxisTemplate::operator*( 241 | const AngleAxisTemplate& rhs) const { 242 | return AngleAxisTemplate(Implementation(C_A_B_ * rhs.C_A_B_)); 243 | } 244 | 245 | template 246 | Scalar AngleAxisTemplate::getDisparityAngle( 247 | const AngleAxisTemplate& rhs) const { 248 | return (rhs * this->inverted()).getUnique().angle(); 249 | } 250 | 251 | template 252 | std::ostream& operator<<(std::ostream& out, 253 | const AngleAxisTemplate& rhs) { 254 | out << rhs.vector().transpose(); 255 | return out; 256 | } 257 | 258 | template 259 | typename AngleAxisTemplate::RotationMatrix 260 | AngleAxisTemplate::getRotationMatrix() const { 261 | return C_A_B_.matrix(); 262 | } 263 | 264 | } // namespace minimal 265 | } // namespace kindr 266 | #endif // KINDR_MIN_ROTATION_ANGLE_AXIS_INL_H_ 267 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/implementation/quat-sim-transform-inl.h: -------------------------------------------------------------------------------- 1 | #ifndef KINDR_MINIMAL_IMPLEMENTATION_QUAT_SIM_TRANSFORM_INL_H_ 2 | #define KINDR_MINIMAL_IMPLEMENTATION_QUAT_SIM_TRANSFORM_INL_H_ 3 | 4 | #include 5 | 6 | namespace kindr { 7 | namespace minimal { 8 | 9 | template 10 | QuatSimTransformTemplate::QuatSimTransformTemplate() : scale_A_B_(1.) {} 11 | 12 | template 13 | QuatSimTransformTemplate::QuatSimTransformTemplate( 14 | const Transform& T_A_B, const Scalar scale_A_B) 15 | : T_A_B_(T_A_B), scale_A_B_(scale_A_B) { 16 | CHECK_GT(scale_A_B, 0.); 17 | } 18 | 19 | template 20 | QuatSimTransformTemplate::QuatSimTransformTemplate( 21 | const Vector7& log_vector) 22 | : T_A_B_(Eigen::Matrix(log_vector.template head<6>())), 23 | scale_A_B_(log_vector(6)) { 24 | CHECK_GT(scale_A_B_, 0.); 25 | } 26 | 27 | template 28 | typename QuatSimTransformTemplate::Vector3 29 | QuatSimTransformTemplate::operator*(const Vector3& rhs) const { 30 | return T_A_B_ * (scale_A_B_ * rhs); 31 | } 32 | 33 | template 34 | typename QuatSimTransformTemplate::Matrix3X 35 | QuatSimTransformTemplate::operator*(const Matrix3X& rhs) const { 36 | return T_A_B_.transformVectorized(scale_A_B_ * rhs); 37 | } 38 | 39 | template 40 | QuatSimTransformTemplate QuatSimTransformTemplate::operator*( 41 | const QuatSimTransformTemplate& rhs) const { 42 | // Rl*sl tl Rr*sr tr Rl*sl*Rr*sr Rl*sl*tr+tl 43 | // 0 1 * 0 1 = 0 1 44 | // 45 | // -> R = Rl*Rr; t = Rl*sl*tr+tl = Siml*tr; s = sl*sr 46 | return QuatSimTransformTemplate( 47 | QuatTransformationTemplate( 48 | T_A_B_.getRotation() * rhs.T_A_B_.getRotation(), 49 | (*this) * rhs.T_A_B_.getPosition()), 50 | scale_A_B_ * rhs.scale_A_B_); 51 | } 52 | 53 | template 54 | QuatSimTransformTemplate QuatSimTransformTemplate::operator*( 55 | const QuatTransformationTemplate& rhs) const { 56 | return operator*(QuatSimTransformTemplate(rhs, 1.)); 57 | } 58 | 59 | template 60 | QuatSimTransformTemplate 61 | QuatSimTransformTemplate::inverse() const { 62 | return QuatSimTransformTemplate( 63 | QuatTransformationTemplate( 64 | T_A_B_.getRotation().inverse(), -T_A_B_.getRotation().inverseRotate( 65 | T_A_B_.getPosition() / scale_A_B_)), 1. / scale_A_B_); 66 | } 67 | 68 | template 69 | inline Eigen::Matrix 70 | QuatSimTransformTemplate::log() const { 71 | Eigen::Matrix result; 72 | result << T_A_B_.log(), scale_A_B_; 73 | return result; 74 | } 75 | 76 | template 77 | Eigen::Matrix 78 | QuatSimTransformTemplate::getTransformationMatrix() const { 79 | Eigen::Matrix result = T_A_B_.getTransformationMatrix(); 80 | result.template topLeftCorner<3, 3>() *= scale_A_B_; 81 | return result; 82 | } 83 | 84 | template 85 | inline QuatSimTransformTemplate operator*( 86 | const QuatTransformationTemplate& lhs, 87 | const QuatSimTransformTemplate& rhs) { 88 | return QuatSimTransformTemplate(lhs, 1.) * rhs; 89 | } 90 | 91 | template 92 | std::ostream & operator<<(std::ostream & out, 93 | const QuatSimTransformTemplate& sim_3) { 94 | out << "Transform:" << std::endl << sim_3.T_A_B_.getTransformationMatrix() << 95 | std::endl; 96 | out << "Scale: " << sim_3.scale_A_B_; 97 | return out; 98 | } 99 | 100 | } // namespace minimal 101 | } // namespace kindr 102 | 103 | #endif // KINDR_MINIMAL_IMPLEMENTATION_QUAT_SIM_TRANSFORM_INL_H_ 104 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/position.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MINIMAL_POSITION_H 26 | #define KINDR_MINIMAL_POSITION_H 27 | 28 | #include 29 | 30 | namespace kindr { 31 | namespace minimal { 32 | 33 | template 34 | using PositionTemplate = Eigen::Matrix; 35 | 36 | typedef PositionTemplate Position; 37 | 38 | } // namespace minimal 39 | } // namespace kindr 40 | 41 | #endif /* KINDR_MINIMAL_POSITION_H */ 42 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/quat-sim-transform.h: -------------------------------------------------------------------------------- 1 | #ifndef KINDR_MINIMAL_QUAT_SIM_TRANSFORM_H_ 2 | #define KINDR_MINIMAL_QUAT_SIM_TRANSFORM_H_ 3 | 4 | #include 5 | 6 | #include "kindr/minimal/quat-transformation.h" 7 | 8 | namespace kindr { 9 | namespace minimal { 10 | 11 | // Scale & rotate then translate = scale then transform. 12 | // In particular, the transformation matrix is: 13 | // 14 | // R*s t R t s*I 0 15 | // 0 1 = 0 1 * 0 1 16 | template 17 | class QuatSimTransformTemplate { 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | typedef QuatTransformationTemplate Transform; 22 | typedef QuatSimTransformTemplate Sim3; 23 | typedef Eigen::Matrix Vector3; 24 | typedef Eigen::Matrix Vector7; 25 | typedef Eigen::Matrix Matrix3X; 26 | 27 | // Creates identity similarity transform. 28 | QuatSimTransformTemplate(); 29 | 30 | QuatSimTransformTemplate(const Transform& T_A_B, const Scalar scale_A_B); 31 | 32 | QuatSimTransformTemplate(const Vector7& log_vector); 33 | 34 | inline Vector3 operator*(const Vector3& rhs) const; 35 | 36 | // Vectorized, applies operator * to each column vector. 37 | inline Matrix3X operator*(const Matrix3X& rhs) const; 38 | 39 | inline Sim3 operator*(const Sim3& rhs) const; 40 | 41 | inline Sim3 operator*(const Transform& rhs) const; 42 | 43 | inline Sim3 inverse() const; 44 | 45 | inline Vector7 log() const; 46 | 47 | inline Eigen::Matrix getTransformationMatrix() const; 48 | inline const Transform& getTransform() const { return T_A_B_; } 49 | inline Scalar getScale() const { return scale_A_B_; } 50 | 51 | inline void setScale(const Scalar scale_A_B) { scale_A_B_ = scale_A_B; } 52 | 53 | private: 54 | Transform T_A_B_; 55 | Scalar scale_A_B_; 56 | 57 | template 58 | friend std::ostream & operator<<( 59 | std::ostream &, const QuatSimTransformTemplate&); 60 | }; 61 | 62 | typedef QuatSimTransformTemplate QuatSimTransform; 63 | 64 | template 65 | inline QuatSimTransformTemplate operator*( 66 | const QuatTransformationTemplate& lhs, 67 | const QuatSimTransformTemplate& rhs); 68 | 69 | template 70 | std::ostream & operator<<(std::ostream & out, 71 | const QuatSimTransformTemplate& sim_3); 72 | 73 | } // namespace minimal 74 | } // namespace kindr 75 | 76 | #include "kindr/minimal/implementation/quat-sim-transform-inl.h" 77 | 78 | #endif // KINDR_MINIMAL_QUAT_SIM_TRANSFORM_H_ 79 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/quat-transformation.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MINIMAL_QUAT_TRANSFORMATION_H_ 26 | #define KINDR_MINIMAL_QUAT_TRANSFORMATION_H_ 27 | 28 | #include 29 | #include 30 | 31 | namespace kindr { 32 | namespace minimal { 33 | 34 | /// \class QuatTransformation 35 | /// \brief A frame transformation built from a quaternion and a point 36 | /// 37 | /// This transformation takes points from frame B to frame A, written 38 | /// as \f${}_{A}\mathbf{p} = \mathbf{T}_{AB} {}_{B}\mathbf{p}\f$ 39 | /// 40 | /// In code, we write: 41 | /// 42 | /// \code{.cpp} 43 | /// A_p = T_A_B.transform(B_p); 44 | /// \endcode 45 | /// 46 | template 47 | class QuatTransformationTemplate { 48 | public: 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | 51 | typedef Eigen::Matrix Vector3; 52 | typedef Eigen::Matrix Vector4; 53 | typedef Eigen::Matrix Vector6; 54 | 55 | typedef Eigen::Matrix Matrix3X; 56 | 57 | typedef PositionTemplate Position; 58 | typedef RotationQuaternionTemplate Rotation; 59 | typedef Eigen::Matrix RotationMatrix; 60 | typedef Eigen::Matrix TransformationMatrix; 61 | 62 | /// \brief Constructor of identity transformation. 63 | QuatTransformationTemplate(); 64 | 65 | explicit QuatTransformationTemplate( 66 | const RotationQuaternionTemplate& q_A_B, const Position& A_t_A_B); 67 | explicit QuatTransformationTemplate( 68 | const typename Rotation::Implementation& q_A_B, const Position& A_t_A_B); 69 | 70 | explicit QuatTransformationTemplate( 71 | const Position& A_t_A_B, const Rotation& q_A_B); 72 | explicit QuatTransformationTemplate( 73 | const Position& A_t_A_B, const typename Rotation::Implementation& q_A_B); 74 | 75 | explicit QuatTransformationTemplate(const TransformationMatrix& T); 76 | 77 | /// \brief a constructor based on the exponential map. 78 | /// translational part in the first 3 dimensions, 79 | /// rotational part in the last 3 dimensions. 80 | QuatTransformationTemplate(const Vector6& x_t_r); 81 | 82 | ~QuatTransformationTemplate(); 83 | 84 | void setIdentity(); 85 | 86 | /// \brief set to random transformation. 87 | QuatTransformationTemplate& setRandom(); 88 | 89 | /// \brief set to random transformation with a given translation norm. 90 | QuatTransformationTemplate& setRandom(Scalar norm_translation); 91 | 92 | /// \brief set to random transformation with a given translation norm and rotation angle. 93 | QuatTransformationTemplate& setRandom(Scalar norm_translation, Scalar angle_rad); 94 | 95 | /// \brief get the position component. 96 | Position& getPosition(); 97 | 98 | /// \brief get the position component. 99 | const Position& getPosition() const; 100 | 101 | /// \brief get the rotation component. 102 | Rotation& getRotation(); 103 | 104 | /// \brief get the rotation component. 105 | const Rotation& getRotation() const; 106 | 107 | /// \brief get the rotation component as an Eigen Quaternion directly. 108 | const Eigen::Quaternion& getEigenQuaternion() const; 109 | 110 | /// \brief get the transformation matrix. 111 | TransformationMatrix getTransformationMatrix() const; 112 | 113 | /// \brief get the rotation matrix. 114 | RotationMatrix getRotationMatrix() const; 115 | 116 | /// \brief get the quaternion of rotation and the position as a vector. 117 | /// [w x y z, x y z] 118 | Eigen::Matrix asVector() const; 119 | 120 | /// \brief compose two transformations. 121 | QuatTransformationTemplate operator*( 122 | const QuatTransformationTemplate& rhs) const; 123 | 124 | /// \brief transform a point. 125 | Vector3 operator*(const Vector3& rhs) const; 126 | 127 | /// \brief transform a point. 128 | Vector3 transform(const Vector3& rhs) const; 129 | 130 | /// \brief transform points. 131 | Matrix3X transformVectorized(const Matrix3X& rhs) const; 132 | 133 | /// \brief transform a point. 134 | Vector4 transform4(const Vector4& rhs) const; 135 | 136 | /// \brief transform a point by the inverse. 137 | Vector3 inverseTransform(const Vector3& rhs) const; 138 | 139 | /// \brief transform a point by the inverse. 140 | Vector4 inverseTransform4(const Vector4& rhs) const; 141 | 142 | /// \brief get the logarithmic map of the transformation 143 | /// note: this is the log map of SO(3)xR(3) and not SE(3) 144 | /// \return vector form of log map with first 3 components the translational 145 | /// part and the last three the rotational part. 146 | Vector6 log() const; 147 | 148 | /// \brief get the exponential map of the parameters, resulting in a valid 149 | /// transformation note: this is the exp map of SO(3)xR(3) and not SE(3) 150 | /// \param[in] vec vector form of log map with first 3 components the translational 151 | /// part and the last three the rotational part. 152 | /// \return The corresponding Transformation. 153 | static QuatTransformationTemplate exp(const Vector6& vec); 154 | 155 | /// \brief get the logarithmic map of the transformation 156 | /// note: this is the log map of SO(3)xR(3) and not SE(3) 157 | /// \return vector form of log map with first 3 components the translational 158 | /// part and the last three the rotational part. 159 | static Vector6 log(const QuatTransformationTemplate& vec); 160 | 161 | /// \brief return a copy of the transformation inverted. 162 | QuatTransformationTemplate inverse() const; 163 | 164 | /// \deprecated use inverse() instead. 165 | QuatTransformationTemplate inverted() const __attribute__((deprecated)); 166 | 167 | /// \brief check for binary equality. 168 | bool operator==(const QuatTransformationTemplate& rhs) const; 169 | 170 | /// \brief Factory to construct a QuatTransformTemplate from a transformation 171 | /// matrix with a near orthonormal rotation matrix. 172 | static QuatTransformationTemplate 173 | constructAndRenormalizeRotation(const TransformationMatrix& T); 174 | 175 | /// \brief cast scalar elements to another type 176 | template 177 | QuatTransformationTemplate cast() const; 178 | 179 | private: 180 | /// The quaternion that takes vectors from B to A. 181 | /// 182 | /// \code{.cpp} 183 | /// A_v = q_A_B_.rotate(B_v); 184 | /// \endcode 185 | Rotation q_A_B_; 186 | /// The vector from the origin of A to the origin of B 187 | /// expressed in A 188 | Position A_t_A_B_; 189 | }; 190 | 191 | typedef QuatTransformationTemplate QuatTransformation; 192 | 193 | template 194 | std::ostream & operator<<(std::ostream & out, 195 | const QuatTransformationTemplate& pose); 196 | 197 | } // namespace minimal 198 | } // namespace kindr 199 | 200 | #include 201 | 202 | #endif // KINDR_MINIMAL_QUAT_TRANSFORMATION_H_ 203 | -------------------------------------------------------------------------------- /third_party/minkindr/include/kindr/minimal/rotation-quaternion.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2015, Autonomous Systems Lab, ETH Zurich 2 | // All rights reserved. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of the nor the 12 | // names of its contributors may be used to endorse or promote products 13 | // derived from this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | // DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | #ifndef KINDR_MIN_ROTATION_QUATERNION_H_ 26 | #define KINDR_MIN_ROTATION_QUATERNION_H_ 27 | 28 | #include 29 | 30 | namespace kindr { 31 | namespace minimal { 32 | 33 | template 34 | class AngleAxisTemplate; 35 | 36 | /// \class RotationQuaternion 37 | /// \brief a minimal implementation of a passive Hamiltonian rotation 38 | /// (unit-length) quaternion 39 | /// 40 | /// This rotation takes vectors from frame B to frame A, written 41 | /// as \f${}_{A}\mathbf{v} = \mathbf{C}_{AB} {}_{B}\mathbf{v}\f$ 42 | /// 43 | /// In code, we write: 44 | /// 45 | /// \code{.cpp} 46 | /// A_v = q_A_B.rotate(B_v); 47 | /// \endcode 48 | /// 49 | template 50 | class RotationQuaternionTemplate { 51 | public: 52 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 53 | 54 | typedef Eigen::Matrix Vector3; 55 | 56 | typedef Vector3 Imaginary; 57 | 58 | typedef Eigen::Matrix Vector4; 59 | 60 | typedef Eigen::Matrix Matrix3X; 61 | 62 | typedef Eigen::Quaternion Implementation; 63 | 64 | typedef Eigen::Matrix RotationMatrix; 65 | 66 | /// \brief initialize to identity. 67 | RotationQuaternionTemplate(); 68 | 69 | /// \brief initialize from angle scaled axis. 70 | explicit RotationQuaternionTemplate(const Vector3& angle_scaled_axis); 71 | 72 | /// \brief initialize from real and imaginary components (real first). 73 | explicit RotationQuaternionTemplate(Scalar w, Scalar x, Scalar y, Scalar z); 74 | 75 | /// \brief initialize from real and imaginary components. 76 | explicit RotationQuaternionTemplate(Scalar real, const Imaginary& imaginary); 77 | 78 | /// \brief initialize from an Eigen quaternion. 79 | explicit RotationQuaternionTemplate(const Implementation& quaternion); 80 | 81 | /// \brief initialize from a rotation matrix. 82 | explicit RotationQuaternionTemplate(const RotationMatrix& matrix); 83 | 84 | /// \brief take an approximate rotation matrix, recover the closest matrix 85 | /// in SO(3) and construct. 86 | static RotationQuaternionTemplate fromApproximateRotationMatrix( 87 | const RotationMatrix& matrix); 88 | 89 | /// \brief initialize from an AngleAxis. 90 | explicit RotationQuaternionTemplate( 91 | const AngleAxisTemplate& angleAxis); 92 | 93 | ~RotationQuaternionTemplate(); 94 | 95 | /// \brief the real component of the quaternion. 96 | Scalar w() const; 97 | /// \brief the first imaginary component of the quaternion. 98 | Scalar x() const; 99 | /// \brief the second imaginary component of the quaternion. 100 | Scalar y() const; 101 | /// \brief the third imaginary component of the quaternion. 102 | Scalar z() const; 103 | 104 | /// \brief the imaginary components of the quaterion. 105 | Imaginary imaginary() const; 106 | 107 | /// \brief get the components of the quaternion as a vector (real first). 108 | Vector4 vector() const; 109 | 110 | /// \brief set the quaternion by its values (real, imaginary). 111 | void setValues(Scalar w, Scalar x, Scalar y, Scalar z); 112 | 113 | /// \brief set the quaternion by its real and imaginary parts. 114 | void setParts(Scalar real, const Imaginary& imag); 115 | 116 | /// \brief get a copy of the representation that is unique. 117 | RotationQuaternionTemplate getUnique() const; 118 | 119 | /// \brief set the quaternion to its unique representation. 120 | RotationQuaternionTemplate& setUnique(); 121 | 122 | /// \brief set the quaternion to identity. 123 | RotationQuaternionTemplate& setIdentity(); 124 | 125 | /// \brief set to random rotation. 126 | RotationQuaternionTemplate& setRandom(); 127 | 128 | /// \brief set to random rotation with a given angle. 129 | RotationQuaternionTemplate& setRandom(Scalar angle_rad); 130 | 131 | /// \brief get a copy of the quaternion inverted. 132 | RotationQuaternionTemplate inverse() const; 133 | 134 | /// \deprecated use inverse instead. 135 | RotationQuaternionTemplate inverted() const __attribute__((deprecated)); 136 | 137 | /// \brief get a copy of the conjugate of the quaternion. 138 | RotationQuaternionTemplate conjugated() const; 139 | 140 | /// \brief rotate a vector, v. 141 | Vector3 rotate(const Vector3& v) const; 142 | 143 | /// \brief rotate vectors v. 144 | Matrix3X rotateVectorized(const Matrix3X& v) const; 145 | 146 | /// \brief rotate a vector, v. 147 | Vector4 rotate4(const Vector4& v) const; 148 | 149 | /// \brief rotate a vector, v. 150 | Vector3 inverseRotate(const Vector3& v) const; 151 | 152 | /// \brief rotate a vector, v. 153 | Vector4 inverseRotate4(const Vector4& v) const; 154 | 155 | /// \brief cast to the implementation type. 156 | Implementation& toImplementation(); 157 | 158 | /// \brief cast to the implementation type. 159 | const Implementation& toImplementation() const; 160 | 161 | /// \brief get the norm of the quaternion. 162 | Scalar norm() const; 163 | 164 | /// \brief get the squared norm of the quaternion. 165 | Scalar squaredNorm() const; 166 | 167 | /// \brief get the angle [rad] between this and the other quaternion. 168 | Scalar getDisparityAngle(const RotationQuaternionTemplate& rhs) const; 169 | 170 | /// \brief get the angle [rad] between this and the angle axis. 171 | Scalar getDisparityAngle(const AngleAxisTemplate& rhs) const; 172 | 173 | /// \brief enforce the unit length constraint. 174 | RotationQuaternionTemplate& normalize(); 175 | 176 | /// \brief compose two quaternions. 177 | RotationQuaternionTemplate operator*( 178 | const RotationQuaternionTemplate& rhs) const; 179 | 180 | /// \brief compose quaternion and angle axis. 181 | RotationQuaternionTemplate operator*( 182 | const AngleAxisTemplate& rhs) const; 183 | 184 | /// \brief assignment operator. 185 | RotationQuaternionTemplate& operator=( 186 | const RotationQuaternionTemplate& rhs); 187 | 188 | /// \brief get the rotation matrix. 189 | RotationMatrix getRotationMatrix() const; 190 | 191 | /// \brief check for binary equality. 192 | bool operator==(const RotationQuaternionTemplate& rhs) const { 193 | return vector() == rhs.vector(); 194 | } 195 | 196 | // Compute the matrix log of the quaternion. 197 | static Vector3 log(const RotationQuaternionTemplate& q); 198 | 199 | // Compute the matrix exponential of the quaternion. 200 | static RotationQuaternionTemplate exp(const Vector3& dx); 201 | 202 | Vector3 log() const; 203 | 204 | /// \brief Check the validity of a rotation matrix. 205 | static bool isValidRotationMatrix(const RotationMatrix& matrix); 206 | static bool isValidRotationMatrix(const RotationMatrix& matrix, 207 | const Scalar threshold); 208 | 209 | /// \brief Factory to construct a RotationQuaternionTemplate from a near 210 | /// orthonormal rotation matrix. 211 | inline static RotationQuaternionTemplate constructAndRenormalize( 212 | const RotationMatrix& R) { 213 | return RotationQuaternionTemplate(Implementation(R).normalized()); 214 | } 215 | 216 | /// \brief cast scalar elements to another type 217 | template 218 | RotationQuaternionTemplate cast() const; 219 | 220 | private: 221 | void normalizationHelper(Implementation* quaternion) const; 222 | 223 | Implementation q_A_B_; 224 | }; 225 | 226 | typedef RotationQuaternionTemplate RotationQuaternion; 227 | 228 | template 229 | std::ostream& operator<<(std::ostream& out, 230 | const RotationQuaternionTemplate& rhs); 231 | 232 | } // namespace minimal 233 | } // namespace kindr 234 | 235 | #include 236 | 237 | #endif // KINDR_MIN_ROTATION_QUATERNION_H_ 238 | -------------------------------------------------------------------------------- /third_party/okvis_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | project(okvis_kinematics) 4 | 5 | # require Eigen3 6 | find_package( Eigen REQUIRED ) 7 | include_directories(${EIGEN_INCLUDE_DIR}) 8 | 9 | # build the library 10 | add_library(${PROJECT_NAME} STATIC src/dependency-tracker.cc) 11 | 12 | # and link it 13 | target_link_libraries(${PROJECT_NAME} 14 | PUBLIC okvis_util 15 | ) 16 | 17 | # installation if required 18 | install(TARGETS ${PROJECT_NAME} 19 | EXPORT okvisTargets 20 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 21 | ) 22 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 23 | 24 | # testing 25 | if(BUILD_TESTS) 26 | if(APPLE) 27 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 28 | else() 29 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 30 | endif(APPLE) 31 | enable_testing() 32 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 33 | add_executable(${PROJECT_TEST_NAME} 34 | test/runTests.cpp 35 | test/TestTransformation.cpp 36 | ) 37 | target_link_libraries(${PROJECT_TEST_NAME} 38 | ${PROJECT_NAME} 39 | ${GTEST_LIBRARY} 40 | pthread) 41 | add_test(test ${PROJECT_TEST_NAME}) 42 | endif() 43 | -------------------------------------------------------------------------------- /third_party/okvis_kinematics/include/okvis/kinematics/FrameTypedefs.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 27, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | * Modified: Andreas Forster (an.forster@gmail.com) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file FrameTypedefs.hpp 36 | * @brief This file contains useful typedefs and structs related to frames. 37 | * @author Stefan Leutenegger 38 | * @author Andreas Forster 39 | */ 40 | 41 | #ifndef INCLUDE_OKVIS_FRAMETYPEDEFS_HPP_ 42 | #define INCLUDE_OKVIS_FRAMETYPEDEFS_HPP_ 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | 49 | /// \brief okvis Main namespace of this package. 50 | namespace okvis { 51 | 52 | /** 53 | * \brief Unique identifier for a keypoint. 54 | * 55 | * A keypoint is identified as the keypoint with index \e keypointIndex 56 | * in the frame with index \e cameraIndex of multiframe with ID \e frameID. 57 | */ 58 | struct KeypointIdentifier 59 | { 60 | /** 61 | * @brief Constructor. 62 | * @param fi Multiframe ID. 63 | * @param ci Camera index. 64 | * @param ki Keypoint index. 65 | */ 66 | KeypointIdentifier(uint64_t fi = 0, size_t ci = 0, size_t ki = 0) 67 | : frameId(fi), 68 | cameraIndex(ci), 69 | keypointIndex(ki) 70 | { 71 | } 72 | 73 | uint64_t frameId; ///< Multiframe ID. 74 | size_t cameraIndex; ///< Camera index. 75 | size_t keypointIndex; ///< Index of the keypoint 76 | 77 | /// \brief Get multiframe ID. 78 | uint64_t getFrameId() 79 | { 80 | return frameId; 81 | } 82 | /// \brief Set multiframe ID. 83 | void setFrameId(uint64_t fid) 84 | { 85 | frameId = fid; 86 | } 87 | /// \brief Are two identifiers identical? 88 | bool isBinaryEqual(const KeypointIdentifier & rhs) const 89 | { 90 | return frameId == rhs.frameId && cameraIndex == rhs.cameraIndex 91 | && keypointIndex == rhs.keypointIndex; 92 | } 93 | /// \brief Equal to operator. 94 | bool operator==(const KeypointIdentifier & rhs) const 95 | { 96 | return isBinaryEqual(rhs); 97 | } 98 | /// \brief Less than operator. Compares first multiframe ID, then camera index, 99 | /// then keypoint index. 100 | bool operator<(const KeypointIdentifier & rhs) const 101 | { 102 | 103 | if (frameId == rhs.frameId) { 104 | if (cameraIndex == rhs.cameraIndex) { 105 | return keypointIndex < rhs.keypointIndex; 106 | } else { 107 | return cameraIndex < rhs.cameraIndex; 108 | } 109 | } 110 | return frameId < rhs.frameId; 111 | } 112 | 113 | }; 114 | 115 | /// \brief Type to store the result of matching. 116 | struct Match 117 | { 118 | /** 119 | * @brief Constructor. 120 | * @param idxA_ Keypoint index of frame A. 121 | * @param idxB_ Keypoint index of frame B. 122 | * @param distance_ Descriptor distance between those two keypoints. 123 | */ 124 | Match(size_t idxA_, size_t idxB_, float distance_) 125 | : idxA(idxA_), 126 | idxB(idxB_), 127 | distance(distance_) 128 | { 129 | } 130 | size_t idxA; ///< Keypoint index in frame A. 131 | size_t idxB; ///< Keypoint index in frame B. 132 | float distance; ///< Distance between the keypoints. 133 | }; 134 | typedef std::vector Matches; 135 | 136 | /** 137 | * @brief A type to store information about a point in the world map. 138 | */ 139 | struct MapPoint 140 | { 141 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 142 | 143 | /// \brief Default constructor. Point is at origin with quality 0.0 and ID 0. 144 | MapPoint() 145 | : id(0), 146 | quality(0.0), 147 | distance(0.0) 148 | { 149 | } 150 | /** 151 | * @brief Constructor. 152 | * @param id ID of the point. E.g. landmark ID. 153 | * @param point Homogeneous coordinate of the point. 154 | * @param quality Quality of the point. Usually between 0 and 1. 155 | * @param distance Distance to origin of the frame the coordinates are given in. 156 | */ 157 | MapPoint(uint64_t id, const Eigen::Vector4d & point, 158 | double quality, double distance) 159 | : id(id), 160 | point(point), 161 | quality(quality), 162 | distance(distance) 163 | { 164 | } 165 | uint64_t id; ///< ID of the point. E.g. landmark ID. 166 | Eigen::Vector4d point; ///< Homogeneous coordinate of the point. 167 | double quality; ///< Quality of the point. Usually between 0 and 1. 168 | double distance; ///< Distance to origin of the frame the coordinates are given in. 169 | std::map observations; ///< Observations of this point. 170 | }; 171 | 172 | typedef std::vector > MapPointVector; 173 | typedef std::map, 174 | Eigen::aligned_allocator > PointMap; 175 | typedef std::map, 176 | Eigen::aligned_allocator > TransformationMap; 177 | 178 | /// \brief For convenience to pass associations - also contains the 3d points. 179 | struct Observation 180 | { 181 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 182 | 183 | /** 184 | * @brief Constructor. 185 | * @param keypointIdx Keypoint ID. 186 | * @param keypointMeasurement Image coordinates of keypoint. [pixels] 187 | * @param keypointSize Keypoint size. Basically standard deviation of the 188 | * image coordinates in pixels. 189 | * @param cameraIdx Camera index of observed keypoint. 190 | * @param frameId Frame ID of observed keypoint. 191 | * @param landmark_W Associated landmark coordinates in world frame. 192 | * @param landmarkId Unique landmark ID 193 | * @param isInitialized Is the landmark initialized? 194 | */ 195 | Observation(size_t keypointIdx, 196 | const Eigen::Vector2d& keypointMeasurement, 197 | double keypointSize, 198 | size_t cameraIdx, 199 | uint64_t frameId, 200 | const Eigen::Vector4d& landmark_W, 201 | uint64_t landmarkId, bool isInitialized) 202 | : keypointIdx(keypointIdx), 203 | cameraIdx(cameraIdx), 204 | frameId(frameId), 205 | keypointMeasurement(keypointMeasurement), 206 | keypointSize(keypointSize), 207 | landmark_W(landmark_W), 208 | landmarkId(landmarkId), 209 | isInitialized(isInitialized) 210 | { 211 | } 212 | Observation() 213 | : keypointIdx(0), 214 | cameraIdx(-1), 215 | frameId(0), 216 | keypointSize(0), 217 | landmarkId(0), 218 | isInitialized(false) 219 | { 220 | } 221 | size_t keypointIdx; ///< Keypoint ID. 222 | size_t cameraIdx; ///< index of the camera this point is observed in 223 | uint64_t frameId; ///< unique pose block ID == multiframe ID 224 | Eigen::Vector2d keypointMeasurement; ///< 2D image keypoint [pixels] 225 | double keypointSize; ///< Keypoint size. Basically standard deviation of the image coordinates in pixels. 226 | Eigen::Vector4d landmark_W; ///< landmark as homogeneous point in body frame B 227 | uint64_t landmarkId; ///< unique landmark ID 228 | bool isInitialized; ///< Initialisation status of landmark 229 | }; 230 | typedef std::vector > ObservationVector; 231 | 232 | // todo: find a better place for this 233 | typedef Eigen::Matrix SpeedAndBiases; 234 | typedef Eigen::Matrix SpeedAndBias; 235 | 236 | } // namespace okvis 237 | 238 | #endif /* INCLUDE_OKVIS_FRAMETYPEDEFS_HPP_ */ 239 | -------------------------------------------------------------------------------- /third_party/okvis_kinematics/include/okvis/kinematics/operators.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 11, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file operators.hpp 35 | * @brief File with some helper functions related to matrix/vector operations. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ 40 | #define INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | /// \brief okvis Main namespace of this package. 48 | namespace okvis { 49 | 50 | /// \brief kinematics Namespace for kinematics functionality, i.e. transformations and stuff. 51 | namespace kinematics { 52 | 53 | // some free helper functions 54 | 55 | /// \brief Cross matrix of a vector [x,y,z]. 56 | /// Adopted from Schweizer-Messer by Paul Furgale. 57 | /// \tparam Scalar_T The scalar type, auto-deducible (typically double). 58 | /// @param[in] x First vector element. 59 | /// @param[in] y Second vector element. 60 | /// @param[in] z Third vector element. 61 | template 62 | inline Eigen::Matrix crossMx(Scalar_T x, Scalar_T y, Scalar_T z) 63 | { 64 | Eigen::Matrix C; 65 | C(0, 0) = 0.0; 66 | C(0, 1) = -z; 67 | C(0, 2) = y; 68 | C(1, 0) = z; 69 | C(1, 1) = 0.0; 70 | C(1, 2) = -x; 71 | C(2, 0) = -y; 72 | C(2, 1) = x; 73 | C(2, 2) = 0.0; 74 | return C; 75 | } 76 | 77 | /// \brief Cross matrix of a vector v. 78 | /// Adopted from Schweizer-Messer by Paul Furgale. 79 | /// \tparam Derived_T The vector type, auto-deducible. 80 | /// @param[in] v The vector. 81 | template 82 | inline Eigen::Matrix::Scalar, 3, 3> crossMx( 83 | Eigen::MatrixBase const & v) 84 | { 85 | EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Eigen::MatrixBase, 3); 86 | assert((v.cols()==3 && v.rows()==1)||(v.rows()==3 && v.cols()==1)); 87 | return crossMx(v(0, 0), v(1, 0), v(2, 0)); 88 | } 89 | 90 | /// \brief Plus matrix of a quaternion, i.e. q_AB*q_BC = plus(q_AB)*q_BC.coeffs(). 91 | /// @param[in] q_AB A Quaternion. 92 | inline Eigen::Matrix4d plus(const Eigen::Quaterniond & q_AB) { 93 | Eigen::Vector4d q = q_AB.coeffs(); 94 | Eigen::Matrix4d Q; 95 | Q(0,0) = q[3]; Q(0,1) = -q[2]; Q(0,2) = q[1]; Q(0,3) = q[0]; 96 | Q(1,0) = q[2]; Q(1,1) = q[3]; Q(1,2) = -q[0]; Q(1,3) = q[1]; 97 | Q(2,0) = -q[1]; Q(2,1) = q[0]; Q(2,2) = q[3]; Q(2,3) = q[2]; 98 | Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) = q[3]; 99 | return Q; 100 | } 101 | 102 | /// \brief Oplus matrix of a quaternion, i.e. q_AB*q_BC = oplus(q_BC)*q_AB.coeffs(). 103 | /// @param[in] q_BC A Quaternion. 104 | inline Eigen::Matrix4d oplus(const Eigen::Quaterniond & q_BC) { 105 | Eigen::Vector4d q = q_BC.coeffs(); 106 | Eigen::Matrix4d Q; 107 | Q(0,0) = q[3]; Q(0,1) = q[2]; Q(0,2) = -q[1]; Q(0,3) = q[0]; 108 | Q(1,0) = -q[2]; Q(1,1) = q[3]; Q(1,2) = q[0]; Q(1,3) = q[1]; 109 | Q(2,0) = q[1]; Q(2,1) = -q[0]; Q(2,2) = q[3]; Q(2,3) = q[2]; 110 | Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) = q[3]; 111 | return Q; 112 | } 113 | 114 | } // namespace kinematics 115 | } // namespace okvis 116 | 117 | 118 | 119 | #endif /* INCLUDE_OKVIS_KINEMATICS_OPERATORS_HPP_ */ 120 | -------------------------------------------------------------------------------- /third_party/okvis_kinematics/src/dependency-tracker.cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VIS4ROB-lab/v4rl_mesh_based_mapping_lib/4c962907e5574a1f6830b57a22b37b5139c110b5/third_party/okvis_kinematics/src/dependency-tracker.cc -------------------------------------------------------------------------------- /third_party/okvis_kinematics/test/TestTransformation.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Dec 5, 2014 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include 35 | #include "gtest/gtest.h" 36 | 37 | TEST(Transformation, operations) { 38 | for (size_t i = 0; i < 100; ++i) { 39 | okvis::kinematics::Transformation T_AB; 40 | T_AB.setRandom(); 41 | okvis::kinematics::Transformation T_BC; 42 | T_BC.setRandom(); 43 | 44 | // Test inverse 45 | EXPECT_TRUE( 46 | ((T_AB * T_AB.inverse()).T() - Eigen::Matrix4d::Identity()).norm() 47 | < 1e-8); 48 | 49 | // Test composition 50 | EXPECT_TRUE(((T_AB * T_BC).T() - T_AB.T() * T_BC.T()).norm() < 1e-8); 51 | 52 | // Test construction 53 | okvis::kinematics::Transformation T_AB_alternative(T_AB.T()); 54 | EXPECT_TRUE((T_AB.T() - T_AB_alternative.T()).norm() < 1e-8); 55 | okvis::kinematics::Transformation T_AB_alternative2(T_AB.r(), T_AB.q()); 56 | EXPECT_TRUE((T_AB.T() - T_AB_alternative2.T()).norm() < 1e-8); 57 | 58 | // Test = 59 | okvis::kinematics::Transformation T_AB_alternative3; 60 | T_AB_alternative3 = T_AB; 61 | EXPECT_TRUE((T_AB.T() - T_AB_alternative3.T()).norm() < 1e-8); 62 | 63 | // Test setters 64 | okvis::kinematics::Transformation T_AB_alternative4; 65 | T_AB_alternative4.set(T_AB.r(), T_AB.q()); 66 | EXPECT_TRUE((T_AB.T() - T_AB_alternative4.T()).norm() < 1e-8); 67 | okvis::kinematics::Transformation T_AB_alternative5; 68 | T_AB_alternative5.set(T_AB.T()); 69 | EXPECT_TRUE((T_AB.T() - T_AB_alternative5.T()).norm() < 1e-8); 70 | 71 | T_AB.setRandom(); 72 | 73 | // Test oplus 74 | const double dp = 1.0e-6; 75 | Eigen::Matrix jacobian_numDiff; 76 | for (size_t i = 0; i < 6; ++i) { 77 | okvis::kinematics::Transformation T_AB_p = T_AB; 78 | okvis::kinematics::Transformation T_AB_m = T_AB; 79 | Eigen::Matrix dp_p; 80 | Eigen::Matrix dp_m; 81 | dp_p.setZero(); 82 | dp_m.setZero(); 83 | dp_p[i] = dp; 84 | dp_m[i] = -dp; 85 | T_AB_p.oplus(dp_p); 86 | T_AB_m.oplus(dp_m); 87 | /*jacobian_numDiff.block<7, 1>(0, i) = (T_AB_p.parameters() 88 | - T_AB_m.parameters()) / (2.0 * dp);*/ 89 | jacobian_numDiff.block<3, 1>(0, i) = (T_AB_p.r() - T_AB_m.r()) 90 | / (2.0 * dp); 91 | jacobian_numDiff.block<4, 1>(3, i) = (T_AB_p.q().coeffs() 92 | - T_AB_m.q().coeffs()) / (2.0 * dp); 93 | } 94 | Eigen::Matrix jacobian; 95 | T_AB.oplusJacobian(jacobian); 96 | //std::cout << jacobian << std::endl; 97 | //std::cout << jacobian_numDiff << std::endl; 98 | EXPECT_TRUE((jacobian - jacobian_numDiff).norm() < 1e-8); 99 | // also check lift Jacobian: dChi/dx*dx/dChi == 1 100 | Eigen::Matrix lift_jacobian; 101 | T_AB.liftJacobian(lift_jacobian); 102 | EXPECT_TRUE( 103 | (lift_jacobian * jacobian - Eigen::Matrix::Identity()) 104 | .norm() < 1e-8); 105 | 106 | // Test minus 107 | okvis::kinematics::Transformation T_AB_disturbed = T_AB; 108 | Eigen::Matrix delta, delta2; 109 | delta.setRandom(); 110 | delta *= 0.1; // quite large disturbance 111 | T_AB_disturbed.oplus(delta); 112 | // get numeric Jacobian 113 | Eigen::Matrix jacobianMinus_numDiff; 114 | /*for(size_t i=0; i<6; ++i){ 115 | okvis::kinematics::Transformation T_AB_p = T_AB_disturbed; 116 | okvis::kinematics::Transformation T_AB_m = T_AB_disturbed; 117 | Eigen::Matrix dp_p; 118 | Eigen::Matrix dp_m; 119 | dp_p.setZero(); 120 | dp_m.setZero(); 121 | dp_p[i] = dp; 122 | dp_m[i] = -dp; 123 | T_AB_p.oplus(dp_p); 124 | T_AB_m.oplus(dp_m); 125 | }*/ 126 | //Eigen::Matrix minusJacobian; 127 | //T_AB_disturbed.minus(T_AB, delta2, minusJacobian); 128 | //EXPECT_TRUE((delta - delta2).norm() < 1e-8); 129 | } 130 | } 131 | -------------------------------------------------------------------------------- /third_party/okvis_kinematics/test/runTests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | #include "gtest/gtest.h" 35 | 36 | int main(int argc, char **argv) { 37 | testing::InitGoogleTest(&argc, argv); 38 | return RUN_ALL_TESTS(); 39 | } 40 | 41 | -------------------------------------------------------------------------------- /third_party/v4rl_dataset_lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.11) 2 | 3 | project(v4rl_dataset_lib) 4 | 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -march=native -Wall -std=c++11 -fPIC -mssse3") 6 | 7 | FIND_PACKAGE(Boost COMPONENTS filesystem system REQUIRED) 8 | include_directories(${Boost_INCLUDE_DIRS}) 9 | include_directories(include) 10 | 11 | FILE( 12 | GLOB SRCS 13 | src/*.cpp 14 | ) 15 | 16 | FILE( 17 | GLOB HPPS 18 | include/*.hpp 19 | ) 20 | 21 | include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/include ) 22 | 23 | add_library(v4rl_dataset_lib ${SRCS} ${HPPS}) 24 | 25 | 26 | add_executable(test_asl_dataset_helper test/test_asl_dataset_helper.cpp src/asl_dataset_helper.cpp include/asl_dataset_helper.h) 27 | 28 | target_link_libraries(test_asl_dataset_helper 29 | ${Boost_LIBRARIES} 30 | ${OpenCV_LIBRARIES} 31 | pthread 32 | yaml-cpp 33 | ) 34 | -------------------------------------------------------------------------------- /third_party/v4rl_dataset_lib/include/asl_dataset_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_ASL_DATASET_HELPER_H_ 2 | #define INCLUDE_ASL_DATASET_HELPER_H_ 3 | 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //TODO change the variable style to lalala_lelelele_lilili 14 | class ASLData{ 15 | public: 16 | ASLData(){} 17 | virtual ~ASLData(){} 18 | std::string datasetUniqueID_; //TODO add some kind of unique id 19 | }; 20 | 21 | class CameraData : public ASLData 22 | { 23 | private: 24 | struct CameraDataMeasurement 25 | { 26 | unsigned long long int timestamp; 27 | std::string filename;//relative to the folder data 28 | }; 29 | 30 | public: 31 | const CameraDataMeasurement& get(size_t i) const 32 | { 33 | return measurement_.at(i); 34 | } 35 | 36 | size_t n() const 37 | { 38 | return measurement_.size(); 39 | } 40 | 41 | const std::string getImagePathRoot() const 42 | { 43 | return imagePathRoot_.string(); 44 | } 45 | 46 | const std::string getImagePath(size_t i) const 47 | { 48 | return (imagePathRoot_ / measurement_[i].filename).string(); 49 | } 50 | 51 | const std::vector& getMeasurement() 52 | { 53 | return measurement_; 54 | } 55 | 56 | 57 | private: 58 | std::vector measurement_; 59 | boost::filesystem::path dataSourceRoot_; 60 | boost::filesystem::path imagePathRoot_; 61 | 62 | CameraData() {} 63 | friend class ASLDatasetHelper; 64 | static ASLData* Create(std::string datasetFolder) 65 | { 66 | boost::filesystem::path dPath(datasetFolder); 67 | dPath /= "data.csv"; 68 | std::ifstream file ( dPath.string() ); 69 | if(file.is_open()) 70 | { 71 | CameraData* dataSource = new CameraData(); 72 | std::string line; 73 | std::getline(file,line);//drop first line 74 | while(std::getline(file,line)) 75 | { 76 | CameraDataMeasurement temp; 77 | line = line.substr( 0, line.find_first_of("#") ); 78 | char buffer[256]; 79 | if(sscanf(line.c_str(),"%llu,%s", &(temp.timestamp), buffer)==2) 80 | { 81 | temp.filename = buffer; 82 | dataSource->measurement_.push_back(temp); 83 | } 84 | } 85 | dataSource->dataSourceRoot_ = dPath.parent_path(); 86 | dataSource->imagePathRoot_ = dPath.parent_path(); 87 | return dataSource; 88 | } 89 | return nullptr; 90 | } 91 | 92 | 93 | }; 94 | 95 | 96 | class ImuData : public ASLData 97 | { 98 | private: 99 | struct ImuDataMeasurement 100 | { 101 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 102 | unsigned long long int timestamp; 103 | Eigen::Vector3d acc; 104 | Eigen::Vector3d gyro; 105 | }; 106 | 107 | public: 108 | const ImuDataMeasurement& get(size_t i) const 109 | { 110 | return measurement_.at(i); 111 | } 112 | 113 | size_t n() const 114 | { 115 | return measurement_.size(); 116 | } 117 | 118 | const std::vector& getMeasurement() 119 | { 120 | return measurement_; 121 | } 122 | 123 | private: 124 | std::vector measurement_; 125 | 126 | ImuData() {} 127 | friend class ASLDatasetHelper; 128 | static ASLData* Create(std::string datasetFolder) 129 | { 130 | boost::filesystem::path dPath(datasetFolder); 131 | dPath /= "data.csv"; 132 | std::ifstream file ( dPath.string() ); 133 | if(file.is_open()) 134 | { 135 | ImuData* dataSource = new ImuData(); 136 | std::string line; 137 | std::getline(file,line);//drop first line 138 | while(std::getline(file,line)) 139 | { 140 | ImuDataMeasurement temp; 141 | line = line.substr( 0, line.find_first_of("#") ); 142 | double acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z; 143 | sscanf(line.c_str(),"%llu,%lf,%lf,%lf,%lf,%lf,%lf", &(temp.timestamp), &gyro_x, &gyro_y, &gyro_z, &acc_x, &acc_y, &acc_z); 144 | temp.acc << acc_x,acc_y,acc_z; 145 | temp.gyro << gyro_x,gyro_y,gyro_z; 146 | dataSource->measurement_.push_back(temp); 147 | } 148 | return dataSource; 149 | } 150 | return nullptr; 151 | } 152 | 153 | 154 | }; 155 | 156 | class non_copyable 157 | { 158 | protected: 159 | non_copyable() = default; 160 | ~non_copyable() = default; 161 | 162 | non_copyable(non_copyable const &) = delete; 163 | void operator=(non_copyable const &x) = delete; 164 | }; 165 | 166 | 167 | class ASLDatasetHelper : public non_copyable 168 | { 169 | public: 170 | ASLDatasetHelper() { 171 | factories["imu"] = &ImuData::Create; 172 | factories["camera"] = &CameraData::Create; 173 | } 174 | 175 | void addDatasetPath(){} 176 | 177 | std::string getDataSourceType(const std::string filename) 178 | { 179 | YAML::Node config = YAML::LoadFile(filename); 180 | return config["sensor_type"].as(); 181 | } 182 | 183 | 184 | 185 | bool loadDataSource( const std::string id,const std::string remapedId="",const std::string datasetPath = "") 186 | { 187 | 188 | if(datasetPath.empty()) 189 | { 190 | return false; 191 | } 192 | std::string finalId = id; 193 | if(!remapedId.empty()) 194 | { 195 | finalId = remapedId; 196 | } 197 | 198 | boost::filesystem::path dPath(datasetPath); 199 | dPath /= id; 200 | dPath /= "sensor.yaml"; 201 | if(boost::filesystem::exists(dPath)) 202 | { 203 | std::string sensorType = getDataSourceType(dPath.string()); 204 | if(factories.count(sensorType)>0) 205 | { 206 | ASLData* dataSource = factories[sensorType](dPath.parent_path().string()); //TODO add test in case of unknow sensor type 207 | if(dataSource != nullptr) 208 | { 209 | data_[finalId] = dataSource; 210 | return true; 211 | } 212 | } 213 | } 214 | return false; 215 | } 216 | 217 | template 218 | const T* getData(const std::string id) const 219 | { 220 | auto search = data_.find(id); 221 | if(search !=data_.end()) 222 | { 223 | ASLData* dataSource = search->second; 224 | return dynamic_cast( dataSource ); 225 | } 226 | 227 | return nullptr; 228 | } 229 | 230 | std::map data_; 231 | std::vector paths; 232 | std::map> factories; 233 | }; 234 | 235 | 236 | 237 | 238 | #endif //INCLUDE_ASL_DATASET_HELPER_H_ 239 | -------------------------------------------------------------------------------- /third_party/v4rl_dataset_lib/include/okvis_features_dataset.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OKVIS_FEATURES_DATASET_HPP_ 2 | #define OKVIS_FEATURES_DATASET_HPP_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace std; 13 | 14 | struct OkvisFeaturesEntry 15 | { 16 | std::string timestamp_string; 17 | std::string keypoints_filename; 18 | std::string descriptors_filename; 19 | }; 20 | 21 | class OkvisFeaturesDataset 22 | { 23 | public: 24 | std::vector data; 25 | std::map index; 26 | 27 | bool getOkvisFeaturesEntry( uint64_t timestamp, OkvisFeaturesEntry & result ) const 28 | { 29 | std::map::const_iterator it = index.find( timestamp ); 30 | 31 | if(it != index.end()) 32 | { 33 | result = data.at( it->second ); 34 | return true; 35 | } 36 | return false; 37 | } 38 | 39 | size_t load( std::string filename ) 40 | { 41 | datasetPath_ = filename; 42 | datasetPath_ = datasetPath_.parent_path() ; 43 | 44 | data.clear(); 45 | 46 | std::ifstream file( filename ); 47 | 48 | if (!file.is_open()) 49 | return 0; 50 | 51 | std::string line; 52 | while(std::getline(file,line)) 53 | { 54 | std::vector strs; 55 | boost::split(strs,line,boost::is_any_of(",")); 56 | 57 | if(strs.size() != 3) 58 | { 59 | std::cerr << "Format problem line #" << line << "#"; 60 | continue; 61 | } 62 | 63 | OkvisFeaturesEntry current_entry; 64 | current_entry.timestamp_string = strs[0]; 65 | current_entry.keypoints_filename = strs[1]; 66 | current_entry.descriptors_filename = strs[2]; 67 | data.push_back( current_entry ); 68 | 69 | uint64_t timestamp = boost::lexical_cast(current_entry.timestamp_string); 70 | index[timestamp] = data.size()-1; 71 | } 72 | 73 | return data.size(); 74 | } 75 | 76 | bool readKeypointsFile( OkvisFeaturesEntry entry, std::vector& keypoints, std::vector& landmarksId ) 77 | { 78 | return readKeypointsFile( (datasetPath_ / entry.keypoints_filename).string(), keypoints, landmarksId ); 79 | } 80 | 81 | // void readDescriptorsFile( OkvisFeaturesEntry entry, cv::Mat& descriptors ) 82 | // { 83 | // readDescriptorsFile( (datasetPath_ / entry.descriptors_filename).string(), descriptors ); 84 | // } 85 | 86 | private: 87 | 88 | boost::filesystem::path datasetPath_; 89 | 90 | // void readDescriptorsFile( std::string filename, cv::Mat& descriptors ) 91 | // { 92 | // cv::FileStorage fs( filename, cv::FileStorage::READ ); 93 | // cv::FileNode descriptorsFileNode = fs["descriptors"]; 94 | // cv::read( descriptorsFileNode, descriptors ); 95 | // fs.release(); 96 | // } 97 | 98 | bool readKeypointsFile( std::string filename, std::vector& keypoints, std::vector& landmarksId ) 99 | { 100 | keypoints.clear(); 101 | landmarksId.clear(); 102 | std::fstream f( filename, std::ios::in ); 103 | if(!f.is_open()) 104 | { 105 | std::cout << "####### Error on read the file-" << filename << std::endl; 106 | return false; 107 | } 108 | 109 | std::string line; 110 | while (!f.eof()) 111 | { 112 | getline(f, line); 113 | if (!f.eof() && !line.empty()) 114 | { 115 | uint64_t landmarkId = 0; 116 | cv::KeyPoint keypoint; 117 | sscanf(line.c_str(), "%lu, %f, %f, %f, %f, %f, %d, %d", &landmarkId, &keypoint.pt.x, &keypoint.pt.y, 118 | &keypoint.size, &keypoint.response, &keypoint.angle, &keypoint.octave, &keypoint.class_id); 119 | keypoints.push_back( keypoint ); 120 | landmarksId.push_back( landmarkId ); 121 | } 122 | } 123 | 124 | f.close(); 125 | return true; 126 | } 127 | }; 128 | 129 | #endif /* OKVIS_FEATURES_DATASET_HPP_ */ 130 | -------------------------------------------------------------------------------- /third_party/v4rl_dataset_lib/include/optimization_window_dataset.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OPTIMIZATION_WINDOW_DATASET_HPP__ 2 | #define OPTIMIZATION_WINDOW_DATASET_HPP__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | using namespace std; 16 | 17 | struct OptimizationWindowEntry 18 | { 19 | std::string timestamp_string; 20 | kindr::minimal::QuatTransformation t_ws; 21 | std::string landmarks_filename; 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | }; 24 | 25 | struct OkvisLandmark 26 | { 27 | //uint64_t id; 28 | Eigen::Vector3d pos; 29 | double quality; 30 | double distance; 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | }; 33 | 34 | typedef std::vector > OkvisLandmarkVector; 35 | typedef std::map, Eigen::aligned_allocator > OkvisLandmarkMap; 36 | 37 | 38 | class OptimizationWindowDataset 39 | { 40 | public: 41 | 42 | std::vector> data; 43 | std::map index; 44 | 45 | bool getOptimizationWindowEntry( uint64_t timestamp, OptimizationWindowEntry & result ) const 46 | { 47 | std::map::const_iterator it = index.find( timestamp ); 48 | 49 | if(it != index.end()) 50 | { 51 | result = data.at( it->second ); 52 | return true; 53 | } 54 | return false; 55 | } 56 | 57 | size_t load( std::string filename ) 58 | { 59 | datasetPath_ = filename; 60 | datasetPath_ = datasetPath_.parent_path() ; 61 | 62 | data.clear(); 63 | 64 | std::ifstream file( filename ); 65 | 66 | if (!file.is_open()) 67 | return 0; 68 | 69 | std::string line; 70 | while(std::getline(file,line)) 71 | { 72 | std::vector strs; 73 | boost::split(strs,line,boost::is_any_of(",")); 74 | 75 | if(strs.size() != 12) 76 | { 77 | LOG(ERROR) << "Format problem line #" << line << "#"; 78 | continue; 79 | } 80 | 81 | OptimizationWindowEntry current_entry; 82 | current_entry.timestamp_string = strs[0]; 83 | 84 | Eigen::Vector3d p_WS(boost::lexical_cast(strs[1]), 85 | boost::lexical_cast(strs[2]), 86 | boost::lexical_cast(strs[3])); 87 | 88 | Eigen::Quaterniond q_WS; 89 | q_WS.x() = boost::lexical_cast(strs[4]); 90 | q_WS.y() = boost::lexical_cast(strs[5]); 91 | q_WS.z() = boost::lexical_cast(strs[6]); 92 | q_WS.w() = boost::lexical_cast(strs[7]); 93 | 94 | current_entry.t_ws = kindr::minimal::QuatTransformation(p_WS,q_WS); 95 | 96 | current_entry.landmarks_filename = strs[9]; 97 | data.push_back( current_entry ); 98 | 99 | 100 | uint64_t timestamp = boost::lexical_cast(current_entry.timestamp_string); 101 | index[timestamp] = data.size()-1; 102 | } 103 | 104 | return data.size(); 105 | } 106 | 107 | bool readLandmarksFile( OptimizationWindowEntry entry, OkvisLandmarkMap& landmarks ) 108 | { 109 | return readLandmarksFile( (datasetPath_ / entry.landmarks_filename).string(), landmarks ); 110 | } 111 | 112 | void readLandmarksPositions( OptimizationWindowEntry entry, std::map >& landmarks ) 113 | { 114 | readLandmarksPositions( (datasetPath_ / entry.landmarks_filename).string(), landmarks ); 115 | } 116 | 117 | private: 118 | 119 | boost::filesystem::path datasetPath_; 120 | 121 | bool readLandmarksFile( std::string filename, OkvisLandmarkMap& landmarks ) 122 | { 123 | landmarks.clear(); 124 | 125 | fstream f( filename, ios::in ); 126 | if(! f.is_open() ) 127 | return false; 128 | 129 | std::string line; 130 | while (!f.eof()) 131 | { 132 | getline(f, line); 133 | if (!f.eof() && !line.empty()) 134 | { 135 | OkvisLandmark landmark; 136 | uint64_t landmarkId; 137 | double x, y, z; 138 | sscanf( line.c_str(), "%lu, %lf, %lf, %lf, %lf, %lf", &landmarkId, &x, &y, &z, &landmark.quality, &landmark.distance ); 139 | landmark.pos = Eigen::Vector3d(x,y,z); 140 | landmarks.insert( std::pair(landmarkId,landmark) ); 141 | } 142 | } 143 | 144 | f.close(); 145 | return true; 146 | } 147 | 148 | void readLandmarksPositions( std::string filename, std::map >& landmarks ) 149 | { 150 | fstream f( filename, ios::in ); 151 | assert( f.is_open() ); 152 | 153 | std::string line; 154 | while (!f.eof()) 155 | { 156 | getline(f, line); 157 | if (!f.eof() && !line.empty()) 158 | { 159 | uint64_t landmarkId; 160 | double x, y, z; 161 | sscanf( line.c_str(), "%lu, %lf, %lf, %lf, %*f, %*f", &landmarkId, &x, &y, &z ); 162 | 163 | std::vector landmarkPosition; 164 | landmarkPosition.push_back( x ); 165 | landmarkPosition.push_back( y ); 166 | landmarkPosition.push_back( z ); 167 | 168 | landmarks.insert( std::pair >(landmarkId,landmarkPosition) ); 169 | } 170 | } 171 | 172 | f.close(); 173 | } 174 | 175 | }; 176 | 177 | #endif 178 | -------------------------------------------------------------------------------- /third_party/v4rl_dataset_lib/src/asl_dataset_helper.cpp: -------------------------------------------------------------------------------- 1 | //empty 2 | -------------------------------------------------------------------------------- /third_party/v4rl_dataset_lib/test/test_asl_dataset_helper.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | int main(void) 5 | { 6 | 7 | ASLDatasetHelper dataHelper; 8 | dataHelper.loadDataSource("imu0","","/home/lucas/data/bags/outputdir"); 9 | dataHelper.loadDataSource("cam0","","/home/lucas/data/bags/outputdir"); 10 | 11 | 12 | const ImuData* imu0 = dataHelper.getData("imu0"); 13 | for(int i=0 ; i < 2 ; i++) 14 | { 15 | std::cout << imu0->get(i).timestamp << " " << imu0->get(i).acc << " " << imu0->get(i).gyro << std::endl; 16 | } 17 | 18 | const CameraData* cam0 = dataHelper.getData("cam0"); 19 | for(int i=0 ; i < 2 ; i++) 20 | { 21 | std::cout << cam0->get(i).timestamp << " " << cam0->getImagePathRoot() << " " << cam0->get(i).filename << std::endl; 22 | } 23 | 24 | return 0; 25 | } 26 | --------------------------------------------------------------------------------