├── LICENSE ├── README.md ├── doc ├── graph_msf.gif ├── imu_parameters.md ├── installation.md └── titleimg.png ├── examples ├── README.md └── excavator_dual_graph │ ├── CMakeLists.txt │ ├── config │ ├── extrinsic_params.yaml │ ├── gnss_params.yaml │ ├── graph_config.yaml │ └── graph_params.yaml │ ├── include │ └── excavator_dual_graph │ │ ├── ExcavatorEstimator.h │ │ └── ExcavatorStaticTransforms.h │ ├── launch │ ├── dual_graph.launch │ └── dual_graph_no_urdf.launch │ ├── package.xml │ ├── rviz │ └── gnss_lidar.rviz │ └── src │ ├── dual_graph_node.cpp │ └── lib │ ├── ExcavatorEstimator.cpp │ ├── ExcavatorStaticTransforms.cpp │ └── readParams.cpp ├── graph_msf ├── CMakeLists.txt ├── config │ ├── gnss_params.yaml │ ├── graph_config.yaml │ └── graph_params.yaml ├── include │ └── graph_msf │ │ ├── Datatypes.hpp │ │ ├── GraphManager.hpp │ │ ├── GraphMsf.h │ │ ├── GraphMsfInterface.h │ │ ├── GraphState.hpp │ │ ├── ImuBuffer.hpp │ │ ├── InterfacePrediction.h │ │ ├── StaticTransforms.h │ │ ├── config │ │ └── GraphConfig.h │ │ ├── factors │ │ ├── HeadingFactor.h │ │ └── PitchFactor.h │ │ ├── geometry │ │ ├── Angle.h │ │ └── math_utils.h │ │ ├── gnss │ │ ├── Gnss.h │ │ └── GnssHandler.h │ │ └── measurements │ │ ├── BinaryMeasurement.h │ │ ├── BinaryMeasurement6D.h │ │ ├── Measurement.h │ │ ├── UnaryMeasurement.h │ │ ├── UnaryMeasurement1D.h │ │ ├── UnaryMeasurement3D.h │ │ └── UnaryMeasurement6D.h ├── package.xml └── src │ └── lib │ ├── Gnss.cpp │ ├── GnssHandler.cpp │ ├── GraphManager.cpp │ ├── GraphMsf.cpp │ ├── GraphMsfInterface.cpp │ └── ImuBuffer.cpp └── graph_msf_ros ├── CMakeLists.txt ├── config ├── extrinsic_params.yaml ├── gnss_params.yaml └── graph_params.yaml ├── include └── graph_msf_ros │ ├── GraphMsfRos.h │ ├── extrinsics │ ├── ElementToRoot.h │ ├── StaticTransformsTf.h │ └── StaticTransformsUrdf.h │ └── util │ └── conversions.h ├── launch └── compslam_ros.launch ├── package.xml ├── rviz └── vis.rviz └── src └── lib ├── GraphMsfRos.cpp ├── StaticTransformsTf.cpp └── StaticTransformsUrdf.cpp /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2022, Julian Nubert, Robotic 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 copyright holder nor the names of its 12 | contributors may be used to endorse or promote products derived 13 | 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 COPYRIGHT HOLDER OR CONTRIBUTORS 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. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Graph-MSF: Graph-based Multi-sensor Fusion for Consistent Localization and State Estimation 2 | 3 | **Authors:** [Julian Nubert](https://juliannubert.com/) ([nubertj@ethz.ch](mailto:nubertj@ethz.ch?subject=[GitHub])) 4 | , [Shehryar Khattak](https://www.linkedin.com/in/shehryar-khattak/) 5 | , [Marco Hutter](https://rsl.ethz.ch/the-lab/people/person-detail.MTIxOTEx.TGlzdC8yNDQxLC0xNDI1MTk1NzM1.html) 6 | 7 | 8 | 9 | ## Resources 10 | **[1] ICRA2022, Philadelphia** 11 | * [project page](https://sites.google.com/leggedrobotics.com/gmfcl). 12 | * [paper](https://arxiv.org/pdf/2203.01389.pdf) 13 | * [video](https://youtu.be/syTV7Ui36jg) 14 | 15 | ## Overview 16 | The presented framework aims for a flexible and fast fusion of multiple sensor modalities. The state estimate is 17 | published at **imu frequency** through IMU pre-integration with a multi-threaded implementation and book-keeping. 18 | The adding of the measurements and the optimization of the graph are performed in different threads. 19 | In contrast to classical filtering-based approaches this graph-based structure also allows for a simple incorporation of 20 | delayed sensor measurements up to the smoothingLag. 21 | 22 | There are two intended **use-cases**: 23 | 24 | 1. Using the dual graph formulation as proosed in [1]. In this part of the implementation there are hard-coded 25 | components for this specific use-case. 26 | 2. A more general graph-based multi-sensor fusion. An example for fusing LiDAR odometry and IMU on the dataset of the 27 | [ETH Zurich Robotic Summer School](https://ethz-robotx.github.io/SuperMegaBot/) will follow shortly. 28 | 29 | 30 | 31 | **© IEEE** 32 | 33 | **Disclaimer:** The framework is still under development and will be updated, extended, and more generalized in the future. 34 | 35 | ## Modules and Packages 36 | This repository contains the following modules: 37 | 38 | 1. _graph_msf_: The core library for the sensor fusion. This library is only dependant on Eigen and GTSAM. 39 | 2. _graph_msf_ros_: This package provides a basic class for using GraphMsf in ROS. It is dependant on GraphMsf and ROS. 40 | 3. _examples_: Examples on how to use GraphMsf and GraphMsfRos. 41 | - [./examples/excavator_dual_graph](./examples/excavator_dual_graph) from [1]. This is the implementation as presented in the paper. 42 | - Single-graph standalone fusion example following soon. 43 | 44 | ## Installation 45 | For the installation instructions please refer to the [./doc/installation.md](./doc/installation.md). 46 | 47 | ## Example Usage 48 | Instructions on how to use and run the examples can be found in the [./examples/README.md](./examples/README.md). 49 | 50 | ## Data 51 | We provide some example datasets from our excavator HEAP. The data contains 52 | * IMU measurements, 53 | * LiDAR odometry from CompSLAM, 54 | * Left and Right GNSS measurements, 55 | * and an (arm-)filtered point cloud. 56 | 57 | The datasets can be found here: [Google Drive Link](https://drive.google.com/drive/folders/1qZg_DNH3wXnQu4tNIcqY925KZFDu8y0M?usp=sharing). 58 | 59 | ## Custom Usage 60 | For custom usage, such as the fusion of more sensor measurements, an own class with the desired functionality can be implemented. 61 | This class only has to inherit from the **GraphMsfInterface** base-class. 62 | 63 | For usage three functionalities have to be implemented by the user: 64 | 65 | 1. (ROS)-callbacks or other code snippets can be used to add measurements through the given interface as 66 | specified [here](graph_msf/include/graph_msf/GraphMsfInterface.h). Examples for this can be seen in the 67 | [excavator_dual_graph](./examples/excavator_dual_graph), where ROS subscribers are used to add the measurements to the graph. 68 | 2. Furthermore, the purely virtual functions _publishState_ needs to be implemented. This method is called after each 69 | arrival of an IMU measurement to publish the state in the desired format. 70 | 3. Lastly, _readParams__ is called during initialization, and is needed to load extrinsic parameters inside the 71 | [StaticTransforms](graph_msf/include/graph_msf/StaticTransforms.h) required for coordinate transformations. 72 | Note that the _StaticTransforms_ class can contain arbitrary extrinsic transformation pairs. 73 | 74 | The measurements can be passed to the interface using the _measurements_ specified in 75 | [./graph_msf/include/measurements](./graph_msf/include/measurements). More measurements can be easily added for 76 | custom usage. 77 | 78 | ## Paper 79 | If you find this code useful, please consider citing 80 | ``` 81 | @inproceedings{nubert2022graph, 82 | title={Graph-based Multi-sensor Fusion for Consistent Localization of Autonomous Construction Robots}, 83 | author={Nubert, Julian and Khattak, Shehryar and Hutter, Marco}, 84 | booktitle={IEEE International Conference on Robotics and Automation (ICRA)}, 85 | year={2022}, 86 | organization={IEEE} 87 | } 88 | ``` 89 | 90 | ## Acknowledgements 91 | The authors thank Marco Tranzatto, Simon Kerscher, Dominic Jud, Lorenzo Terenzi, Timo Schoenegg and the remaining HEAP 92 | team for patiently testing parts of this framework during their experiments. 93 | -------------------------------------------------------------------------------- /doc/graph_msf.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/graph_msf/48044a1e5945db0595e5962f4f59bedbb7083695/doc/graph_msf.gif -------------------------------------------------------------------------------- /doc/imu_parameters.md: -------------------------------------------------------------------------------- 1 | # IMU Parameters 2 | Parameters for IMU can be calculated using these [instructions](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model). 3 | ## Used: Lord microstrain MV5-AR IMU in Roofbox 4 | * [Datasheet](https://www.microstrain.com/sites/default/files/mv5-ar_datasheet_8400-0122_rev_d.pdf) 5 | * Values 6 | * Noise density 7 | * Accelerometer: 85 ug/sqrt(Hz)=85e-5m/s^2/sqrt(Hz) 8 | * Gyroscope: 0.0075° /sec/√Hz=1.309e-4 rad/s/sqrt(Hz) -------------------------------------------------------------------------------- /doc/installation.md: -------------------------------------------------------------------------------- 1 | # Installation 2 | 3 | ## Requirements 4 | 5 | #### Eigen3 6 | Make sure you have the Eigen3-headers in your include path. This is usually automatically the case if you have ROS installed. 7 | 8 | #### GTSAM 9 | We install GTSAM 4.1.1 from source, but also other versions and pre-built binaries might work. We recommend a local installation of GTSAM. 10 | 11 | * Version: [GTSAM 4.1.1](https://github.com/borglab/gtsam/tree/4.1.1) 12 | * Clone and checkout at an arbitrary location. 13 | 14 | ```bash 15 | git clone git@github.com:borglab/gtsam.git 16 | cd gtsam 17 | git checkout 4.1.1 18 | mkdir build && cd build 19 | ``` 20 | 21 | * Use CMake with the following options. Note that _-DCMAKE_INSTALL_PREFIX:PATH=$HOME/.local_ specifies the location where GTSAM is installed to and can be individually adapted. This flag can also be removed, leading to a global installation. 22 | 23 | ```bash 24 | cmake -DCMAKE_INSTALL_PREFIX:PATH=$HOME/.local -DCMAKE_BUILD_TYPE=Release -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_USE_QUATERNIONS=ON -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF .. 25 | ``` 26 | 27 | * Compile and install locally: 28 | 29 | ```bash 30 | make install -j$(nproc) 31 | ``` 32 | 33 | * Environment variables (e.g. add to your .bashrc-file): 34 | 35 | ``` 36 | export CMAKE_PREFIX_PATH=$HOME/.local/:$CMAKE_PREFIX_PATH 37 | export LD_LIBRARY_PATH=$HOME/.local/lib/:$LD_LIBRARY_PATH 38 | export LIBRARY_PATH=${LIBRARY_PATH}:${LD_LIBRARY_PATH} 39 | ``` 40 | This is usually only needed if you you choose a non-standard (local) install directory. 41 | 42 | ## graph_msf 43 | ### Workspace 44 | GraphMsf only has two main dependencies: Eigen3 and GTSAM. These were installed in the last step. 45 | For compiling graph_msf create a workspace and set it to _Release_-mode. 46 | ```bash 47 | mkdir catkin_ws 48 | mkdir src 49 | catkin init 50 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 51 | cd src && git clone https://github.com/leggedrobotics/graph_msf.git 52 | ``` 53 | 54 | ### Compiling 55 | Now the library then can be compiled using: 56 | ```bash 57 | catkin build graph_msf 58 | ``` 59 | 60 | ## graph_msf_ros 61 | In contrast to _graph_msf_ this package also depends on standard ROS dependencies. We tested the framework with ROS Noetic on Ubuntu 20.04. 62 | For compiling the code run: 63 | ```bash 64 | catkin build graph_msf_ros 65 | ``` 66 | -------------------------------------------------------------------------------- /doc/titleimg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/graph_msf/48044a1e5945db0595e5962f4f59bedbb7083695/doc/titleimg.png -------------------------------------------------------------------------------- /examples/README.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | 3 | ## excavator_dual_graph 4 | 5 | ### Visualization 6 | For a visualization of a simplified model of our excavator HEAP clone the following repository into the workspace: 7 | ```bash 8 | https://github.com/leggedrobotics/rsl_heap.git 9 | ``` 10 | This step can also be left out. 11 | 12 | ### Compiling the Code 13 | This is the example of the code of our ICRA2022 paper [1]. 14 | For compiling this code simply run: 15 | ```bash 16 | catkin build excavator_dual_graph 17 | ``` 18 | 19 | ### Data 20 | For two example scenarios on our test-field at ETH Zurich please download the data located at the 21 | [Google Drive Link](https://drive.google.com/drive/folders/1qZg_DNH3wXnQu4tNIcqY925KZFDu8y0M?usp=sharing). 22 | 23 | ### Running the Code 24 | In terminal 1 run (after sourcing the workspace): 25 | ```bash 26 | roslaunch excavator_dual_graph dual_graph.launch 27 | # or if heap_urdf not available 28 | roslaunch excavator_dual_graph dual_graph_no_urdf.launch 29 | ``` 30 | In terminal 2 play one of the rosbags located at the provided link location. Make sure to set the `--clock`-flag. 31 | Either 32 | ```bash 33 | rosbag play 1_digging_filtered.bag --clock 34 | # or 35 | rosbag play 2_driving_away_filtered.bag --clock 36 | ``` 37 | 38 | ### Expected Output 39 | The expected output looks like this: 40 | 41 | 42 | 43 | Here, yellow depicts the measured GNSS path, green depicts the estimated path in the world frame, and blue depicts the 44 | estimated path in the odometry frame. -------------------------------------------------------------------------------- /examples/excavator_dual_graph/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(excavator_dual_graph) 3 | 4 | set(CATKIN_PACKAGE_DEPENDENCIES 5 | graph_msf 6 | graph_msf_ros 7 | geometry_msgs 8 | nav_msgs 9 | sensor_msgs 10 | roscpp 11 | std_msgs 12 | tf 13 | message_filters 14 | ) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | ${CATKIN_PACKAGE_DEPENDENCIES} 18 | ) 19 | 20 | #find_package(Eigen3 REQUIRED) 21 | #message("Eigen Version:" ${EIGEN3_VERSION_STRING}) 22 | #message("Eigen Path:" ${Eigen3_DIR}) 23 | 24 | catkin_package( 25 | INCLUDE_DIRS include 26 | LIBRARIES ${PROJECT_NAME} 27 | CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} 28 | ) 29 | 30 | ########### 31 | ## Build ## 32 | ########### 33 | set(CMAKE_CXX_STANDARD 14) 34 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 35 | 36 | include_directories( 37 | include 38 | ${catkin_INCLUDE_DIRS} 39 | ) 40 | 41 | # Library 42 | add_library(${PROJECT_NAME} 43 | src/lib/ExcavatorEstimator.cpp 44 | src/lib/readParams.cpp 45 | src/lib/ExcavatorStaticTransforms.cpp 46 | ) 47 | target_link_libraries(${PROJECT_NAME} 48 | ${catkin_LIBRARIES}) 49 | 50 | # Executable 51 | add_executable(${PROJECT_NAME}_node src/dual_graph_node.cpp) 52 | target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}) 53 | 54 | add_dependencies(${PROJECT_NAME} 55 | ${catkin_EXPORTED_TARGETS}) 56 | 57 | # Add clang tooling 58 | find_package(cmake_clang_tools QUIET) 59 | if(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) 60 | add_clang_tooling( 61 | TARGET ${PROJECT_NAME} 62 | SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include 63 | CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include 64 | CF_FIX 65 | ) 66 | endif(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) 67 | 68 | ############# 69 | ## Install ## 70 | ############# 71 | 72 | install(TARGETS ${PROJECT_NAME} 73 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 74 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 75 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 76 | ) 77 | 78 | install(DIRECTORY include/${PROJECT_NAME}/ 79 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 80 | ) 81 | 82 | ## Mark cpp header files for installation 83 | install(DIRECTORY config launch rviz 84 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 85 | ) 86 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/config/extrinsic_params.yaml: -------------------------------------------------------------------------------- 1 | #External Prior/Transform Input 2 | extrinsics: 3 | mapFrame: map 4 | odomFrame: odom 5 | baseLinkFrame: BASE 6 | lidarFrame: os_sensor #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame 7 | imuFrame: imu_box_link # IMU_CABIN_link #Frame of IMU used in integrator - Used for lookup transform to lidarFrame 8 | imuBaseFrame: IMU_link 9 | cabinFrame: CABIN 10 | gnssFrame1: GNSS_L 11 | gnssFrame2: GNSS_R 12 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/config/gnss_params.yaml: -------------------------------------------------------------------------------- 1 | gnss: 2 | useGnssReference: true 3 | referenceLatitude: 47.4084860363 # Wangen a.A.: 47.234593265310046, Hoengg: 47.4084860363, Oberglatt: 47.4801402 4 | referenceLongitude: 8.50435818058 # Wangen a.A.: 7.681415329904487, Hoengg: 8.50435818058, Oberglatt: 8.49975325 5 | referenceAltitude: 565.0 # Wangen a.A.: 472.58400000000074, Hoengg: 565.0, Oberglatt: 474.9 6 | referenceHeading: 0.0 # radians, # Wangen a.A.: 0.0, Hoengg: 0.0, Oberglatt: 2.6305 7 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/config/graph_config.yaml: -------------------------------------------------------------------------------- 1 | #Common 2 | common_params: 3 | verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors 4 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/config/graph_params.yaml: -------------------------------------------------------------------------------- 1 | # Sensor Params 2 | sensor_params: 3 | imuRate: 100 #Rate of IMU input (Hz) 4 | imuBufferLength: 200 5 | lidarOdometryRate: 5 6 | gnssRate: 20 7 | imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot 8 | 9 | # Factor Graph 10 | graph_params: 11 | useIsam: true # if false, then levenberg-marquardt is used 12 | smootherLag: 1.5 #Lag of fixed lag smoother[seconds], not needed for ISAM2 13 | additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors 14 | findUnusedFactorSlots: false 15 | enableDetailedResults: false 16 | usingFallbackGraph: true 17 | 18 | # Outlier Rejection 19 | outlier_params: 20 | gnssOutlierThreshold: 0.2 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements 21 | 22 | # Noise Parameters 23 | noise_params: 24 | ## IMU 25 | ### Accelerometer 26 | accNoiseDensity: 7.225e-08 #Continuous-time "Covariance" of accelerometer, microstrain: sigma^2=7.225e-7 27 | accBiasRandomWalk: 1.0e-05 #Continuous-time "Covariance" describing accelerometer bias random walk, default: 1.0e-06 28 | accBiasPrior: 0.0 #Prior/starting value of accelerometer bias 29 | ### Gyro 30 | gyrNoiseDensity: 1.71e-08 #Continuous-time "Covariance" of gyroscope measurements, microstrain: sigma^2=1.71-08 31 | gyrBiasRandomWalk: 9.33e-06 #gnss:9.33e-08 #lidar: 9.33e-01 #Continuous-time "Covariance" describing gyroscope bias random walk, default: 9.33e-08 32 | gyrBiasPrior: 0.0 #Prior/starting value of gyroscope bias 33 | ### Preintegration 34 | integrationNoiseDensity: 1.0e-03 #continuous-time "Covariance" describing integration uncertainty, default: 1.0e-06 35 | biasAccOmegaPreInt: 1.0e-03 #covariance of bias used for preintegration, default: 1.0e-2 36 | ## LiDAR 37 | poseBetweenNoise: [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ] # gnss [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ] # lidar: [ 1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0 ] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) 38 | poseUnaryNoise: [ 1000.0, 1000.0, 10.0, 10.0, 10.0, 10.0 ] # ORDER RPY(rad) - XYZ(meters) First tests: [ 1000.0, 1000.0, 1000.0, 2.0, 2.0, 2.0 ] 39 | ## GNSS 40 | gnssPositionUnaryNoise: 1.0 #1.0e-02 # x, y, z of global position 41 | gnssHeadingUnaryNoise: 1.0 # x,y,z of heading vector 42 | 43 | # Relinearization 44 | relinearization_params: 45 | positionReLinTh: 5.0e-10 #Position linearization threshold 46 | rotationReLinTh: 1.0e-10 #Rotation linearization threshold 47 | velocityReLinTh: 1.0e-10 #Linear Velocity linearization threshold 48 | accBiasReLinTh: 1.0e-10 #Accelerometer bias linearization threshold 49 | gyrBiasReLinTh: 1.0e-10 #Gyroscope bias linearization threshold 50 | relinearizeSkip: 1 51 | enableRelinearization: true 52 | evaluateNonlinearError: false 53 | cacheLinearizedFactors: true 54 | enablePartialRelinearizationCheck: true 55 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/include/excavator_dual_graph/ExcavatorEstimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef EXCAVATORESTIMATOR_H 9 | #define EXCAVATORESTIMATOR_H 10 | 11 | // std 12 | #include 13 | 14 | // ROS 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // Workspace 26 | #include "excavator_dual_graph/ExcavatorStaticTransforms.h" 27 | #include "graph_msf/gnss/GnssHandler.h" 28 | #include "graph_msf/measurements/UnaryMeasurement6D.h" 29 | #include "graph_msf_ros/GraphMsfRos.h" 30 | 31 | // Defined Macros 32 | #define ROS_QUEUE_SIZE 100 33 | #define NUM_GNSS_CALLBACKS_UNTIL_START 20 // 0 34 | 35 | namespace excavator_se { 36 | 37 | class ExcavatorEstimator : public graph_msf::GraphMsfRos { 38 | public: 39 | ExcavatorEstimator(std::shared_ptr privateNodePtr); 40 | 41 | private: 42 | // Publish State 43 | void publishState_(const double imuTimeK, const Eigen::Matrix4d& T_W_O, const Eigen::Matrix4d& T_O_Ik, const Eigen::Vector3d& Ic_v_W_Ic, 44 | const Eigen::Vector3d& I_w_W_I) override; 45 | 46 | // Parameter Reading Commodity Function 47 | void readParams_(const ros::NodeHandle& privateNode); 48 | 49 | void initializePublishers_(std::shared_ptr& privateNodePtr) override; 50 | 51 | void initializeSubscribers_(std::shared_ptr& privateNodePtr) override; 52 | 53 | void initializeMessages_(std::shared_ptr& privateNodePtr); 54 | 55 | // GNSS Handler 56 | std::shared_ptr gnssHandlerPtr_; 57 | 58 | // Time 59 | std::chrono::time_point startTime_; 60 | std::chrono::time_point currentTime_; 61 | 62 | // Config ------------------------------------- 63 | 64 | // Rates 65 | double lidarRate_ = 5.0; 66 | double gnssLeftRate_ = 20.0; 67 | double gnssRightRate_ = 20.0; 68 | 69 | // Noise 70 | Eigen::Matrix poseBetweenNoise_; 71 | Eigen::Matrix poseUnaryNoise_; 72 | double gnssPositionUnaryNoise_ = 1.0; // in [m] 73 | double gnssHeadingUnaryNoise_ = 1.0; // in [rad] 74 | 75 | /// Flags 76 | bool usingLioFlag_ = true; 77 | 78 | // ROS Related stuff ---------------------------- 79 | 80 | // Callbacks 81 | void imuCallback_(const sensor_msgs::Imu::ConstPtr& imuPtr); 82 | void lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& lidar_odom_ptr); 83 | void gnssCallback_(const sensor_msgs::NavSatFix::ConstPtr& leftGnssPtr, const sensor_msgs::NavSatFix::ConstPtr& rightGnssPtr); 84 | 85 | // Node 86 | ros::NodeHandle privateNode_; 87 | 88 | // Subscribers 89 | // Instances 90 | ros::Subscriber subImu_; 91 | ros::Subscriber subLidarOdometry_; 92 | message_filters::Subscriber subGnssLeft_; 93 | message_filters::Subscriber subGnssRight_; 94 | tf::TransformListener tfListener_; 95 | // GNSS Sync Policy 96 | typedef message_filters::sync_policies::ExactTime GnssExactSyncPolicy; 97 | boost::shared_ptr> gnssExactSyncPtr_; // ROS Exact Sync Policy Message Filter 98 | // TF 99 | tf::TransformListener listener_; 100 | 101 | // Publishers 102 | // Odometry 103 | ros::Publisher pubEstOdomImu_; 104 | ros::Publisher pubEstMapImu_; 105 | // Path 106 | ros::Publisher pubEstOdomImuPath_; 107 | ros::Publisher pubEstMapImuPath_; 108 | ros::Publisher pubMeasMapGnssLPath_; 109 | ros::Publisher pubMeasMapGnssRPath_; 110 | ros::Publisher pubMeasMapLidarPath_; 111 | // TF 112 | tf::TransformBroadcaster tfBroadcaster_; 113 | 114 | // Messages 115 | // Odometry 116 | nav_msgs::OdometryPtr odomImuMsgPtr_; 117 | nav_msgs::OdometryPtr mapImuMsgPtr_; 118 | // Path 119 | nav_msgs::PathPtr estOdomImuPathPtr_; 120 | nav_msgs::PathPtr estMapImuPathPtr_; 121 | nav_msgs::PathPtr measMapLeftGnssPathPtr_; 122 | nav_msgs::PathPtr measMapRightGnssPathPtr_; 123 | nav_msgs::PathPtr measMapLidarPathPtr_; 124 | }; 125 | } // namespace excavator_se 126 | #endif // end M545ESTIMATORGRAPH_H 127 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/include/excavator_dual_graph/ExcavatorStaticTransforms.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef ExcavatorStaticTransforms_H 9 | #define ExcavatorStaticTransforms_H 10 | // Workspace 11 | #include "graph_msf_ros/extrinsics/StaticTransformsTf.h" 12 | 13 | namespace excavator_se { 14 | 15 | class ExcavatorStaticTransforms : public graph_msf::StaticTransformsTf { 16 | public: 17 | ExcavatorStaticTransforms(std::shared_ptr privateNodePtr); 18 | 19 | // Setters 20 | void setMapFrame(const std::string& s) { mapFrame_ = s; } 21 | void setOdomFrame(const std::string& s) { odomFrame_ = s; } 22 | void setLidarFrame(const std::string& s) { lidarFrame_ = s; } 23 | void setLeftGnssFrame(const std::string& s) { leftGnssFrame_ = s; } 24 | void setRightGnssFrame(const std::string& s) { rightGnssFrame_ = s; } 25 | void setCabinFrame(const std::string& s) { cabinFrame_ = s; } 26 | void setBaseLinkFrame(const std::string& s) { baseLinkFrame_ = s; } 27 | 28 | // Getters 29 | const std::string& getMapFrame() { return mapFrame_; } 30 | const std::string& getOdomFrame() { return odomFrame_; } 31 | const std::string& getLidarFrame() { return lidarFrame_; } 32 | const std::string& getLeftGnssFrame() { return leftGnssFrame_; } 33 | const std::string& getRightGnssFrame() { return rightGnssFrame_; } 34 | const std::string& getCabinFrame() { return cabinFrame_; } 35 | const std::string& getBaseLinkFrame() { return baseLinkFrame_; } 36 | 37 | private: 38 | void findTransformations() override; 39 | 40 | // Members 41 | std::string mapFrame_; 42 | std::string odomFrame_; 43 | std::string lidarFrame_; 44 | std::string leftGnssFrame_; 45 | std::string rightGnssFrame_; 46 | std::string cabinFrame_; 47 | std::string baseLinkFrame_; 48 | }; 49 | } // namespace excavator_se 50 | #endif // end AsopStaticTransforms_H 51 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/launch/dual_graph.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/launch/dual_graph_no_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | excavator_dual_graph 4 | 0.0.0 5 | 6 | 7 | State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry and GNSS estimates. 8 | 9 | 10 | Julian Nubert 11 | BSD 12 | Julian Nubert 13 | 14 | catkin 15 | graph_msf 16 | graph_msf_ros 17 | geometry_msgs 18 | nav_msgs 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | tf 23 | std_srvs 24 | message_filters 25 | 26 | graph_msf 27 | graph_msf_ros 28 | geometry_msgs 29 | nav_msgs 30 | roscpp 31 | std_msgs 32 | sensor_msgs 33 | tf 34 | std_srvs 35 | message_filters 36 | heap_urdf 37 | 38 | rostest 39 | rosbag 40 | 41 | 42 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/rviz/gnss_lidar.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /ExcavatorModel1 9 | - /TF1/Frames1 10 | Splitter Ratio: 0.4882352948188782 11 | Tree Height: 787 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 0.6000000238418579 56 | Class: rviz/RobotModel 57 | Collision Enabled: true 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | BASE: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | BASE_inertia: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | BOOM: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | CABIN: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | DIPPER: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | ENDEFFECTOR: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | ENDEFFECTOR_CONTACT: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Expand Joint Details: false 95 | Expand Link Details: false 96 | Expand Tree: false 97 | GNSS_L: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | GNSS_R: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | IMU_base_link: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | IMU_link: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | LF_BEAM: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | LF_BEARING: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | LF_KNUCKLE: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | LF_PARALLEL: 132 | Alpha: 1 133 | Show Axes: false 134 | Show Trail: false 135 | Value: true 136 | LF_SWIVEL: 137 | Alpha: 1 138 | Show Axes: false 139 | Show Trail: false 140 | Value: true 141 | LF_WHEEL: 142 | Alpha: 1 143 | Show Axes: false 144 | Show Trail: false 145 | Value: true 146 | LF_WHEEL_CONTACT: 147 | Alpha: 1 148 | Show Axes: false 149 | Show Trail: false 150 | LH_BEAM: 151 | Alpha: 1 152 | Show Axes: false 153 | Show Trail: false 154 | Value: true 155 | LH_KNUCKLE: 156 | Alpha: 1 157 | Show Axes: false 158 | Show Trail: false 159 | Value: true 160 | LH_OUTRIGGER: 161 | Alpha: 1 162 | Show Axes: false 163 | Show Trail: false 164 | Value: true 165 | LH_ROTATOR: 166 | Alpha: 1 167 | Show Axes: false 168 | Show Trail: false 169 | Value: true 170 | LH_WHEEL: 171 | Alpha: 1 172 | Show Axes: false 173 | Show Trail: false 174 | Value: true 175 | LH_WHEEL_CONTACT: 176 | Alpha: 1 177 | Show Axes: false 178 | Show Trail: false 179 | Link Tree Style: Links in Alphabetic Order 180 | RF_BEAM: 181 | Alpha: 1 182 | Show Axes: false 183 | Show Trail: false 184 | Value: true 185 | RF_BEARING: 186 | Alpha: 1 187 | Show Axes: false 188 | Show Trail: false 189 | Value: true 190 | RF_KNUCKLE: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | Value: true 195 | RF_PARALLEL: 196 | Alpha: 1 197 | Show Axes: false 198 | Show Trail: false 199 | Value: true 200 | RF_SWIVEL: 201 | Alpha: 1 202 | Show Axes: false 203 | Show Trail: false 204 | Value: true 205 | RF_WHEEL: 206 | Alpha: 1 207 | Show Axes: false 208 | Show Trail: false 209 | Value: true 210 | RF_WHEEL_CONTACT: 211 | Alpha: 1 212 | Show Axes: false 213 | Show Trail: false 214 | RH_BEAM: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | Value: true 219 | RH_KNUCKLE: 220 | Alpha: 1 221 | Show Axes: false 222 | Show Trail: false 223 | Value: true 224 | RH_OUTRIGGER: 225 | Alpha: 1 226 | Show Axes: false 227 | Show Trail: false 228 | Value: true 229 | RH_ROTATOR: 230 | Alpha: 1 231 | Show Axes: false 232 | Show Trail: false 233 | Value: true 234 | RH_WHEEL: 235 | Alpha: 1 236 | Show Axes: false 237 | Show Trail: false 238 | Value: true 239 | RH_WHEEL_CONTACT: 240 | Alpha: 1 241 | Show Axes: false 242 | Show Trail: false 243 | ROTO: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | Value: true 248 | ROTO_BASE: 249 | Alpha: 1 250 | Show Axes: false 251 | Show Trail: false 252 | Value: true 253 | SHOVEL: 254 | Alpha: 1 255 | Show Axes: false 256 | Show Trail: false 257 | Value: true 258 | TELE: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | Value: true 263 | camMainView: 264 | Alpha: 1 265 | Show Axes: false 266 | Show Trail: false 267 | imu_box_base_link: 268 | Alpha: 1 269 | Show Axes: false 270 | Show Trail: false 271 | imu_box_link: 272 | Alpha: 1 273 | Show Axes: false 274 | Show Trail: false 275 | livox_frame: 276 | Alpha: 1 277 | Show Axes: false 278 | Show Trail: false 279 | Value: true 280 | os_imu: 281 | Alpha: 1 282 | Show Axes: false 283 | Show Trail: false 284 | os_lidar: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | os_sensor: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | roof_top_box: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | Name: ExcavatorModel 299 | Robot Description: heap_description 300 | TF Prefix: "" 301 | Update Interval: 0 302 | Value: true 303 | Visual Enabled: true 304 | - Class: rviz/TF 305 | Enabled: true 306 | Frame Timeout: 15 307 | Frames: 308 | All Enabled: false 309 | Marker Alpha: 1 310 | Marker Scale: 1 311 | Name: TF 312 | Show Arrows: false 313 | Show Axes: true 314 | Show Names: true 315 | Tree: 316 | {} 317 | Update Interval: 0 318 | Value: true 319 | - Alpha: 1 320 | Autocompute Intensity Bounds: false 321 | Autocompute Value Bounds: 322 | Max Value: 10 323 | Min Value: -10 324 | Value: true 325 | Axis: Z 326 | Channel Name: z 327 | Class: rviz/PointCloud2 328 | Color: 255; 255; 255 329 | Color Transformer: Intensity 330 | Decay Time: 0 331 | Enabled: true 332 | Invert Rainbow: false 333 | Max Color: 255; 255; 255 334 | Max Intensity: 16.27729034423828 335 | Min Color: 0; 0; 0 336 | Min Intensity: -3.2934958934783936 337 | Name: FilteredPointCloud 338 | Position Transformer: XYZ 339 | Queue Size: 10 340 | Selectable: true 341 | Size (Pixels): 2 342 | Size (m): 0.009999999776482582 343 | Style: Points 344 | Topic: /ouster_points_self_filtered 345 | Unreliable: false 346 | Use Fixed Frame: true 347 | Use rainbow: true 348 | Value: true 349 | - Alpha: 1 350 | Autocompute Intensity Bounds: true 351 | Class: grid_map_rviz_plugin/GridMap 352 | Color: 200; 200; 200 353 | Color Layer: elevation 354 | Color Transformer: GridMapLayer 355 | Enabled: true 356 | Height Layer: elevation 357 | Height Transformer: GridMapLayer 358 | History Length: 1 359 | Invert Rainbow: false 360 | Max Color: 255; 255; 255 361 | Max Intensity: 10 362 | Min Color: 0; 0; 0 363 | Min Intensity: 0 364 | Name: GridMap 365 | Show Grid Lines: true 366 | Topic: /elevation_mapping/elevation_map 367 | Unreliable: false 368 | Use Rainbow: true 369 | Value: true 370 | - Alpha: 1 371 | Buffer Length: 1 372 | Class: rviz/Path 373 | Color: 252; 233; 79 374 | Enabled: true 375 | Head Diameter: 0.30000001192092896 376 | Head Length: 0.20000000298023224 377 | Length: 0.30000001192092896 378 | Line Style: Lines 379 | Line Width: 0.029999999329447746 380 | Name: Path 381 | Offset: 382 | X: 0 383 | Y: 0 384 | Z: 0 385 | Pose Color: 255; 85; 255 386 | Pose Style: None 387 | Queue Size: 10 388 | Radius: 0.029999999329447746 389 | Shaft Diameter: 0.10000000149011612 390 | Shaft Length: 0.10000000149011612 391 | Topic: /graph_msf/meas_path_map_gnssL 392 | Unreliable: false 393 | Value: true 394 | - Alpha: 1 395 | Buffer Length: 1 396 | Class: rviz/Path 397 | Color: 252; 233; 79 398 | Enabled: true 399 | Head Diameter: 0.30000001192092896 400 | Head Length: 0.20000000298023224 401 | Length: 0.30000001192092896 402 | Line Style: Lines 403 | Line Width: 0.029999999329447746 404 | Name: Path 405 | Offset: 406 | X: 0 407 | Y: 0 408 | Z: 0 409 | Pose Color: 255; 85; 255 410 | Pose Style: None 411 | Queue Size: 10 412 | Radius: 0.029999999329447746 413 | Shaft Diameter: 0.10000000149011612 414 | Shaft Length: 0.10000000149011612 415 | Topic: /graph_msf/meas_path_map_gnssR 416 | Unreliable: false 417 | Value: true 418 | - Alpha: 1 419 | Buffer Length: 1 420 | Class: rviz/Path 421 | Color: 25; 255; 0 422 | Enabled: true 423 | Head Diameter: 0.30000001192092896 424 | Head Length: 0.20000000298023224 425 | Length: 0.30000001192092896 426 | Line Style: Lines 427 | Line Width: 0.029999999329447746 428 | Name: Path 429 | Offset: 430 | X: 0 431 | Y: 0 432 | Z: 0 433 | Pose Color: 255; 85; 255 434 | Pose Style: None 435 | Queue Size: 10 436 | Radius: 0.029999999329447746 437 | Shaft Diameter: 0.10000000149011612 438 | Shaft Length: 0.10000000149011612 439 | Topic: /graph_msf/meas_path_map_Lidar 440 | Unreliable: false 441 | Value: true 442 | - Alpha: 1 443 | Buffer Length: 1 444 | Class: rviz/Path 445 | Color: 25; 255; 0 446 | Enabled: true 447 | Head Diameter: 0.30000001192092896 448 | Head Length: 0.20000000298023224 449 | Length: 0.30000001192092896 450 | Line Style: Lines 451 | Line Width: 0.029999999329447746 452 | Name: Path 453 | Offset: 454 | X: 0 455 | Y: 0 456 | Z: 0 457 | Pose Color: 255; 85; 255 458 | Pose Style: None 459 | Queue Size: 10 460 | Radius: 0.029999999329447746 461 | Shaft Diameter: 0.10000000149011612 462 | Shaft Length: 0.10000000149011612 463 | Topic: /graph_msf/est_path_map_imu 464 | Unreliable: false 465 | Value: true 466 | - Alpha: 1 467 | Buffer Length: 1 468 | Class: rviz/Path 469 | Color: 52; 101; 164 470 | Enabled: true 471 | Head Diameter: 0.30000001192092896 472 | Head Length: 0.20000000298023224 473 | Length: 0.30000001192092896 474 | Line Style: Lines 475 | Line Width: 0.029999999329447746 476 | Name: Path 477 | Offset: 478 | X: 0 479 | Y: 0 480 | Z: 0 481 | Pose Color: 255; 85; 255 482 | Pose Style: None 483 | Queue Size: 10 484 | Radius: 0.029999999329447746 485 | Shaft Diameter: 0.10000000149011612 486 | Shaft Length: 0.10000000149011612 487 | Topic: /graph_msf/est_path_odom_imu 488 | Unreliable: false 489 | Value: true 490 | - Alpha: 1 491 | Autocompute Intensity Bounds: true 492 | Autocompute Value Bounds: 493 | Max Value: 10 494 | Min Value: -10 495 | Value: true 496 | Axis: Z 497 | Channel Name: intensity 498 | Class: rviz/PointCloud2 499 | Color: 255; 255; 255 500 | Color Transformer: Intensity 501 | Decay Time: 0 502 | Enabled: true 503 | Invert Rainbow: false 504 | Max Color: 255; 255; 255 505 | Min Color: 0; 0; 0 506 | Name: PointCloud2 507 | Position Transformer: XYZ 508 | Queue Size: 10 509 | Selectable: true 510 | Size (Pixels): 1 511 | Size (m): 0.009999999776482582 512 | Style: Points 513 | Topic: /compslam_lio/laser_map 514 | Unreliable: false 515 | Use Fixed Frame: true 516 | Use rainbow: true 517 | Value: true 518 | - Alpha: 1 519 | Autocompute Intensity Bounds: true 520 | Autocompute Value Bounds: 521 | Max Value: 10 522 | Min Value: -10 523 | Value: true 524 | Axis: Z 525 | Channel Name: intensity 526 | Class: rviz/PointCloud2 527 | Color: 255; 255; 255 528 | Color Transformer: FlatColor 529 | Decay Time: 0 530 | Enabled: true 531 | Invert Rainbow: false 532 | Max Color: 255; 255; 255 533 | Min Color: 0; 0; 0 534 | Name: PointCloud2 535 | Position Transformer: XYZ 536 | Queue Size: 10 537 | Selectable: true 538 | Size (Pixels): 2 539 | Size (m): 0.009999999776482582 540 | Style: Points 541 | Topic: /compslam_lio/registered_cloud 542 | Unreliable: false 543 | Use Fixed Frame: true 544 | Use rainbow: true 545 | Value: true 546 | Enabled: true 547 | Global Options: 548 | Background Color: 0; 0; 0 549 | Default Light: true 550 | Fixed Frame: odom 551 | Frame Rate: 30 552 | Name: root 553 | Tools: 554 | - Class: rviz/Interact 555 | Hide Inactive Objects: true 556 | - Class: rviz/MoveCamera 557 | - Class: rviz/Select 558 | - Class: rviz/FocusCamera 559 | - Class: rviz/Measure 560 | - Class: rviz/SetInitialPose 561 | Theta std deviation: 0.2617993950843811 562 | Topic: /initialpose 563 | X std deviation: 0.5 564 | Y std deviation: 0.5 565 | - Class: rviz/SetGoal 566 | Topic: /move_base_simple/goal 567 | - Class: rviz/PublishPoint 568 | Single click: true 569 | Topic: /clicked_point 570 | Value: true 571 | Views: 572 | Current: 573 | Class: rviz/Orbit 574 | Distance: 15.508606910705566 575 | Enable Stereo Rendering: 576 | Stereo Eye Separation: 0.05999999865889549 577 | Stereo Focal Distance: 1 578 | Swap Stereo Eyes: false 579 | Value: false 580 | Field of View: 0.7853981852531433 581 | Focal Point: 582 | X: 0 583 | Y: 0 584 | Z: 0 585 | Focal Shape Fixed Size: false 586 | Focal Shape Size: 0.05000000074505806 587 | Invert Z Axis: false 588 | Name: Current View 589 | Near Clip Distance: 0.009999999776482582 590 | Pitch: 0.7853981852531433 591 | Target Frame: odom 592 | Yaw: 0.7853981852531433 593 | Saved: ~ 594 | Window Geometry: 595 | Displays: 596 | collapsed: false 597 | Height: 1016 598 | Hide Left Dock: false 599 | Hide Right Dock: true 600 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001020000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000064f0000003efc0100000002fb0000000800540069006d006500000000000000064f0000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 601 | Selection: 602 | collapsed: false 603 | Time: 604 | collapsed: false 605 | Tool Properties: 606 | collapsed: false 607 | Views: 608 | collapsed: true 609 | Width: 1848 610 | X: 72 611 | Y: 27 612 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/src/dual_graph_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // ROS 9 | #include 10 | // Local packages 11 | #include "excavator_dual_graph/ExcavatorEstimator.h" 12 | 13 | // Main node entry point 14 | int main(int argc, char** argv) { 15 | // ROS related 16 | ros::init(argc, argv, "laserOdometry"); 17 | std::shared_ptr privateNodePtr = std::make_shared("~"); 18 | /// Do multi-threaded spinner 19 | ros::MultiThreadedSpinner spinner(4); 20 | 21 | // Create Instance 22 | excavator_se::ExcavatorEstimator excavatorEstimator(privateNodePtr); 23 | spinner.spin(); 24 | 25 | return 0; 26 | } -------------------------------------------------------------------------------- /examples/excavator_dual_graph/src/lib/ExcavatorEstimator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Implementation 9 | #include "excavator_dual_graph/ExcavatorEstimator.h" 10 | 11 | // Project 12 | #include "excavator_dual_graph/ExcavatorStaticTransforms.h" 13 | 14 | // Workspace 15 | #include "graph_msf/measurements/BinaryMeasurement6D.h" 16 | #include "graph_msf/measurements/UnaryMeasurement1D.h" 17 | #include "graph_msf/measurements/UnaryMeasurement3D.h" 18 | #include "graph_msf/measurements/UnaryMeasurement6D.h" 19 | #include "graph_msf_ros/util/conversions.h" 20 | 21 | namespace excavator_se { 22 | 23 | ExcavatorEstimator::ExcavatorEstimator(std::shared_ptr privateNodePtr) : privateNode_(*privateNodePtr) { 24 | std::cout << YELLOW_START << "ExcavatorEstimator" << GREEN_START << " Setting up." << COLOR_END << std::endl; 25 | 26 | // Configurations ---------------------------- 27 | graphConfigPtr_ = std::make_shared(); 28 | staticTransformsPtr_ = std::make_shared(privateNodePtr); 29 | gnssHandlerPtr_ = std::make_shared(); 30 | 31 | // Get ROS params and set extrinsics 32 | readParams_(privateNode_); 33 | staticTransformsPtr_->findTransformations(); 34 | 35 | if (not graph_msf::GraphMsfInterface::setup_()) { 36 | throw std::runtime_error("CompslamSeInterface could not be initiallized"); 37 | } 38 | 39 | // Publishers ---------------------------- 40 | initializePublishers_(privateNodePtr); 41 | 42 | // Subscribers ---------------------------- 43 | initializeSubscribers_(privateNodePtr); 44 | 45 | // Messages ---------------------------- 46 | initializeMessages_(privateNodePtr); 47 | 48 | std::cout << YELLOW_START << "CompslamEstimator" << GREEN_START << " Set up successfully." << COLOR_END << std::endl; 49 | } 50 | 51 | void ExcavatorEstimator::initializePublishers_(std::shared_ptr& privateNodePtr) { 52 | // Odometry 53 | pubEstOdomImu_ = privateNode_.advertise("/graph_msf/odometry_odom_imu", ROS_QUEUE_SIZE); 54 | pubEstMapImu_ = privateNode_.advertise("/graph_msf/odometry_map_imu", ROS_QUEUE_SIZE); 55 | // Paths 56 | pubEstOdomImuPath_ = privateNode_.advertise("/graph_msf/est_path_odom_imu", ROS_QUEUE_SIZE); 57 | pubEstMapImuPath_ = privateNode_.advertise("/graph_msf/est_path_map_imu", ROS_QUEUE_SIZE); 58 | pubMeasMapGnssLPath_ = privateNode_.advertise("/graph_msf/meas_path_map_gnssL", ROS_QUEUE_SIZE); 59 | pubMeasMapGnssRPath_ = privateNode_.advertise("/graph_msf/meas_path_map_gnssR", ROS_QUEUE_SIZE); 60 | pubMeasMapLidarPath_ = privateNode_.advertise("/graph_msf/meas_path_map_Lidar", ROS_QUEUE_SIZE); 61 | } 62 | 63 | void ExcavatorEstimator::initializeSubscribers_(std::shared_ptr& privateNodePtr) { 64 | // Imu 65 | subImu_ = privateNode_.subscribe("/imu_topic", ROS_QUEUE_SIZE, &ExcavatorEstimator::imuCallback_, this, 66 | ros::TransportHints().tcpNoDelay()); 67 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END << " Initialized IMU cabin subscriber." << std::endl; 68 | 69 | // LiDAR Odometry 70 | subLidarOdometry_ = privateNode_.subscribe( 71 | "/lidar_odometry_topic", ROS_QUEUE_SIZE, &ExcavatorEstimator::lidarOdometryCallback_, this, ros::TransportHints().tcpNoDelay()); 72 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END << " Initialized LiDAR Odometry subscriber." << std::endl; 73 | 74 | // GNSS 75 | if (graphConfigPtr_->usingGnssFlag) { 76 | subGnssLeft_.subscribe(*privateNodePtr, "/gnss_topic_1", ROS_QUEUE_SIZE); 77 | subGnssRight_.subscribe(*privateNodePtr, "/gnss_topic_2", ROS_QUEUE_SIZE); 78 | gnssExactSyncPtr_.reset( 79 | new message_filters::Synchronizer(GnssExactSyncPolicy(ROS_QUEUE_SIZE), subGnssLeft_, subGnssRight_)); 80 | gnssExactSyncPtr_->registerCallback(boost::bind(&ExcavatorEstimator::gnssCallback_, this, _1, _2)); 81 | std::cout << YELLOW_START << "FactorGraphFiltering" << COLOR_END << " Initialized Gnss subscriber (for both Gnss topics)." << std::endl; 82 | } else { 83 | std::cout << YELLOW_START << "CompslamEstimator" << GREEN_START 84 | << " Gnss usage is set to false. Hence, lidar unary factors will be activated after graph initialization." << COLOR_END 85 | << std::endl; 86 | } 87 | } 88 | 89 | void ExcavatorEstimator::initializeMessages_(std::shared_ptr& privateNodePtr) { 90 | // Odometry 91 | odomImuMsgPtr_ = nav_msgs::OdometryPtr(new nav_msgs::Odometry); 92 | mapImuMsgPtr_ = nav_msgs::OdometryPtr(new nav_msgs::Odometry); 93 | // Path 94 | estOdomImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); 95 | estMapImuPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); 96 | measMapLeftGnssPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); 97 | measMapRightGnssPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); 98 | measMapLidarPathPtr_ = nav_msgs::PathPtr(new nav_msgs::Path); 99 | } 100 | 101 | void ExcavatorEstimator::imuCallback_(const sensor_msgs::Imu::ConstPtr& imuMsgPtr) { 102 | static bool firstCallbackFlag__ = true; 103 | if (firstCallbackFlag__) { 104 | firstCallbackFlag__ = false; 105 | } 106 | Eigen::Vector3d linearAcc(imuMsgPtr->linear_acceleration.x, imuMsgPtr->linear_acceleration.y, imuMsgPtr->linear_acceleration.z); 107 | Eigen::Vector3d angularVel(imuMsgPtr->angular_velocity.x, imuMsgPtr->angular_velocity.y, imuMsgPtr->angular_velocity.z); 108 | graph_msf::GraphMsfInterface::addImuMeasurementAndPublishState_(linearAcc, angularVel, imuMsgPtr->header.stamp.toSec()); 109 | } 110 | 111 | void ExcavatorEstimator::lidarOdometryCallback_(const nav_msgs::Odometry::ConstPtr& odomLidarPtr) { 112 | // Static members 113 | static int odometryCallbackCounter__ = -1; 114 | static std::shared_ptr odometryKm1Ptr__; 115 | 116 | // Counter 117 | ++odometryCallbackCounter__; 118 | 119 | Eigen::Matrix4d compslam_T_Wl_Lk; 120 | graph_msf::odomMsgToEigen(*odomLidarPtr, compslam_T_Wl_Lk); 121 | 122 | // Transform to IMU frame 123 | double timeK = odomLidarPtr->header.stamp.toSec(); 124 | 125 | // Measurement 126 | std::shared_ptr odometryKPtr; 127 | // Create pseudo unary factors 128 | if (graphConfigPtr_->usingGnssFlag) { 129 | odometryKPtr = std::make_unique( 130 | "Lidar 6D", dynamic_cast(staticTransformsPtr_.get())->getLidarFrame(), int(lidarRate_), timeK, 131 | compslam_T_Wl_Lk, poseUnaryNoise_); 132 | if (odometryCallbackCounter__ > 0) { 133 | graph_msf::GraphMsfInterface::addDualOdometryMeasurement_(*odometryKm1Ptr__, *odometryKPtr, poseBetweenNoise_); 134 | } 135 | } else { // real unary factors 136 | odometryKPtr = std::make_unique( 137 | "Lidar 6D", dynamic_cast(staticTransformsPtr_.get())->getLidarFrame(), int(lidarRate_), timeK, 138 | compslam_T_Wl_Lk, poseUnaryNoise_); 139 | graph_msf::GraphMsfInterface::addUnaryPoseMeasurement_(*odometryKPtr); 140 | } 141 | 142 | if (!areYawAndPositionInited_() && (!graphConfigPtr_->usingGnssFlag || secondsSinceStart_() > 15)) { 143 | std::cout << YELLOW_START << "ExcavatorEstimator" << GREEN_START 144 | << " LiDAR odometry callback is setting global cabin yaw to 0, as it was not set so far." << COLOR_END << std::endl; 145 | graph_msf::GraphMsfInterface::initYawAndPosition_( 146 | compslam_T_Wl_Lk, dynamic_cast(staticTransformsPtr_.get())->getLidarFrame()); 147 | } 148 | 149 | // Wrap up iteration 150 | odometryKm1Ptr__ = odometryKPtr; 151 | 152 | // Add to path message 153 | addToPathMsg(measMapLidarPathPtr_, dynamic_cast(staticTransformsPtr_.get())->getMapFrame(), 154 | odomLidarPtr->header.stamp, compslam_T_Wl_Lk.block<3, 1>(0, 3), graphConfigPtr_->imuBufferLength * 4); 155 | 156 | // Publish Path 157 | pubMeasMapLidarPath_.publish(measMapLidarPathPtr_); 158 | } 159 | 160 | void ExcavatorEstimator::gnssCallback_(const sensor_msgs::NavSatFix::ConstPtr& leftGnssMsgPtr, 161 | const sensor_msgs::NavSatFix::ConstPtr& rightGnssMsgPtr) { 162 | // Static method variables 163 | static Eigen::Vector3d accumulatedLeftCoordinates__(0.0, 0.0, 0.0); 164 | static Eigen::Vector3d accumulatedRightCoordinates__(0.0, 0.0, 0.0); 165 | static Eigen::Vector3d W_t_W_GnssL_km1__, W_t_W_GnssR_km1__; 166 | static int gnssCallbackCounter__ = 0; 167 | 168 | // Start 169 | ++gnssCallbackCounter__; 170 | Eigen::Vector3d leftGnssCoord = Eigen::Vector3d(leftGnssMsgPtr->latitude, leftGnssMsgPtr->longitude, leftGnssMsgPtr->altitude); 171 | Eigen::Vector3d rightGnssCoord = Eigen::Vector3d(rightGnssMsgPtr->latitude, rightGnssMsgPtr->longitude, rightGnssMsgPtr->altitude); 172 | Eigen::Vector3d estCovarianceXYZ(leftGnssMsgPtr->position_covariance[0], leftGnssMsgPtr->position_covariance[4], 173 | leftGnssMsgPtr->position_covariance[8]); 174 | if (!graphConfigPtr_->usingGnssFlag) { 175 | ROS_WARN("Received Gnss message, but usage is set to false."); 176 | graph_msf::GraphMsfInterface::activateFallbackGraph(); 177 | return; 178 | } else if (gnssCallbackCounter__ < NUM_GNSS_CALLBACKS_UNTIL_START + 1) { 179 | // Wait until measurements got accumulated 180 | accumulatedLeftCoordinates__ += leftGnssCoord; 181 | accumulatedRightCoordinates__ += rightGnssCoord; 182 | if (!(gnssCallbackCounter__ % 10)) { 183 | std::cout << YELLOW_START << "ExcavatorEstimator" << COLOR_END << " NOT ENOUGH Gnss MESSAGES ARRIVED!" << std::endl; 184 | } 185 | return; 186 | } else if (gnssCallbackCounter__ == NUM_GNSS_CALLBACKS_UNTIL_START + 1) { 187 | gnssHandlerPtr_->initHandler(accumulatedLeftCoordinates__ / NUM_GNSS_CALLBACKS_UNTIL_START, 188 | accumulatedRightCoordinates__ / NUM_GNSS_CALLBACKS_UNTIL_START); 189 | } 190 | 191 | // Convert to cartesian coordinates 192 | Eigen::Vector3d W_t_W_GnssL, W_t_W_GnssR; 193 | gnssHandlerPtr_->convertNavSatToPositions(leftGnssCoord, rightGnssCoord, W_t_W_GnssL, W_t_W_GnssR); 194 | double yaw_W_C = gnssHandlerPtr_->computeYaw(W_t_W_GnssL, W_t_W_GnssR); 195 | 196 | // Initialization 197 | if (gnssCallbackCounter__ == NUM_GNSS_CALLBACKS_UNTIL_START + 1) { 198 | if (not graph_msf::GraphMsfInterface::initYawAndPosition_( 199 | yaw_W_C, dynamic_cast(staticTransformsPtr_.get())->getCabinFrame(), W_t_W_GnssL, 200 | dynamic_cast(staticTransformsPtr_.get())->getLeftGnssFrame())) { 201 | // Decrease counter if not successfully initialized 202 | --gnssCallbackCounter__; 203 | } 204 | } else { 205 | graph_msf::UnaryMeasurement1D meas_yaw_W_C("Gnss yaw", 206 | dynamic_cast(staticTransformsPtr_.get())->getCabinFrame(), 207 | int(gnssLeftRate_), leftGnssMsgPtr->header.stamp.toSec(), yaw_W_C, gnssHeadingUnaryNoise_); 208 | graph_msf::GraphMsfInterface::addGnssHeadingMeasurement_(meas_yaw_W_C); 209 | graph_msf::UnaryMeasurement3D meas_W_t_W_GnssL( 210 | "Gnss left", dynamic_cast(staticTransformsPtr_.get())->getLeftGnssFrame(), gnssLeftRate_, 211 | leftGnssMsgPtr->header.stamp.toSec(), W_t_W_GnssL, 212 | Eigen::Vector3d(gnssPositionUnaryNoise_, gnssPositionUnaryNoise_, gnssPositionUnaryNoise_)); 213 | graph_msf::GraphMsfInterface::addDualGnssPositionMeasurement_(meas_W_t_W_GnssL, W_t_W_GnssL_km1__, estCovarianceXYZ); 214 | graph_msf::UnaryMeasurement3D meas_W_t_W_GnssR( 215 | "Gnss right", dynamic_cast(staticTransformsPtr_.get())->getRightGnssFrame(), gnssRightRate_, 216 | leftGnssMsgPtr->header.stamp.toSec(), W_t_W_GnssR, 217 | Eigen::Vector3d(gnssPositionUnaryNoise_, gnssPositionUnaryNoise_, gnssPositionUnaryNoise_)); 218 | graph_msf::GraphMsfInterface::addDualGnssPositionMeasurement_(meas_W_t_W_GnssR, W_t_W_GnssR_km1__, estCovarianceXYZ); 219 | } 220 | W_t_W_GnssL_km1__ = W_t_W_GnssL; 221 | W_t_W_GnssR_km1__ = W_t_W_GnssR; 222 | 223 | // Left GNSS 224 | /// Add to Path 225 | addToPathMsg(measMapLeftGnssPathPtr_, dynamic_cast(staticTransformsPtr_.get())->getMapFrame(), 226 | leftGnssMsgPtr->header.stamp, W_t_W_GnssL, graphConfigPtr_->imuBufferLength * 4); 227 | /// Publish path 228 | pubMeasMapGnssLPath_.publish(measMapLeftGnssPathPtr_); 229 | // Right GNSS 230 | /// Add to path 231 | addToPathMsg(measMapRightGnssPathPtr_, dynamic_cast(staticTransformsPtr_.get())->getMapFrame(), 232 | rightGnssMsgPtr->header.stamp, W_t_W_GnssR, graphConfigPtr_->imuBufferLength * 4); 233 | /// Publish path 234 | pubMeasMapGnssRPath_.publish(measMapRightGnssPathPtr_); 235 | } 236 | 237 | void ExcavatorEstimator::publishState_(const double imuTimeK, const Eigen::Matrix4d& T_W_O, const Eigen::Matrix4d& T_O_Ik, 238 | const Eigen::Vector3d& Ic_v_W_Ic, const Eigen::Vector3d& I_w_W_I) { 239 | // Used transforms 240 | Eigen::Matrix4d T_I_L = staticTransformsPtr_ 241 | ->rv_T_frame1_frame2(staticTransformsPtr_.get()->getImuFrame(), 242 | dynamic_cast(staticTransformsPtr_.get())->getLidarFrame()) 243 | .inverse(); 244 | 245 | // Pose 246 | Eigen::Matrix4d T_O_L = T_O_Ik * T_I_L; 247 | Eigen::Matrix4d T_W_L = T_W_O * T_O_L; 248 | 249 | // Publish odometry message for odom->imu with 100 Hz 250 | addToOdometryMsg(odomImuMsgPtr_, dynamic_cast(staticTransformsPtr_.get())->getOdomFrame(), 251 | dynamic_cast(staticTransformsPtr_.get())->getImuFrame(), ros::Time(imuTimeK), T_O_Ik, 252 | Ic_v_W_Ic, I_w_W_I); 253 | pubEstOdomImu_.publish(odomImuMsgPtr_); 254 | addToPathMsg(estOdomImuPathPtr_, dynamic_cast(staticTransformsPtr_.get())->getOdomFrame(), 255 | ros::Time(imuTimeK), T_O_Ik.block<3, 1>(0, 3), graphConfigPtr_->imuBufferLength * 20); 256 | pubEstOdomImuPath_.publish(estOdomImuPathPtr_); 257 | // Publish odometry message for map->imu with 100 Hz 258 | addToOdometryMsg(mapImuMsgPtr_, dynamic_cast(staticTransformsPtr_.get())->getMapFrame(), 259 | dynamic_cast(staticTransformsPtr_.get())->getImuFrame(), ros::Time(imuTimeK), T_W_O * T_O_Ik, 260 | Ic_v_W_Ic, I_w_W_I); 261 | pubEstOdomImu_.publish(mapImuMsgPtr_); 262 | addToPathMsg(estMapImuPathPtr_, dynamic_cast(staticTransformsPtr_.get())->getMapFrame(), ros::Time(imuTimeK), 263 | (T_W_O * T_O_Ik).block<3, 1>(0, 3), graphConfigPtr_->imuBufferLength * 20); 264 | pubEstMapImuPath_.publish(estMapImuPathPtr_); 265 | 266 | // Convert Measurements 267 | 268 | // Publish to TF 269 | // W_O 270 | static tf::Transform transform_W_O; 271 | transform_W_O.setOrigin(tf::Vector3(T_W_O(0, 3), T_W_O(1, 3), T_W_O(2, 3))); 272 | Eigen::Quaterniond q_W_O(T_W_O.block<3, 3>(0, 0)); 273 | transform_W_O.setRotation(tf::Quaternion(q_W_O.x(), q_W_O.y(), q_W_O.z(), q_W_O.w())); 274 | tfBroadcaster_.sendTransform(tf::StampedTransform(transform_W_O, ros::Time(imuTimeK), 275 | dynamic_cast(staticTransformsPtr_.get())->getMapFrame(), 276 | dynamic_cast(staticTransformsPtr_.get())->getOdomFrame())); 277 | // I->B 278 | static tf::StampedTransform transform_I_B; 279 | tfListener_.waitForTransform(staticTransformsPtr_->getImuFrame(), 280 | dynamic_cast(staticTransformsPtr_.get())->getBaseLinkFrame(), ros::Time(0), 281 | ros::Duration(0.1)); 282 | listener_.lookupTransform(staticTransformsPtr_->getImuFrame(), 283 | dynamic_cast(staticTransformsPtr_.get())->getBaseLinkFrame(), ros::Time(0), 284 | transform_I_B); 285 | 286 | // Publish T_O_B 287 | static tf::Transform transform_O_I; 288 | transform_O_I.setOrigin(tf::Vector3(T_O_Ik(0, 3), T_O_Ik(1, 3), T_O_Ik(2, 3))); 289 | Eigen::Quaterniond q_O_I(T_O_Ik.block<3, 3>(0, 0)); 290 | transform_O_I.setRotation(tf::Quaternion(q_O_I.x(), q_O_I.y(), q_O_I.z(), q_O_I.w())); 291 | tfBroadcaster_.sendTransform( 292 | tf::StampedTransform(transform_O_I * transform_I_B, ros::Time(imuTimeK), 293 | dynamic_cast(staticTransformsPtr_.get())->getOdomFrame(), 294 | dynamic_cast(staticTransformsPtr_.get())->getBaseLinkFrame())); 295 | } 296 | 297 | } // namespace excavator_se -------------------------------------------------------------------------------- /examples/excavator_dual_graph/src/lib/ExcavatorStaticTransforms.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Implementation 9 | #include "excavator_dual_graph/ExcavatorStaticTransforms.h" 10 | 11 | // ROS 12 | #include 13 | #include 14 | 15 | // Workspace 16 | #include "graph_msf_ros/util/conversions.h" 17 | 18 | namespace excavator_se { 19 | 20 | ExcavatorStaticTransforms::ExcavatorStaticTransforms(std::shared_ptr privateNodePtr) 21 | : graph_msf::StaticTransformsTf(*privateNodePtr) { 22 | std::cout << YELLOW_START << "StaticTransformsTf" << GREEN_START << " Initializing static transforms..." << COLOR_END << std::endl; 23 | } 24 | 25 | void ExcavatorStaticTransforms::findTransformations() { 26 | // Print to console -------------------------- 27 | std::cout << YELLOW_START << "StaticTransformsTf" << COLOR_END << " Looking up transforms in TF-tree."; 28 | std::cout << YELLOW_START << "StaticTransformsTf" << COLOR_END << " Transforms between the following frames are required:" << std::endl; 29 | std::cout << YELLOW_START << "StaticTransformsTf" << COLOR_END << " " << lidarFrame_ << ", " << leftGnssFrame_ << ", " << rightGnssFrame_ 30 | << ", " << cabinFrame_ << ", " << imuFrame_ << ", " << baseLinkFrame_ << std::endl; 31 | std::cout << YELLOW_START << "StaticTransformsTf" << COLOR_END << " Waiting for up to 100 seconds until they arrive..." << std::endl; 32 | 33 | // Temporary variable 34 | static tf::StampedTransform transform; 35 | 36 | // Look up transforms ---------------------------- 37 | // Sleep before subscribing, otherwise sometimes dying in the beginning of rosbag 38 | ros::Rate rosRate(10); 39 | rosRate.sleep(); 40 | 41 | // Imu to Cabin Link --- 42 | listener_.waitForTransform(imuFrame_, cabinFrame_, ros::Time(0), ros::Duration(100.0)); 43 | listener_.lookupTransform(imuFrame_, cabinFrame_, ros::Time(0), transform); 44 | // I_Cabin 45 | graph_msf::tfToMatrix4(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, cabinFrame_)); 46 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END 47 | << " Translation I_Cabin: " << rv_T_frame1_frame2(imuFrame_, cabinFrame_).block<3, 1>(0, 3) << std::endl; 48 | // Cabin_I 49 | lv_T_frame1_frame2(cabinFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, cabinFrame_).inverse(); 50 | 51 | // Imu to Base Link --- 52 | listener_.waitForTransform(imuFrame_, baseLinkFrame_, ros::Time(0), ros::Duration(1.0)); 53 | listener_.lookupTransform(imuFrame_, baseLinkFrame_, ros::Time(0), transform); 54 | // I_Cabin 55 | graph_msf::tfToMatrix4(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, baseLinkFrame_)); 56 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END 57 | << " Translation I_Base: " << rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).block<3, 1>(0, 3) << std::endl; 58 | // Cabin_I 59 | lv_T_frame1_frame2(baseLinkFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, baseLinkFrame_).inverse(); 60 | 61 | // Imu to LiDAR Link --- 62 | std::cout << YELLOW_START << "StaticTransformsTf" << COLOR_END << " Waiting for transform for 10 seconds."; 63 | listener_.waitForTransform(imuFrame_, lidarFrame_, ros::Time(0), ros::Duration(1.0)); 64 | listener_.lookupTransform(imuFrame_, lidarFrame_, ros::Time(0), transform); 65 | // I_Lidar 66 | graph_msf::tfToMatrix4(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, lidarFrame_)); 67 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END 68 | << " Translation I_Lidar: " << rv_T_frame1_frame2(imuFrame_, lidarFrame_).block<3, 1>(0, 3) << std::endl; 69 | // Lidar_I 70 | lv_T_frame1_frame2(lidarFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, lidarFrame_).inverse(); 71 | 72 | // Imu to GNSS Left Link --- 73 | listener_.waitForTransform(imuFrame_, leftGnssFrame_, ros::Time(0), ros::Duration(1.0)); 74 | listener_.lookupTransform(imuFrame_, leftGnssFrame_, ros::Time(0), transform); 75 | // I_GnssL 76 | graph_msf::tfToMatrix4(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, leftGnssFrame_)); 77 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END 78 | << " Translation I_GnssL: " << rv_T_frame1_frame2(imuFrame_, leftGnssFrame_).block<3, 1>(0, 3) << std::endl; 79 | // GnssL_I 80 | lv_T_frame1_frame2(leftGnssFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, leftGnssFrame_).inverse(); 81 | 82 | // Imu to GNSS Right Link --- 83 | listener_.waitForTransform(imuFrame_, rightGnssFrame_, ros::Time(0), ros::Duration(1.0)); 84 | listener_.lookupTransform(imuFrame_, rightGnssFrame_, ros::Time(0), transform); 85 | // I_GnssR 86 | graph_msf::tfToMatrix4(tf::Transform(transform), lv_T_frame1_frame2(imuFrame_, rightGnssFrame_)); 87 | std::cout << YELLOW_START << "CompslamEstimator" << COLOR_END 88 | << " Translation I_GnssR: " << rv_T_frame1_frame2(imuFrame_, rightGnssFrame_).block<3, 1>(0, 3) << std::endl; 89 | // GnssL_I 90 | lv_T_frame1_frame2(rightGnssFrame_, imuFrame_) = rv_T_frame1_frame2(imuFrame_, rightGnssFrame_).inverse(); 91 | 92 | std::cout << YELLOW_START << "StaticTransformsTf" << GREEN_START << " Transforms looked up successfully." << COLOR_END << std::endl; 93 | } 94 | 95 | } // namespace excavator_se 96 | -------------------------------------------------------------------------------- /examples/excavator_dual_graph/src/lib/readParams.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Implementation 9 | #include "excavator_dual_graph/ExcavatorEstimator.h" 10 | 11 | namespace excavator_se { 12 | 13 | template 14 | void printKey(const std::string& key, T value) { 15 | std::cout << YELLOW_START << "ExcavatorEstimator " << COLOR_END << key << " set to: " << value << std::endl; 16 | } 17 | template <> 18 | void printKey(const std::string& key, std::vector vector) { 19 | std::cout << YELLOW_START << "ExcavatorEstimator " << COLOR_END << key << " set to: "; 20 | for (const auto& element : vector) { 21 | std::cout << element << ","; 22 | } 23 | std::cout << std::endl; 24 | } 25 | // Simon Kerscher: Implementation of Templating 26 | template 27 | T tryGetParam(const std::string& key, const ros::NodeHandle& privateNode) { 28 | T value; 29 | if (privateNode.getParam(key, value)) { 30 | printKey(key, value); 31 | return value; 32 | } else { 33 | throw std::runtime_error("ExcavatorEstimator - " + key + " not specified."); 34 | } 35 | } 36 | 37 | void ExcavatorEstimator::readParams_(const ros::NodeHandle& privateNode) { 38 | // Variables for parameter fetching 39 | double dParam; 40 | int iParam; 41 | bool bParam; 42 | std::string sParam; 43 | 44 | if (!graphConfigPtr_) { 45 | throw std::runtime_error("ExcavatorEstimator: graphConfigPtr must be initialized."); 46 | } 47 | 48 | // Configuration 49 | /// Using Gnss 50 | graphConfigPtr_->usingGnssFlag = tryGetParam("launch/usingGnss", privateNode); 51 | 52 | // Set frames 53 | /// Map 54 | std::string frame = tryGetParam("extrinsics/mapFrame", privateNode); 55 | dynamic_cast(staticTransformsPtr_.get())->setMapFrame(frame); 56 | /// Odom 57 | frame = tryGetParam("extrinsics/odomFrame", privateNode); 58 | dynamic_cast(staticTransformsPtr_.get())->setOdomFrame(frame); 59 | /// base_link 60 | frame = tryGetParam("extrinsics/baseLinkFrame", privateNode); 61 | dynamic_cast(staticTransformsPtr_.get())->setBaseLinkFrame(frame); 62 | /// IMU 63 | //// Cabin IMU 64 | frame = tryGetParam("extrinsics/imuFrame", privateNode); 65 | staticTransformsPtr_->setImuFrame(frame); 66 | /// LiDAR frame 67 | frame = tryGetParam("extrinsics/lidarFrame", privateNode); 68 | dynamic_cast(staticTransformsPtr_.get())->setLidarFrame(frame); 69 | /// Cabin frame 70 | frame = tryGetParam("extrinsics/cabinFrame", privateNode); 71 | dynamic_cast(staticTransformsPtr_.get())->setCabinFrame(frame); 72 | /// Left Gnss frame 73 | frame = tryGetParam("extrinsics/gnssFrame1", privateNode); 74 | dynamic_cast(staticTransformsPtr_.get())->setLeftGnssFrame(frame); 75 | /// Right Gnss frame 76 | frame = tryGetParam("extrinsics/gnssFrame2", privateNode); 77 | dynamic_cast(staticTransformsPtr_.get())->setRightGnssFrame(frame); 78 | 79 | // IMU gravity definition 80 | graphConfigPtr_->imuGravityDirection = tryGetParam("launch/imuGravityDirection", privateNode); 81 | 82 | // Factor Graph Parameters 83 | graphConfigPtr_->imuRate = tryGetParam("sensor_params/imuRate", privateNode); 84 | graphConfigPtr_->maxSearchDeviation = 1.0 / graphConfigPtr_->imuRate; 85 | graphConfigPtr_->imuBufferLength = tryGetParam("sensor_params/imuBufferLength", privateNode); 86 | lidarRate_ = tryGetParam("sensor_params/lidarOdometryRate", privateNode); 87 | gnssLeftRate_ = tryGetParam("sensor_params/gnssRate", privateNode); 88 | gnssRightRate_ = tryGetParam("sensor_params/gnssRate", privateNode); 89 | graphConfigPtr_->imuTimeOffset = tryGetParam("sensor_params/imuTimeOffset", privateNode); 90 | graphConfigPtr_->useIsam = tryGetParam("graph_params/useIsam", privateNode); 91 | graphConfigPtr_->smootherLag = tryGetParam("graph_params/smootherLag", privateNode); 92 | 93 | // TODO --> replace 94 | graphConfigPtr_->additionalIterations = tryGetParam("graph_params/additionalOptimizationIterations", privateNode); 95 | graphConfigPtr_->findUnusedFactorSlots = tryGetParam("graph_params/findUnusedFactorSlots", privateNode); 96 | graphConfigPtr_->enableDetailedResults = tryGetParam("graph_params/enableDetailedResults", privateNode); 97 | graphConfigPtr_->usingFallbackGraphFlag = tryGetParam("graph_params/usingFallbackGraph", privateNode); 98 | 99 | // Outlier Parameters 100 | graphConfigPtr_->gnssOutlierThresold = tryGetParam("outlier_params/gnssOutlierThreshold", privateNode); 101 | 102 | // Noise Parameters 103 | /// Accelerometer 104 | 105 | graphConfigPtr_->accNoiseDensity = tryGetParam("noise_params/accNoiseDensity", privateNode); 106 | graphConfigPtr_->accBiasRandomWalk = tryGetParam("noise_params/accBiasRandomWalk", privateNode); 107 | { 108 | const double accBiasPrior = tryGetParam("noise_params/accBiasPrior", privateNode); 109 | graphConfigPtr_->accBiasPrior = Eigen::Vector3d(accBiasPrior, accBiasPrior, accBiasPrior); 110 | } 111 | /// Gyro 112 | graphConfigPtr_->gyroNoiseDensity = tryGetParam("noise_params/gyrNoiseDensity", privateNode); 113 | graphConfigPtr_->gyroBiasRandomWalk = tryGetParam("noise_params/gyrBiasRandomWalk", privateNode); 114 | { 115 | const double gyroBiasPrior = tryGetParam("noise_params/gyrBiasPrior", privateNode); 116 | graphConfigPtr_->gyroBiasPrior = Eigen::Vector3d(gyroBiasPrior, gyroBiasPrior, gyroBiasPrior); 117 | } 118 | /// Preintegration 119 | graphConfigPtr_->integrationNoiseDensity = tryGetParam("noise_params/integrationNoiseDensity", privateNode); 120 | graphConfigPtr_->biasAccOmegaPreint = tryGetParam("noise_params/biasAccOmegaPreInt", privateNode); 121 | /// LiDAR Odometry 122 | { 123 | const auto poseBetweenNoise = tryGetParam>("noise_params/poseBetweenNoise", privateNode); // roll,pitch,yaw,x,y,z 124 | poseBetweenNoise_ << poseBetweenNoise[0], poseBetweenNoise[1], poseBetweenNoise[2], poseBetweenNoise[3], poseBetweenNoise[4], 125 | poseBetweenNoise[5]; 126 | } 127 | { 128 | const auto poseUnaryNoise = tryGetParam>("noise_params/poseUnaryNoise", privateNode); // roll,pitch,yaw,x,y,z 129 | poseUnaryNoise_ << poseUnaryNoise[0], poseUnaryNoise[1], poseUnaryNoise[2], poseUnaryNoise[3], poseUnaryNoise[4], poseUnaryNoise[5]; 130 | } 131 | /// Gnss 132 | gnssPositionUnaryNoise_ = tryGetParam("noise_params/gnssPositionUnaryNoise", privateNode); 133 | gnssHeadingUnaryNoise_ = tryGetParam("noise_params/gnssHeadingUnaryNoise", privateNode); 134 | 135 | // Relinearization 136 | graphConfigPtr_->positionReLinTh = tryGetParam("relinearization_params/positionReLinTh", privateNode); 137 | graphConfigPtr_->rotationReLinTh = tryGetParam("relinearization_params/rotationReLinTh", privateNode); 138 | graphConfigPtr_->velocityReLinTh = tryGetParam("relinearization_params/velocityReLinTh", privateNode); 139 | graphConfigPtr_->accBiasReLinTh = tryGetParam("relinearization_params/accBiasReLinTh", privateNode); 140 | graphConfigPtr_->gyroBiasReLinTh = tryGetParam("relinearization_params/gyrBiasReLinTh", privateNode); 141 | graphConfigPtr_->relinearizeSkip = tryGetParam("relinearization_params/relinearizeSkip", privateNode); 142 | graphConfigPtr_->enableRelinearization = tryGetParam("relinearization_params/enableRelinearization", privateNode); 143 | graphConfigPtr_->evaluateNonlinearError = tryGetParam("relinearization_params/evaluateNonlinearError", privateNode); 144 | graphConfigPtr_->cacheLinearizedFactors = tryGetParam("relinearization_params/cacheLinearizedFactors", privateNode); 145 | graphConfigPtr_->enablePartialRelinearizationCheck = 146 | tryGetParam("relinearization_params/enablePartialRelinearizationCheck", privateNode); 147 | 148 | // Common Parameters 149 | verboseLevel_ = tryGetParam("common_params/verbosity", privateNode); 150 | graphConfigPtr_->verboseLevel = verboseLevel_; 151 | 152 | if (graphConfigPtr_->usingGnssFlag) { 153 | // Gnss parameters 154 | gnssHandlerPtr_->usingGnssReferenceFlag = tryGetParam("gnss/useGnssReference", privateNode); 155 | { 156 | const double referenceLatitude = tryGetParam("gnss/referenceLatitude", privateNode); 157 | gnssHandlerPtr_->setGnssReferenceLatitude(referenceLatitude); 158 | } 159 | { 160 | const double referenceLongitude = tryGetParam("gnss/referenceLongitude", privateNode); 161 | gnssHandlerPtr_->setGnssReferenceLongitude(referenceLongitude); 162 | } 163 | { 164 | const double referenceAltitude = tryGetParam("gnss/referenceAltitude", privateNode); 165 | gnssHandlerPtr_->setGnssReferenceAltitude(referenceAltitude); 166 | } 167 | { 168 | const double referenceHeading = tryGetParam("gnss/referenceHeading", privateNode); 169 | gnssHandlerPtr_->setGnssReferenceHeading(referenceHeading); 170 | } 171 | } 172 | } 173 | 174 | } // namespace excavator_se 175 | -------------------------------------------------------------------------------- /graph_msf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(graph_msf) 3 | 4 | set(CATKIN_PACKAGE_DEPENDENCIES 5 | ) 6 | 7 | find_package(catkin REQUIRED COMPONENTS 8 | ${CATKIN_PACKAGE_DEPENDENCIES} 9 | ) 10 | 11 | find_package(Eigen3 REQUIRED) 12 | message("Eigen Version:" ${EIGEN3_VERSION_STRING}) 13 | message("Eigen Path:" ${Eigen3_DIR}) 14 | 15 | find_package(GTSAM REQUIRED) 16 | message("GTSAM version: ${GTSAM_VERSION}") 17 | message("GTSAM path: ${GTSAM_DIR}") 18 | 19 | #generate_messages( 20 | # DEPENDENCIES 21 | # std_msgs 22 | #) 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} 26 | DEPENDS 27 | INCLUDE_DIRS include 28 | LIBRARIES ${PROJECT_NAME} 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | set(CMAKE_CXX_STANDARD 14) 35 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 36 | 37 | include_directories( 38 | include 39 | ${catkin_INCLUDE_DIRS} 40 | ${GTSAM_INCLUDE_DIR} 41 | ${Python3_INCLUDE_DIRS} 42 | ) 43 | 44 | add_library(${PROJECT_NAME} 45 | src/lib/GraphMsf.cpp 46 | src/lib/GraphMsfInterface.cpp 47 | src/lib/GraphManager.cpp 48 | src/lib/ImuBuffer.cpp 49 | src/lib/Gnss.cpp 50 | src/lib/GnssHandler.cpp) 51 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} gtsam gtsam_unstable) 52 | 53 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 54 | 55 | # Add clang tooling 56 | find_package(cmake_clang_tools QUIET) 57 | if(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) 58 | add_clang_tooling( 59 | TARGET ${PROJECT_NAME} 60 | SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include 61 | CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include 62 | CF_FIX 63 | ) 64 | endif(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) 65 | 66 | ############# 67 | ## Install ## 68 | ############# 69 | 70 | install(TARGETS ${PROJECT_NAME} 71 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 72 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 73 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 74 | ) 75 | 76 | install(DIRECTORY include/${PROJECT_NAME}/ 77 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 78 | ) 79 | 80 | ## Mark cpp header files for installation 81 | install(DIRECTORY config 82 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 83 | ) 84 | -------------------------------------------------------------------------------- /graph_msf/config/gnss_params.yaml: -------------------------------------------------------------------------------- 1 | gnss: 2 | useGnssReference: true 3 | referenceLatitude: 47.4084860363 # Wangen a.A.: 47.234593265310046, Hoengg: 47.4084860363, Oberglatt: 47.4801402 4 | referenceLongitude: 8.50435818058 # Wangen a.A.: 7.681415329904487, Hoengg: 8.50435818058, Oberglatt: 8.49975325 5 | referenceAltitude: 565.0 # Wangen a.A.: 472.58400000000074, Hoengg: 565.0, Oberglatt: 474.9 6 | referenceHeading: 0.0 # radians, # Wangen a.A.: 0.0, Hoengg: 0.0, Oberglatt: 2.6305 7 | -------------------------------------------------------------------------------- /graph_msf/config/graph_config.yaml: -------------------------------------------------------------------------------- 1 | #Common 2 | common_params: 3 | verbosity: 0 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors 4 | -------------------------------------------------------------------------------- /graph_msf/config/graph_params.yaml: -------------------------------------------------------------------------------- 1 | # Sensor Params 2 | sensor_params: 3 | imuRate: 100 #Rate of IMU input (Hz) 4 | imuBufferLength: 200 5 | lidarOdometryRate: 5 6 | gnssRate: 20 7 | imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot 8 | 9 | # Factor Graph 10 | graph_params: 11 | useIsam: true # if false, then levenberg-marquardt is used 12 | smootherLag: 1.5 #Lag of fixed lag smoother[seconds], not needed for ISAM2 13 | additionalOptimizationIterations: 0 #Additional iterations of graph optimizer after update with new factors 14 | findUnusedFactorSlots: false 15 | enableDetailedResults: false 16 | usingFallbackGraph: true 17 | 18 | # Outlier Rejection 19 | outlier_params: 20 | gnssOutlierThreshold: 0.2 # in meters, if jumping more than this, it is considered as absent GNSS, occurs between two GNSS measurements 21 | 22 | # Noise Parameters 23 | noise_params: 24 | ## IMU 25 | ### Accelerometer 26 | accNoiseDensity: 7.225e-08 #Continuous-time "Covariance" of accelerometer, microstrain: sigma^2=7.225e-7 27 | accBiasRandomWalk: 1.0e-05 #Continuous-time "Covariance" describing accelerometer bias random walk, default: 1.0e-06 28 | accBiasPrior: 0.0 #Prior/starting value of accelerometer bias 29 | ### Gyro 30 | gyrNoiseDensity: 1.71e-08 #Continuous-time "Covariance" of gyroscope measurements, microstrain: sigma^2=1.71-08 31 | gyrBiasRandomWalk: 9.33e-06 #gnss:9.33e-08 #lidar: 9.33e-01 #Continuous-time "Covariance" describing gyroscope bias random walk, default: 9.33e-08 32 | gyrBiasPrior: 0.0 #Prior/starting value of gyroscope bias 33 | ### Preintegration 34 | integrationNoiseDensity: 1.0e-03 #continuous-time "Covariance" describing integration uncertainty, default: 1.0e-06 35 | biasAccOmegaPreInt: 1.0e-03 #covariance of bias used for preintegration, default: 1.0e-2 36 | ## LiDAR 37 | poseBetweenNoise: [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ] # gnss [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ] # lidar: [ 1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0 ] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) 38 | poseUnaryNoise: [ 1000.0, 1000.0, 10.0, 10.0, 10.0, 10.0 ] # ORDER RPY(rad) - XYZ(meters) First tests: [ 1000.0, 1000.0, 1000.0, 2.0, 2.0, 2.0 ] 39 | ## GNSS 40 | gnssPositionUnaryNoise: 1.0 #1.0e-02 # x, y, z of global position 41 | gnssHeadingUnaryNoise: 1.0 # x,y,z of heading vector 42 | 43 | # Relinearization 44 | relinearization_params: 45 | positionReLinTh: 5.0e-10 #Position linearization threshold 46 | rotationReLinTh: 1.0e-10 #Rotation linearization threshold 47 | velocityReLinTh: 1.0e-10 #Linear Velocity linearization threshold 48 | accBiasReLinTh: 1.0e-10 #Accelerometer bias linearization threshold 49 | gyrBiasReLinTh: 1.0e-10 #Gyroscope bias linearization threshold 50 | relinearizeSkip: 1 51 | enableRelinearization: true 52 | evaluateNonlinearError: false 53 | cacheLinearizedFactors: true 54 | enablePartialRelinearizationCheck: true 55 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/Datatypes.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_DATATYPES_HPP 9 | #define GRAPH_MSF_DATATYPES_HPP 10 | 11 | namespace graph_msf { 12 | 13 | // Map from time to 6D IMU measurements 14 | typedef std::map, Eigen::aligned_allocator>> TimeToImuMap; 15 | 16 | // Map from time to gtsam key 17 | typedef std::map, Eigen::aligned_allocator>> TimeToKeyMap; 18 | 19 | } // namespace graph_msf 20 | 21 | #endif // GRAPH_MSF_DATATYPES_HPP 22 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/GraphManager.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MANAGER_HPP_ 9 | #define GRAPH_MANAGER_HPP_ 10 | 11 | // C++ 12 | #include 13 | #include 14 | #include 15 | 16 | // GTSAM 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | // Factors 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | // Package 29 | #include "graph_msf/GraphState.hpp" 30 | #include "graph_msf/ImuBuffer.hpp" 31 | #include "graph_msf/config/GraphConfig.h" 32 | #include "graph_msf/factors/HeadingFactor.h" 33 | #include "graph_msf/measurements/UnaryMeasurement6D.h" 34 | 35 | namespace graph_msf { 36 | 37 | #define GREEN_START "\033[92m" 38 | #define YELLOW_START "\033[33m" 39 | #define RED_START "\033[31m" 40 | #define COLOR_END "\033[0m" 41 | 42 | class GraphManager { 43 | public: 44 | GraphManager(std::shared_ptr graphConfigPtr); 45 | ~GraphManager(){}; 46 | 47 | // Change Graph 48 | bool initImuIntegrators(const double g, const std::string& imuGravityDirection); 49 | bool initPoseVelocityBiasGraph(const double ts, const gtsam::Pose3& init_pose); 50 | gtsam::NavState addImuFactorAndGetState(const double imuTimeK, const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, 51 | bool& relocalizationFlag); 52 | gtsam::Key addPoseBetweenFactorToGlobalGraph(const double lidarTimeKm1, const double lidarTimeK, const double rate, 53 | const Eigen::Matrix& poseBetweenNoise, const gtsam::Pose3& pose); 54 | void addPoseUnaryFactorToGlobalGraph(const double lidarTimeK, const double rate, const Eigen::Matrix& poseUnaryNoise, 55 | const gtsam::Pose3& unaryPose); 56 | void addPoseUnaryFactorToFallbackGraph(const double lidarTimeK, const double rate, const Eigen::Matrix& poseUnaryNoise, 57 | const gtsam::Pose3& pose); 58 | void addGnssPositionUnaryFactor(double gnssTime, const double rate, const Eigen::Vector3d& gnssPositionUnaryNoise, 59 | const gtsam::Vector3& position); 60 | void addGnssHeadingUnaryFactor(double gnssTime, const double rate, const double gnssHeadingUnaryNoise, const double measuredYaw); 61 | bool addZeroMotionFactor(double maxTimestampDistance, double timeKm1, double timeK, const gtsam::Pose3 pose); 62 | 63 | // Graph selection 64 | void activateGlobalGraph(); 65 | void activateFallbackGraph(); 66 | 67 | // Update graph and get new state 68 | gtsam::NavState updateActiveGraphAndGetState(double& currentTime); 69 | 70 | // Compute state at specific key 71 | gtsam::NavState calculateActiveStateAtKey(const gtsam::Key& key); 72 | 73 | // IMU Buffer interface 74 | /// Estimate attitude from IMU 75 | inline bool estimateAttitudeFromImu(const std::string& imuGravityDirection, gtsam::Rot3& initAttitude, double& gravityMagnitude, 76 | Eigen::Vector3d& gyrBias) { 77 | return imuBuffer_.estimateAttitudeFromImu(imuGravityDirection, initAttitude, gravityMagnitude, gyrBias); 78 | } 79 | /// Add to IMU buffer 80 | inline void addToIMUBuffer(double ts, const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel) { 81 | imuBuffer_.addToIMUBuffer(ts, linearAcc(0), linearAcc(1), linearAcc(2), angularVel(0), angularVel(1), angularVel(2)); 82 | } 83 | 84 | // Accessors 85 | /// Getters 86 | Eigen::Vector3d& getInitGyrBiasReference() { return graphConfigPtr_->gyroBiasPrior; } 87 | // auto iterations() const { return additonalIterations_; } 88 | const State& getGraphState() { return graphState_; } 89 | const gtsam::Key getStateKey() { return stateKey_; } 90 | const gtsam::imuBias::ConstantBias getIMUBias() { return graphState_.imuBias(); } 91 | gtsam::ISAM2Params& getIsamParamsReference() { return isamParams_; } 92 | 93 | // Status 94 | bool globalGraphActiveFlag() { 95 | const std::lock_guard swappingActiveGraphLock(swappingActiveGraphMutex_); 96 | return activeSmootherPtr_ == globalSmootherPtr_; 97 | } 98 | bool fallbackGraphActiveFlag() { 99 | const std::lock_guard swappingActiveGraphLock(swappingActiveGraphMutex_); 100 | return activeSmootherPtr_ == fallbackSmootherPtr_; //&& numOptimizationsSinceGraphSwitching_ >= 1; 101 | } 102 | 103 | private: 104 | // Methods 105 | template 106 | bool addFactorToGraph_(std::shared_ptr modifiedGraphPtr, const gtsam::NoiseModelFactor* noiseModelFactorPtr); 107 | template 108 | bool addFactorToGraph_(std::shared_ptr modifiedGraphPtr, const gtsam::NoiseModelFactor* noiseModelFactorPtr, 109 | const double measurementTimestamp); 110 | template 111 | bool addFactorSafelyToGraph_(std::shared_ptr modifiedGraphPtr, 112 | const gtsam::NoiseModelFactor* noiseModelFactorPtr, const double measurementTimestamp); 113 | /// Update IMU integrators 114 | void updateImuIntegrators_(const TimeToImuMap& imuMeas); 115 | // Calculate state at key for graph 116 | static gtsam::NavState calculateStateAtKey(std::shared_ptr graphPtr, 117 | const std::shared_ptr graphConfigPtr, const gtsam::Key& key); 118 | // Add Factors for a smoother 119 | static void addFactorsToSmootherAndOptimize(std::shared_ptr smootherPtr, 120 | const gtsam::NonlinearFactorGraph& newGraphFactors, const gtsam::Values& newGraphValues, 121 | const std::map& newGraphKeysTimestampsMap, 122 | const std::shared_ptr graphConfigPtr); 123 | /// Find graph keys for timestamps 124 | bool findGraphKeys_(double maxTimestampDistance, double timeKm1, double timeK, gtsam::Key& keyKm1, gtsam::Key& keyK, 125 | const std::string& name = "lidar"); 126 | /// Generate new key 127 | const auto newStateKey_() { return ++stateKey_; } 128 | /// Associate timestamp to each 'value key', e.g. for graph key 0, value keys (x0,v0,b0) need to be associated 129 | inline void writeValueKeysToKeyTimeStampMap_(const gtsam::Values& values, const double measurementTime, 130 | std::shared_ptr> keyTimestampMapPtr) { 131 | for (const auto& value : values) { 132 | (*keyTimestampMapPtr)[value.key] = measurementTime; 133 | } 134 | } 135 | 136 | // Objects 137 | boost::shared_ptr imuParamsPtr_; 138 | std::shared_ptr imuBiasPriorPtr_; 139 | State graphState_; 140 | gtsam::ISAM2Params isamParams_; 141 | 142 | // Graphs 143 | std::shared_ptr globalSmootherPtr_; 144 | std::shared_ptr fallbackSmootherPtr_; 145 | /// Data buffers 146 | std::shared_ptr globalFactorsBufferPtr_; 147 | std::shared_ptr fallbackFactorsBufferPtr_; 148 | // Values map 149 | std::shared_ptr globalGraphValuesBufferPtr_; 150 | std::shared_ptr fallbackGraphValuesBufferPtr_; 151 | // Timestampmaps 152 | std::shared_ptr> globalGraphKeysTimestampsMapBufferPtr_; 153 | std::shared_ptr> fallbackGraphKeysTimestampsMapBufferPtr_; 154 | 155 | /// Counter 156 | int numOptimizationsSinceGraphSwitching_ = 0; 157 | bool sentRelocalizationCommandAlready_ = true; 158 | 159 | /// Buffer Preintegrator 160 | std::shared_ptr globalImuBufferPreintegratorPtr_; 161 | std::shared_ptr fallbackImuBufferPreintegratorPtr_; 162 | /// Step Preintegrator 163 | std::shared_ptr imuStepPreintegratorPtr_; 164 | /// IMU Buffer 165 | ImuBuffer imuBuffer_; // Need to get rid of this 166 | gtsam::Vector6 lastImuVector_; 167 | 168 | /// Config 169 | std::shared_ptr graphConfigPtr_ = NULL; 170 | /// Graph names 171 | std::vector graphNames_{"globalGraph", "fallbackGraph"}; 172 | /// Selector 173 | std::shared_ptr activeSmootherPtr_ = globalSmootherPtr_; 174 | std::shared_ptr activeFactorsBufferPtr_ = globalFactorsBufferPtr_; 175 | std::shared_ptr activeImuBufferPreintegratorPtr_ = globalImuBufferPreintegratorPtr_; 176 | std::shared_ptr activeGraphValuesBufferPtr_ = globalGraphValuesBufferPtr_; 177 | std::shared_ptr> activeGraphKeysTimestampsMapBufferPtr_ = globalGraphKeysTimestampsMapBufferPtr_; 178 | 179 | // Member variables 180 | /// Mutex 181 | std::mutex operateOnGraphDataMutex_; 182 | std::mutex activelyUsingActiveGraphMutex_; 183 | std::mutex swappingActiveGraphMutex_; 184 | /// Propagated state (at IMU frequency) 185 | gtsam::NavState imuPropagatedState_; 186 | 187 | /// Factor Graph 188 | gtsam::Key stateKey_ = 0; // Current state key 189 | double stateTime_; 190 | }; 191 | } // namespace graph_msf 192 | 193 | #endif // GRAPH_MANAGER_HPP_ -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/GraphMsf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_H 9 | #define GRAPH_MSF_H 10 | 11 | // C++ 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // Package 18 | #include "StaticTransforms.h" 19 | #include "graph_msf/GraphManager.hpp" 20 | #include "graph_msf/InterfacePrediction.h" 21 | #include "graph_msf/config/GraphConfig.h" 22 | #include "graph_msf/geometry/math_utils.h" 23 | #include "graph_msf/measurements/BinaryMeasurement6D.h" 24 | #include "graph_msf/measurements/UnaryMeasurement1D.h" 25 | #include "graph_msf/measurements/UnaryMeasurement3D.h" 26 | #include "graph_msf/measurements/UnaryMeasurement6D.h" 27 | 28 | // Defined macros 29 | #define ROS_QUEUE_SIZE 100 30 | #define REQUIRED_GNSS_NUM_NOT_JUMPED 40 // 2*singleGnssJumping = 2*20 = 40 31 | #define GNSS_COVARIANCE_VIOLATION_THRESHOLD 0.1 // 10000 32 | #define GREEN_START "\033[92m" 33 | #define YELLOW_START "\033[33m" 34 | #define COLOR_END "\033[0m" 35 | 36 | namespace graph_msf { 37 | 38 | class GraphMsf { 39 | public: 40 | // Constructor 41 | GraphMsf(); 42 | // Destructor 43 | ~GraphMsf(){}; 44 | 45 | // Setup 46 | bool setup(std::shared_ptr graphConfigPtr, std::shared_ptr staticTransformsPtr); 47 | 48 | // Required Initialization 49 | bool initYawAndPosition(const double yaw_W_frame1, const std::string& frame1, const Eigen::Vector3d& W_t_W_frame2, 50 | const std::string& frame2); 51 | bool initYawAndPosition(const Eigen::Matrix4d& T_O_frame, const std::string& frameName); 52 | bool initYawAndPosition(Eigen::Matrix4d T_O_I); 53 | bool yawAndPositionInited(); 54 | 55 | // Graph Manipulation 56 | void activateFallbackGraph(); 57 | 58 | // Adderfunctions 59 | /// Return 60 | bool addImuMeasurement(const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, const double imuTimeK, 61 | std::shared_ptr& predictionPtr); 62 | /// No return 63 | void addOdometryMeasurement(const BinaryMeasurement6D& delta); 64 | void addUnaryPoseMeasurement(const UnaryMeasurement6D& unary); 65 | void addDualOdometryMeasurement(const UnaryMeasurement6D& odometryKm1, const UnaryMeasurement6D& odometryK, 66 | const Eigen::Matrix& poseBetweenNoise); 67 | void addDualGnssPositionMeasurement(const UnaryMeasurement3D& W_t_W_frame, const Eigen::Vector3d& lastPosition, 68 | const Eigen::Vector3d& estCovarianceXYZ); 69 | void addGnssPositionMeasurement(const UnaryMeasurement3D& W_t_W_frame); 70 | void addGnssHeadingMeasurement(const UnaryMeasurement1D& yaw_W_frame); 71 | 72 | // Getters 73 | bool getLogPlots() { return logPlots_; } 74 | void getLatestOptimizedState(Eigen::Matrix4d& optState, double& time) { 75 | time = optTime_; 76 | optState = T_W_I_opt_.matrix(); 77 | } 78 | bool getNormalOperationFlag() const { return normalOperationFlag_; } 79 | 80 | protected: 81 | // Methods ------------- 82 | /// Worker functions 83 | //// Set Imu Attitude 84 | bool alignImu_(); 85 | //// Initialize the graph 86 | void initGraph_(const double timeStamp_k); 87 | //// Updating the factor graph 88 | void optimizeGraph_(); 89 | 90 | /// Utility functions 91 | //// Geometric transformation to IMU in world frame 92 | gtsam::Vector3 W_t_W_Frame1_to_W_t_W_Frame2_(const gtsam::Point3& W_t_W_frame1, const std::string& frame1, const std::string& frame2, 93 | const gtsam::Rot3& R_W_frame2); 94 | //// Get the robot heading from the two Gnss positions 95 | static gtsam::Point3 getRobotHeading_(const Eigen::Vector3d& leftPosition, const Eigen::Vector3d& rightPosition); 96 | 97 | // Threads 98 | std::thread optimizeGraphThread_; /// Thread 5: Update of the graph as soon as new lidar measurement has arrived 99 | 100 | // Mutex 101 | std::mutex initYawAndPositionMutex_; 102 | std::mutex optimizeGraphMutex_; 103 | 104 | // Factor graph 105 | std::shared_ptr graphMgrPtr_ = NULL; 106 | 107 | // Graph Config 108 | std::shared_ptr graphConfigPtr_ = NULL; 109 | std::shared_ptr staticTransformsPtr_ = NULL; 110 | 111 | /// Flags 112 | //// Configuration 113 | bool usingFallbackGraphFlag_ = true; 114 | 115 | //// Initialization 116 | bool alignedImuFlag_ = false; 117 | bool foundInitialYawAndPositionFlag_ = false; 118 | bool initedGraphFlag_ = false; 119 | bool receivedOdometryFlag_ = false; 120 | //// During operation 121 | bool optimizeGraphFlag_ = false; 122 | bool gnssCovarianceViolatedFlag_ = false; 123 | bool normalOperationFlag_ = false; 124 | 125 | /// Times 126 | double imuTimeKm1_; 127 | double imuTimeOffset_ = 0.0; 128 | 129 | /// Transformations with timestamps 130 | /// Pose 131 | gtsam::Pose3 T_W_Ik_; 132 | gtsam::Pose3 T_W_O_; 133 | /// Velocity 134 | gtsam::Vector3 I_v_W_I_; 135 | gtsam::Vector3 I_w_W_I_; 136 | /// Timestamp 137 | double imuTimeK_; 138 | /// Other 139 | gtsam::Pose3 T_W_I0_; // Initial IMU pose (in graph) 140 | gtsam::Pose3 T_W_I_opt_; 141 | double optTime_; 142 | 143 | /// Attitudes 144 | double gravityConstant_ = 9.81; // Will be overwritten 145 | double yaw_W_I0_; 146 | gtsam::Vector3 W_t_W_I0_; 147 | double imuAttitudePitch_; 148 | double imuAttitudeRoll_; 149 | 150 | /// Counter 151 | long gnssCallbackCounter_ = 0; 152 | int gnssNotJumpingCounter_ = REQUIRED_GNSS_NUM_NOT_JUMPED; 153 | 154 | /// Logging 155 | bool logPlots_ = false; 156 | }; 157 | 158 | } // namespace graph_msf 159 | 160 | #endif // GRAPH_MSF_H 161 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/GraphMsfInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF__INTERFACE_H 9 | #define GRAPH_MSF__INTERFACE_H 10 | 11 | #include 12 | #include 13 | 14 | // Workspace 15 | #include "StaticTransforms.h" 16 | #include "graph_msf/config/GraphConfig.h" 17 | #include "graph_msf/measurements/BinaryMeasurement6D.h" 18 | #include "graph_msf/measurements/UnaryMeasurement1D.h" 19 | #include "graph_msf/measurements/UnaryMeasurement3D.h" 20 | #include "graph_msf/measurements/UnaryMeasurement6D.h" 21 | 22 | namespace graph_msf { 23 | 24 | class GraphMsf; 25 | 26 | class GraphMsfInterface { 27 | public: 28 | GraphMsfInterface(); 29 | 30 | protected: 31 | // Setup 32 | bool setup_(); 33 | 34 | // Required for initialization 35 | bool initYawAndPosition_(const double yaw_W_frame1, const std::string& frame1, const Eigen::Vector3d& t_W_frame2, 36 | const std::string& frame2); 37 | bool initYawAndPosition_(const Eigen::Matrix4d& T_W_frame, const std::string& frameName); 38 | bool areYawAndPositionInited_(); 39 | 40 | // Graph Maniupulation 41 | 42 | void activateFallbackGraph(); 43 | 44 | // Write measurements 45 | void addImuMeasurementAndPublishState_(const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, const double imuTimeK); 46 | void addOdometryMeasurement_(const BinaryMeasurement6D& delta); 47 | void addUnaryPoseMeasurement_(const UnaryMeasurement6D& unary); 48 | void addDualOdometryMeasurement_(const UnaryMeasurement6D& odometryKm1, const UnaryMeasurement6D& odometryK, 49 | const Eigen::Matrix& poseBetweenNoise); 50 | void addDualGnssPositionMeasurement_(const UnaryMeasurement3D& W_t_W_frame, const Eigen::Vector3d& lastPosition, 51 | const Eigen::Vector3d& estCovarianceXYZ); 52 | void addGnssPositionMeasurement_(const UnaryMeasurement3D& W_t_W_frame); 53 | void addGnssHeadingMeasurement_(const UnaryMeasurement1D& yaw_W_frame); 54 | 55 | // Publish 56 | virtual void publishState_(const double imuTimeK, const Eigen::Matrix4d& T_W_O, const Eigen::Matrix4d& T_O_Ik, 57 | const Eigen::Vector3d& I_v_W_I, const Eigen::Vector3d& I_w_W_I) = 0; 58 | 59 | // Getters 60 | bool isInNormalOperation() const; 61 | 62 | // Member Variables 63 | /// GraphMsf 64 | std::shared_ptr graphMsfPtr_ = NULL; 65 | /// Graph Configuration 66 | std::shared_ptr graphConfigPtr_ = NULL; 67 | std::shared_ptr staticTransformsPtr_ = NULL; 68 | 69 | /// Verbosity 70 | int verboseLevel_ = 0; 71 | 72 | private: 73 | // Threads 74 | std::thread publishStateThread_; 75 | 76 | // Functions 77 | void publishStateAndMeasureTime_(const double imuTimeK, const Eigen::Matrix4d& T_W_O, const Eigen::Matrix4d& T_O_Ik, 78 | const Eigen::Vector3d& I_v_W_I, const Eigen::Vector3d& I_w_W_I); 79 | }; 80 | 81 | } // namespace graph_msf 82 | 83 | #endif // GRAPH_MSF_INTERFACE_H 84 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/GraphState.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_STATE_HPP_ 9 | #define GRAPH_STATE_HPP_ 10 | 11 | // C++ 12 | #include 13 | 14 | // GTSAM 15 | #include 16 | #include 17 | 18 | namespace graph_msf { 19 | 20 | // Class defining Robot State 21 | class State { 22 | private: 23 | gtsam::Key key_ = 0; // key 24 | double ts_ = 0.0; // timestamp 25 | gtsam::NavState navState_; // pose, velocity 26 | gtsam::imuBias::ConstantBias imuBias_; // imu bias 27 | std::mutex stateMutex_; 28 | 29 | public: 30 | State(){}; // Constructor 31 | ~State(){}; // Destructor 32 | 33 | // Accessors 34 | const auto key() const { return key_; } 35 | const auto ts() const { return ts_; } 36 | const auto& navState() const { return navState_; } 37 | const auto& imuBias() const { return imuBias_; } 38 | 39 | // Update state Graph Key and Timestamp 40 | void updateKeyAndTimestamp(const gtsam::Key key, const double ts) { 41 | std::lock_guard lock(stateMutex_); 42 | key_ = key; 43 | ts_ = ts; 44 | } 45 | 46 | // Update state Graph Key, Timestamp and NavState(Pose+Velocity) 47 | void updateNavState(const gtsam::Key key, const double ts, const gtsam::NavState& navState) { 48 | std::lock_guard lock(stateMutex_); 49 | key_ = key; 50 | ts_ = ts; 51 | navState_ = navState; 52 | } 53 | 54 | // Update state Graph Key, Timestamp and IMU Bias estimate 55 | void updateImuBias(const gtsam::Key key, const double ts, const gtsam::imuBias::ConstantBias& imuBias) { 56 | std::lock_guard lock(stateMutex_); 57 | key_ = key; 58 | ts_ = ts; 59 | imuBias_ = imuBias; 60 | } 61 | 62 | // Update state Graph Key, Timestamp, NavState(Pose+Velocity) and IMU Bias estimate 63 | void updateNavStateAndBias(const gtsam::Key key, const double ts, const gtsam::NavState& navState, 64 | const gtsam::imuBias::ConstantBias& imuBias) { 65 | std::lock_guard lock(stateMutex_); 66 | key_ = key; 67 | ts_ = ts; 68 | navState_ = navState; 69 | imuBias_ = imuBias; 70 | } 71 | 72 | // Overload assignment operator 73 | State& operator=(State& other) { 74 | this->key_ = other.key_; 75 | this->ts_ = other.ts_; 76 | this->navState_ = other.navState_; 77 | this->imuBias_ = other.imuBias_; 78 | return *this; 79 | } 80 | }; 81 | 82 | } // namespace graph_msf 83 | 84 | #endif // GRAPH_STATE_HPP_ -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/ImuBuffer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef IMU_MANAGER_HPP_ 9 | #define IMU_MANAGER_HPP_ 10 | #define DEFAULT_IMU_RATE 100 11 | 12 | // C++ 13 | #include 14 | #include 15 | 16 | // Eigen 17 | #include 18 | 19 | // GTSAM 20 | #include 21 | #include 22 | 23 | // Package 24 | #include "graph_msf/Datatypes.hpp" 25 | 26 | namespace graph_msf { 27 | 28 | #define GREEN_START "\033[92m" 29 | #define YELLOW_START "\033[33m" 30 | #define RED_START "\033[31m" 31 | #define COLOR_END "\033[0m" 32 | 33 | class ImuBuffer { 34 | public: 35 | // Constructor 36 | ImuBuffer() : imuRate_(DEFAULT_IMU_RATE) { 37 | // Reset IMU Buffer 38 | timeToImuBuffer_.clear(); 39 | } 40 | 41 | // Destructor 42 | ~ImuBuffer() = default; 43 | 44 | // Setters 45 | inline void setImuRate(double d) { imuRate_ = d; } 46 | inline void setImuBufferLength(int i) { imuBufferLength_ = i; } 47 | inline void setVerboseLevel(int i) { verboseLevel_ = i; } 48 | 49 | // Add to buffers 50 | void addToIMUBuffer(double ts, double accX, double accY, double accZ, double gyrX, double gyrY, double gyrZ); 51 | void addToKeyBuffer(double ts, gtsam::Key key); 52 | 53 | // Getters 54 | inline double getImuRate() const { return imuRate_; } 55 | inline double getLatestTimestampInBuffer() const { return tLatestInBuffer_; } 56 | void getLastTwoMeasurements(TimeToImuMap& imuMap); 57 | bool getClosestKeyAndTimestamp(double& tInGraph, gtsam::Key& key, const std::string& callingName, const double maxSearchDeviation, 58 | const double tLidar); 59 | bool getIMUBufferIteratorsInInterval(const double& ts_start, const double& ts_end, TimeToImuMap::iterator& s_itr, 60 | TimeToImuMap::iterator& e_itr); 61 | 62 | // Public member functions 63 | /// Determine initial IMU pose w.r.t to gravity vector pointing up 64 | bool estimateAttitudeFromImu(const std::string& imuGravityDirection, gtsam::Rot3& init_attitude, double& gravity_magnitude, 65 | Eigen::Vector3d& gyrBias); 66 | 67 | private: 68 | // Member variables 69 | TimeToImuMap timeToImuBuffer_; // IMU buffer 70 | TimeToKeyMap timeToKeyBuffer_; 71 | double imuRate_ = -1; // Rate of IMU input (Hz) - Used to calculate minimum measurements needed to calculate gravity and init attitude 72 | int imuBufferLength_ = -1; 73 | const double imuPoseInitWaitSecs_ = 1.0; // Multiplied with _imuRate 74 | int verboseLevel_ = 0; 75 | double tLatestInBuffer_ = 0.0; 76 | std::mutex writeInBufferMutex_; 77 | }; 78 | 79 | } // namespace graph_msf 80 | 81 | #endif // IMU_MANAGER_HPP_ -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/InterfacePrediction.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef INTERFACE_PREDICTION_H 9 | #define INTERFACE_PREDICTION_H 10 | 11 | #include 12 | 13 | namespace graph_msf { 14 | 15 | struct InterfacePrediction { 16 | InterfacePrediction(const Eigen::Matrix4d& T_W_O_, const Eigen::Matrix4d& T_O_Ik_, const Eigen::Vector3d& I_v_W_I_, 17 | const Eigen::Vector3d& I_w_W_I_) 18 | : T_W_O(T_W_O_), T_O_Ik(T_O_Ik_), I_v_W_I(I_v_W_I_), I_w_W_I(I_w_W_I_) {} 19 | 20 | const Eigen::Matrix4d T_W_O; 21 | const Eigen::Matrix4d T_O_Ik; 22 | const Eigen::Vector3d I_v_W_I; 23 | const Eigen::Vector3d I_w_W_I; 24 | }; 25 | 26 | } // namespace graph_msf 27 | 28 | #endif // INTERFACE_PREDICTION_H 29 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/StaticTransforms.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef STATIC_TRANSFORMS_H 9 | #define STATIC_TRANSFORMS_H 10 | 11 | #include 12 | #include 13 | 14 | namespace graph_msf { 15 | 16 | #define GREEN_START "\033[92m" 17 | #define YELLOW_START "\033[33m" 18 | #define COLOR_END "\033[0m" 19 | 20 | class StaticTransforms { 21 | public: 22 | // Constructor 23 | StaticTransforms() { std::cout << YELLOW_START << "StaticTransforms" << COLOR_END << " StaticTransforms instance created." << std::endl; } 24 | 25 | // Setters 26 | Eigen::Matrix4d& lv_T_frame1_frame2(const std::string& frame1, const std::string& frame2) { 27 | std::pair framePair(frame1, frame2); 28 | // auto keyIterator = T_frame1_frame2_map_.find(framePair); 29 | // if (keyIterator == T_frame1_frame2_map_.end()) { 30 | // std::cout << YELLOW_START << "StaticTransforms" << COLOR_END << " No transform found for " << frame1 << " and " << frame2 31 | // << std::endl; 32 | // throw std::runtime_error("No transform found for " + frame1 + " and " + frame2); 33 | // } 34 | return T_frame1_frame2_map_[framePair]; 35 | } 36 | 37 | void setImuFrame(const std::string& s) { imuFrame_ = s; } 38 | 39 | // Getters 40 | const Eigen::Matrix4d& rv_T_frame1_frame2(const std::string& frame1, const std::string& frame2) { 41 | std::pair framePair(frame1, frame2); 42 | auto keyIterator = T_frame1_frame2_map_.find(framePair); 43 | if (keyIterator == T_frame1_frame2_map_.end()) { 44 | std::cout << YELLOW_START << "StaticTransforms" << COLOR_END << " No transform found for " << frame1 << " and " << frame2 45 | << std::endl; 46 | throw std::runtime_error("No transform found for " + frame1 + " and " + frame2); 47 | } 48 | return keyIterator->second; 49 | } 50 | 51 | const std::string& getImuFrame() { return imuFrame_; } 52 | 53 | // Functionality 54 | virtual void findTransformations() = 0; 55 | 56 | protected: 57 | // General container class 58 | std::map, Eigen::Matrix4d> T_frame1_frame2_map_; 59 | 60 | // Required frames 61 | std::string imuFrame_; // If used --> copied to imuCabinFrame_ 62 | }; 63 | 64 | } // namespace graph_msf 65 | 66 | #endif // STATIC_TRANSFORMS_H 67 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/config/GraphConfig.h: -------------------------------------------------------------------------------- 1 | #ifndef GRAPHCONFIG_H 2 | #define GRAPHCONFIG_H 3 | 4 | #include 5 | 6 | namespace graph_msf { 7 | 8 | struct GraphConfig { 9 | GraphConfig() {} 10 | 11 | // Strings 12 | std::string imuGravityDirection = "up"; 13 | 14 | // Booleans 15 | /// ISAM 16 | bool useIsam = true; 17 | bool findUnusedFactorSlots = false; 18 | bool enableDetailedResults = false; 19 | bool enableRelinearization = true; 20 | bool evaluateNonlinearError = true; 21 | bool cacheLinearizedFactors = true; 22 | bool enablePartialRelinearizationCheck = true; 23 | /// Flags 24 | bool usingFallbackGraphFlag = true; 25 | bool usingGnssFlag = true; 26 | bool usingLioFlag = true; 27 | 28 | // Integers 29 | int verboseLevel = 0; 30 | int imuBufferLength = 200; 31 | int additionalIterations = 0; 32 | int relinearizeSkip = 0; 33 | 34 | // Doubles 35 | double imuRate = 100; 36 | double maxSearchDeviation = 0.01; 37 | double imuTimeOffset = 0.0; 38 | double smootherLag = 6.0; 39 | double zeroMotionTh = 0.0; 40 | double gnssOutlierThresold = 0.3; 41 | /// Noise Params 42 | double accNoiseDensity = 1e-8; 43 | double accBiasRandomWalk = 1e-8; 44 | double gyroNoiseDensity = 1e-8; 45 | double gyroBiasRandomWalk = 1e-8; 46 | double integrationNoiseDensity = 1.0; 47 | double biasAccOmegaPreint = 1.0; 48 | /// Optimization 49 | double positionReLinTh = 1e-3; 50 | double rotationReLinTh = 1e-3; 51 | double velocityReLinTh = 1e-3; 52 | double accBiasReLinTh = 1e-3; 53 | double gyroBiasReLinTh = 1e-3; 54 | 55 | // Eigen 56 | Eigen::Vector3d accBiasPrior = Eigen::Vector3d(0.0, 0.0, 0.0); 57 | Eigen::Vector3d gyroBiasPrior = Eigen::Vector3d(0.0, 0.0, 0.0); 58 | }; 59 | 60 | } // namespace graph_msf 61 | 62 | #endif // GRAPHCONFIG_H 63 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/factors/HeadingFactor.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_HEADING_FACTOR_H 9 | #define GRAPH_MSF_HEADING_FACTOR_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace graph_msf { 24 | 25 | /** 26 | * Factor to estimate rotation given gnss robot heading 27 | */ 28 | class HeadingFactor : public gtsam::NoiseModelFactor1 { 29 | public: 30 | /** 31 | * Constructor of factor that estimates nav to body rotation bRn 32 | * @param key of the unknown rotation bRn in the factor graph 33 | * @param measured magnetometer reading, a 3-vector 34 | * @param model of the additive Gaussian noise that is assumed 35 | */ 36 | HeadingFactor(gtsam::Key j, double yaw, const gtsam::SharedNoiseModel& model) 37 | : gtsam::NoiseModelFactor1(model, j), yaw_(yaw) {} 38 | 39 | // Destructor 40 | virtual ~HeadingFactor() {} 41 | 42 | /** 43 | * Evaluate error function 44 | * @brief vector of errors 45 | */ 46 | gtsam::Vector evaluateError(const gtsam::Pose3& q, boost::optional H_Ptr = boost::none) const { 47 | // Jacobian 48 | if (H_Ptr) { 49 | (*H_Ptr) = (gtsam::Matrix(1, 6) << 0.0, 0.0, -1.0, 0.0, 0.0, 0.0).finished(); // [rad] [m] 50 | } 51 | 52 | // calculate error 53 | double yaw_error = q.rotation().yaw() - yaw_; 54 | 55 | while (yaw_error < -M_PI) yaw_error += 2 * M_PI; 56 | while (yaw_error > M_PI) yaw_error -= 2 * M_PI; 57 | 58 | return gtsam::Vector1(yaw_error); 59 | } 60 | 61 | private: 62 | double yaw_; // yaw measurement 63 | }; 64 | 65 | } // namespace graph_msf 66 | 67 | #endif // GRAPH_MSF_HEADING_FACTOR_H -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/factors/PitchFactor.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_PITCH_FACTOR_H 9 | #define GRAPH_MSF_PITCH_FACTOR_H 10 | 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace graph_msf { 24 | 25 | /** 26 | * Factor to estimate rotation given gnss robot pitch 27 | */ 28 | class PitchFactor : public gtsam::NoiseModelFactor1 { 29 | public: 30 | /** 31 | * Constructor of factor that estimates nav to body rotation bRn 32 | * @param key of the unknown rotation bRn in the factor graph 33 | * @param measured magnetometer reading, a 3-vector 34 | * @param model of the additive Gaussian noise that is assumed 35 | */ 36 | PitchFactor(gtsam::Key j, double pitch, const gtsam::SharedNoiseModel& model) 37 | : gtsam::NoiseModelFactor1(model, j), pitch_(pitch) {} 38 | 39 | // Destructor 40 | virtual ~PitchFactor() {} 41 | 42 | /** 43 | * Evaluate error function 44 | * @brief vector of errors 45 | */ 46 | gtsam::Vector evaluateError(const gtsam::Pose3& q, boost::optional H_Ptr = boost::none) const { 47 | // Jacobian 48 | if (H_Ptr) { 49 | (*H_Ptr) = (gtsam::Matrix(1, 6) << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0).finished(); // [rad] [m] 50 | } 51 | 52 | // calculate error 53 | double pitch_error = q.rotation().pitch() - pitch_; 54 | 55 | while (pitch_error < -M_PI) pitch_error += 2 * M_PI; 56 | while (pitch_error > M_PI) pitch_error -= 2 * M_PI; 57 | 58 | return gtsam::Vector1(pitch_error); 59 | } 60 | 61 | private: 62 | double pitch_; // pitch measurement 63 | }; 64 | 65 | } // namespace graph_msf 66 | 67 | #endif // GRAPH_MSF_PITCH_FACTOR_H -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/geometry/Angle.h: -------------------------------------------------------------------------------- 1 | #ifndef LOAM_ANGLE_H 2 | #define LOAM_ANGLE_H 3 | 4 | #include 5 | #include 6 | 7 | namespace graph_msf { 8 | 9 | /** \brief Class for holding an angle. 10 | * 11 | * This class provides buffered access to sine and cosine values to the represented angular value. 12 | */ 13 | class Angle { 14 | public: 15 | Angle() : _radian(0.0), _cos(1.0), _sin(0.0) {} 16 | 17 | Angle(float radValue) : _radian(radValue), _cos(std::cos(radValue)), _sin(std::sin(radValue)) {} 18 | 19 | Angle(const Angle& other) : _radian(other._radian), _cos(other._cos), _sin(other._sin) {} 20 | 21 | void operator=(const Angle& rhs) { 22 | _radian = (rhs._radian); 23 | _cos = (rhs._cos); 24 | _sin = (rhs._sin); 25 | } 26 | 27 | void operator+=(const float& radValue) { *this = (_radian + radValue); } 28 | 29 | void operator+=(const Angle& other) { *this = (_radian + other._radian); } 30 | 31 | void operator-=(const float& radValue) { *this = (_radian - radValue); } 32 | 33 | void operator-=(const Angle& other) { *this = (_radian - other._radian); } 34 | 35 | Angle operator-() const { 36 | Angle out; 37 | out._radian = -_radian; 38 | out._cos = _cos; 39 | out._sin = -(_sin); 40 | return out; 41 | } 42 | 43 | float rad() const { return _radian; } 44 | 45 | float deg() const { return float(_radian * 180 / M_PI); } 46 | 47 | float cos() const { return _cos; } 48 | 49 | float sin() const { return _sin; } 50 | 51 | private: 52 | float _radian; ///< angle value in radian 53 | float _cos; ///< cosine of the angle 54 | float _sin; ///< sine of the angle 55 | }; 56 | 57 | } // namespace graph_msf 58 | 59 | #endif // LOAM_ANGLE_H 60 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/geometry/math_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_MATH_UTILS_H 9 | #define GRAPH_MSF_MATH_UTILS_H 10 | 11 | #include 12 | #include "graph_msf/geometry/Angle.h" 13 | 14 | namespace graph_msf { 15 | 16 | /** \brief Convert the given radian angle to degrees. 17 | * 18 | * @param radians The radian angle to convert. 19 | * @return The angle in degrees. 20 | */ 21 | inline double rad2deg(double radians) { 22 | return radians * 180.0 / M_PI; 23 | } 24 | 25 | /** \brief Convert the given radian angle to degrees. 26 | * 27 | * @param radians The radian angle to convert. 28 | * @return The angle in degrees. 29 | */ 30 | inline float rad2deg(float radians) { 31 | return (float)(radians * 180.0 / M_PI); 32 | } 33 | 34 | /** \brief Convert the given degree angle to radian. 35 | * 36 | * @param degrees The degree angle to convert. 37 | * @return The radian angle. 38 | */ 39 | inline double deg2rad(double degrees) { 40 | return degrees * M_PI / 180.0; 41 | } 42 | 43 | /** \brief Convert the given degree angle to radian. 44 | * 45 | * @param degrees The degree angle to convert. 46 | * @return The radian angle. 47 | */ 48 | inline float deg2rad(float degrees) { 49 | return (float)(degrees * M_PI / 180.0); 50 | } 51 | 52 | /** \brief Calculate the squared difference of the given two points. 53 | * 54 | * @param a The first point. 55 | * @param b The second point. 56 | * @return The squared difference between point a and b. 57 | */ 58 | template 59 | inline float calcSquaredDiff(const PointT& a, const PointT& b) { 60 | float diffX = a.x - b.x; 61 | float diffY = a.y - b.y; 62 | float diffZ = a.z - b.z; 63 | 64 | return diffX * diffX + diffY * diffY + diffZ * diffZ; 65 | } 66 | 67 | /** \brief Calculate the squared difference of the given two points. 68 | * 69 | * @param a The first point. 70 | * @param b The second point. 71 | * @param wb The weighting factor for the SECOND point. 72 | * @return The squared difference between point a and b. 73 | */ 74 | template 75 | inline float calcSquaredDiff(const PointT& a, const PointT& b, const float& wb) { 76 | float diffX = a.x - b.x * wb; 77 | float diffY = a.y - b.y * wb; 78 | float diffZ = a.z - b.z * wb; 79 | 80 | return diffX * diffX + diffY * diffY + diffZ * diffZ; 81 | } 82 | 83 | /** \brief Calculate the absolute distance of the point to the origin. 84 | * 85 | * @param p The point. 86 | * @return The distance to the point. 87 | */ 88 | template 89 | inline float calcPointDistance(const PointT& p) { 90 | return std::sqrt(p.x * p.x + p.y * p.y + p.z * p.z); 91 | } 92 | 93 | /** \brief Calculate the squared distance of the point to the origin. 94 | * 95 | * @param p The point. 96 | * @return The squared distance to the point. 97 | */ 98 | template 99 | inline float calcSquaredPointDistance(const PointT& p) { 100 | return p.x * p.x + p.y * p.y + p.z * p.z; 101 | } 102 | 103 | /** \brief Rotate the given point by the specified angle around the x-axis. 104 | * 105 | * @param p the point to rotate 106 | * @param ang the rotation angle 107 | */ 108 | template 109 | inline void rotX(PointT& p, const Angle& ang) { 110 | float y = p.y; 111 | p.y = ang.cos() * y - ang.sin() * p.z; 112 | p.z = ang.sin() * y + ang.cos() * p.z; 113 | } 114 | 115 | /** \brief Rotate the given point by the specified angle around the y-axis. 116 | * 117 | * @param p the point to rotate 118 | * @param ang the rotation angle 119 | */ 120 | template 121 | inline void rotY(PointT& p, const Angle& ang) { 122 | float x = p.x; 123 | p.x = ang.cos() * x + ang.sin() * p.z; 124 | p.z = ang.cos() * p.z - ang.sin() * x; 125 | } 126 | 127 | /** \brief Rotate the given point by the specified angle around the z-axis. 128 | * 129 | * @param p the point to rotate 130 | * @param ang the rotation angle 131 | */ 132 | template 133 | inline void rotZ(PointT& p, const Angle& ang) { 134 | float x = p.x; 135 | p.x = ang.cos() * x - ang.sin() * p.y; 136 | p.y = ang.sin() * x + ang.cos() * p.y; 137 | } 138 | 139 | /** \brief Rotate the given point by the specified angles around the z-, x- respectively y-axis. 140 | * 141 | * @param p the point to rotate 142 | * @param angZ the rotation angle around the z-axis 143 | * @param angX the rotation angle around the x-axis 144 | * @param angY the rotation angle around the y-axis 145 | */ 146 | template 147 | inline void rotateZXY(PointT& p, const Angle& angZ, const Angle& angX, const Angle& angY) { 148 | rotZ(p, angZ); 149 | rotX(p, angX); 150 | rotY(p, angY); 151 | } 152 | 153 | /** \brief Rotate the given point by the specified angles around the y-, x- respectively z-axis. 154 | * 155 | * @param p the point to rotate 156 | * @param angY the rotation angle around the y-axis 157 | * @param angX the rotation angle around the x-axis 158 | * @param angZ the rotation angle around the z-axis 159 | */ 160 | template 161 | inline void rotateYXZ(PointT& p, const Angle& angY, const Angle& angX, const Angle& angZ) { 162 | rotY(p, angY); 163 | rotX(p, angX); 164 | rotZ(p, angZ); 165 | } 166 | 167 | /** \brief Invert a homogenous transformation matrix 168 | * 169 | * @param m_in input 4x4 matrix 170 | * @param m_out inverted 4x4 matrix 171 | */ 172 | inline void invertHomogenousMatrix(const Eigen::Matrix4d& m_in, Eigen::Matrix4d& m_out) { 173 | m_out = Eigen::Matrix4d::Identity(); 174 | Eigen::Matrix3d R = m_in.block<3, 3>(0, 0); 175 | Eigen::Vector3d t = m_in.block<3, 1>(0, 3); 176 | m_out.block<3, 3>(0, 0) = R.transpose(); 177 | m_out.block<3, 1>(0, 3) = -R.transpose() * t; 178 | } 179 | 180 | } // namespace graph_msf 181 | 182 | #endif // GRAPH_MSF_MATH_UTILS_H 183 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/gnss/Gnss.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 by Dominic Jud, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GNSS_H 9 | #define GNSS_H 10 | 11 | #pragma once 12 | 13 | #include 14 | 15 | namespace graph_msf { 16 | 17 | class Gnss { 18 | public: 19 | Gnss(); 20 | virtual ~Gnss() = default; 21 | 22 | void setReference(const double& referenceLatitude, const double& referenceLongitude, const double& referenceAltitude, 23 | const double& referenceHeading); 24 | Eigen::Vector3d gpsToCartesian(const double& latitudeInDegrees, const double& longitudeInDegrees, const double& altitude); 25 | Eigen::Vector3d cartesianToGps(const Eigen::Matrix position); 26 | Eigen::Vector3d besselEllipsoidToMercator(const double& latitudeInRad, const double& longitudeInRad, const double& altitude); 27 | void setMercatorReferenceFrame(const Eigen::Vector3d newReferencePoint); 28 | 29 | protected: 30 | void calculateConversionParameters(); 31 | 32 | /// Reference longitude of the GPS measurements [deg] 33 | double referenceLongitude_ = 7.43958; 34 | 35 | /// Reference latitude of the GPS measurements [deg] 36 | double referenceLatitude_ = 46.95241; 37 | 38 | /// Reference altitude of the GPS measurements [m] 39 | double referenceAltitude_ = 0.0; 40 | 41 | /// Reference heading for the GPS measurements [rad] 42 | double referenceHeading_ = 0.0; 43 | 44 | // Conversion parameters 45 | double earthRadiusN_ = 0.0; 46 | double earthRadiusE_ = 0.0; 47 | 48 | // WGS84 constants for conversion 49 | const double equatorialRadius_ = 6378137.0; 50 | const double flattening_ = 1.0 / 298.257223563; 51 | // calculate WGS84 constants 52 | const double excentricity2_ = 2 * flattening_ - flattening_ * flattening_; 53 | 54 | // As described in : 55 | // https://www.swisstopo.admin.ch/content/swisstopo-internet/de/topics/survey/reference-systems/switzerland/_jcr_content/contentPar/tabs/items/dokumente_publikatio/tabPar/downloadlist/downloadItems/517_1459343190376.download/refsys_d.pdf 56 | // Major Axis Bessel Ellipsoid 57 | const double a_ = 6377397.155; 58 | // 1. numerical Excentricity Bessel Ellipsoid 59 | const double e2_ = 0.006674372230614; 60 | // Latitude Bern 61 | const double phi0_ = 46.95241 * M_PI / 180.0; 62 | // Longitude Bern 63 | const double lambda0_ = 7.43958 * M_PI / 180.0; 64 | // Projection Sphere Radius 65 | const double r_ = a_ * sqrt(1 - e2_) / (1 - e2_ * sin(phi0_) * sin(phi0_)); 66 | // Ratio Sphere Length to Ellipsoid Length; 67 | const double alpha_ = sqrt(1 + e2_ / (1 - e2_) * pow(cos(phi0_), 4)); 68 | // Latitude of Zero Point on Sphere 69 | const double b0_ = asin(sin(phi0_) / alpha_); 70 | // Konstante der Breitenformel 71 | const double k_ = log(tan(M_PI_4 + b0_ / 2.0)) - alpha_ * log(tan(M_PI_4 + phi0_ / 2.0)) + 72 | alpha_ * sqrt(e2_) / 2.0 * log((1 + sqrt(e2_) * sin(phi0_)) / (1 - sqrt(e2_) * sin(phi0_))); 73 | // xyz-offset 74 | Eigen::Vector3d xyzOffset_ = Eigen::Vector3d::Zero(); 75 | }; 76 | 77 | } // namespace graph_msf 78 | 79 | #endif // GNSS_H 80 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/gnss/GnssHandler.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_GNSSHANDLER_H 9 | #define GRAPH_MSF_GNSSHANDLER_H 10 | 11 | // C++ 12 | #include 13 | // Workspace 14 | #include "graph_msf/gnss/Gnss.h" 15 | 16 | // Defined macros 17 | #define GREEN_START "\033[92m" 18 | #define YELLOW_START "\033[33m" 19 | #define COLOR_END "\033[0m" 20 | 21 | namespace graph_msf { 22 | 23 | class GnssHandler { 24 | public: 25 | GnssHandler(); 26 | 27 | // Methods 28 | void initHandler(const Eigen::Vector3d& accumulatedLeftCoordinates, const Eigen::Vector3d& accumulatedRightCoordinates); 29 | 30 | void convertNavSatToPositions(const Eigen::Vector3d& leftGnssCoordinate, const Eigen::Vector3d& rightGnssCoordinate, 31 | Eigen::Vector3d& leftPosition, Eigen::Vector3d& rightPosition); 32 | double computeYaw(const Eigen::Vector3d& gnssPos1, const Eigen::Vector3d& gnssPos2); 33 | 34 | // Flags 35 | bool usingGnssReferenceFlag = false; 36 | 37 | // Setters 38 | void setGnssReferenceLatitude(const double gnssReferenceLatitude) { gnssReferenceLatitude_ = gnssReferenceLatitude; } 39 | void setGnssReferenceLongitude(const double gnssReferenceLongitude) { gnssReferenceLongitude_ = gnssReferenceLongitude; } 40 | void setGnssReferenceAltitude(const double gnssReferenceAltitude) { gnssReferenceAltitude_ = gnssReferenceAltitude; } 41 | void setGnssReferenceHeading(const double gnssReferenceHeading) { gnssReferenceHeading_ = gnssReferenceHeading; } 42 | 43 | private: 44 | // Member methods 45 | Eigen::Vector3d computeHeading_(const Eigen::Vector3d& gnssPos1, const Eigen::Vector3d& gnssPos2); 46 | //// Compute yaw from the heading vector 47 | static double computeYawFromHeadingVector_(const Eigen::Vector3d& headingVector); 48 | 49 | // Member variables 50 | Gnss gnssSensor_; 51 | Eigen::Vector3d W_t_W_GnssL0_; 52 | double globalAttitudeYaw_; 53 | 54 | // Reference Parameters 55 | double gnssReferenceLatitude_; 56 | double gnssReferenceLongitude_; 57 | double gnssReferenceAltitude_; 58 | double gnssReferenceHeading_; 59 | }; 60 | 61 | } // namespace graph_msf 62 | 63 | #endif // GRAPH_MSF_GNSSHANDLER_H 64 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/BinaryMeasurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_DELTAMEASUREMENT_H 9 | #define GRAPH_MSF_DELTAMEASUREMENT_H 10 | 11 | #include "graph_msf/measurements/Measurement.h" 12 | 13 | namespace graph_msf { 14 | 15 | struct BinaryMeasurement : public Measurement { 16 | public: 17 | BinaryMeasurement(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeKm1, 18 | const double timeK) 19 | : Measurement(measurementName, frameName, measurementRate, timeK), timeKm1_(timeKm1) {} 20 | 21 | virtual MeasurementType measurementType() override { return measurementType_; } 22 | 23 | protected: 24 | MeasurementType measurementType_ = MeasurementType::Binary; 25 | double timeKm1_; 26 | }; 27 | 28 | } // namespace graph_msf 29 | 30 | #endif // GRAPH_MSF_DELTAMEASUREMENT_H 31 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/BinaryMeasurement6D.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_DELTAMEASUREMENT6D_H 9 | #define GRAPH_MSF_DELTAMEASUREMENT6D_H 10 | 11 | #include "graph_msf/measurements/BinaryMeasurement.h" 12 | 13 | namespace graph_msf { 14 | 15 | struct BinaryMeasurement6D : public BinaryMeasurement { 16 | public: 17 | BinaryMeasurement6D(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeKm1, 18 | const double timeK, const Eigen::Matrix4d& T_delta, const Eigen::Matrix measurementNoise) 19 | : BinaryMeasurement(measurementName, frameName, measurementRate, timeKm1, timeK), 20 | T_delta_(T_delta), 21 | measurementNoise_(measurementNoise) {} 22 | 23 | const Eigen::Matrix4d& T_delta() const { return T_delta_; } 24 | const Eigen::Matrix& measurementNoise() const { return measurementNoise_; } 25 | 26 | protected: 27 | Eigen::Matrix4d T_delta_; 28 | Eigen::Matrix measurementNoise_; 29 | }; 30 | 31 | } // namespace graph_msf 32 | 33 | #endif // GRAPH_MSF_DELTAMEASUREMENT_H 34 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/Measurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_MEASUREMENT_H 9 | #define GRAPH_MSF_MEASUREMENT_H 10 | 11 | #include 12 | #include 13 | 14 | namespace graph_msf { 15 | 16 | // Enum that contains 2 possible measurement types 17 | enum class MeasurementType { Unary, Binary }; 18 | 19 | struct Measurement { 20 | public: 21 | Measurement(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeStamp) 22 | : measurementName_(measurementName), frameName_(frameName), measurementRate_(measurementRate), timeK_(timeStamp) {} 23 | 24 | // Public Methods 25 | const std::string& measurementName() const { return measurementName_; } 26 | const std::string& frameName() const { return frameName_; } 27 | int measurementRate() const { return measurementRate_; } 28 | double timeK() const { return timeK_; } 29 | 30 | // Pure Virtual Class 31 | virtual MeasurementType measurementType() = 0; 32 | 33 | protected: 34 | // Members 35 | std::string measurementName_; 36 | std::string frameName_; 37 | int measurementRate_; 38 | double timeK_; 39 | 40 | // Unknown at this level 41 | MeasurementType measurementType_; 42 | }; 43 | 44 | } // namespace graph_msf 45 | 46 | #endif // GRAPH_MSF_MEASUREMENT_H 47 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/UnaryMeasurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_UNARYMEASUREMENT_H 9 | #define GRAPH_MSF_UNARYMEASUREMENT_H 10 | 11 | #include "graph_msf/measurements/Measurement.h" 12 | 13 | namespace graph_msf { 14 | 15 | struct UnaryMeasurement : public Measurement { 16 | public: 17 | UnaryMeasurement(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeStamp) 18 | : Measurement(measurementName, frameName, measurementRate, timeStamp) {} 19 | 20 | virtual MeasurementType measurementType() override { return measurementType_; } 21 | 22 | protected: 23 | MeasurementType measurementType_ = MeasurementType::Unary; 24 | }; 25 | 26 | } // namespace graph_msf 27 | 28 | #endif // GRAPH_MSF_UNARYMEASUREMENT_H 29 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/UnaryMeasurement1D.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_UNARYMEASUREMENT1D_H 9 | #define GRAPH_MSF_UNARYMEASUREMENT1D_H 10 | 11 | #include "graph_msf/measurements/UnaryMeasurement.h" 12 | 13 | namespace graph_msf { 14 | 15 | struct UnaryMeasurement1D : public UnaryMeasurement { 16 | public: 17 | UnaryMeasurement1D(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeStamp, 18 | const double measurement, const double measurementNoise) 19 | : UnaryMeasurement(measurementName, frameName, measurementRate, timeStamp), 20 | measurement_(measurement), 21 | measurementNoise_(measurementNoise) {} 22 | 23 | const double& measurementValue() const { return measurement_; } 24 | const double& measurementNoise() const { return measurementNoise_; } 25 | 26 | protected: 27 | double measurement_; 28 | double measurementNoise_; 29 | }; 30 | 31 | } // namespace graph_msf 32 | 33 | #endif // GRAPH_MSF_UNARYMEASUREMENT_H 34 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/UnaryMeasurement3D.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_UNARYMEASUREMENT3D_H 9 | #define GRAPH_MSF_UNARYMEASUREMENT3D_H 10 | 11 | #include "graph_msf/measurements/UnaryMeasurement.h" 12 | 13 | namespace graph_msf { 14 | 15 | struct UnaryMeasurement3D : public UnaryMeasurement { 16 | public: 17 | UnaryMeasurement3D(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeStamp, 18 | const Eigen::Vector3d& measurement, const Eigen::Vector3d& measurementNoise) 19 | : UnaryMeasurement(measurementName, frameName, measurementRate, timeStamp), 20 | measurement_(measurement), 21 | measurementNoise_(measurementNoise) {} 22 | 23 | const Eigen::Vector3d& measurementVector() const { return measurement_; } 24 | const Eigen::Vector3d& measurementNoise() const { return measurementNoise_; } 25 | 26 | protected: 27 | Eigen::Vector3d measurement_; 28 | Eigen::Vector3d measurementNoise_; 29 | }; 30 | 31 | } // namespace graph_msf 32 | 33 | #endif // GRAPH_MSF_UNARYMEASUREMENT_H 34 | -------------------------------------------------------------------------------- /graph_msf/include/graph_msf/measurements/UnaryMeasurement6D.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef GRAPH_MSF_UNARYMEASUREMENT6D_H 9 | #define GRAPH_MSF_UNARYMEASUREMENT6D_H 10 | 11 | #include "graph_msf/measurements/UnaryMeasurement.h" 12 | 13 | namespace graph_msf { 14 | 15 | struct UnaryMeasurement6D : public UnaryMeasurement { 16 | public: 17 | UnaryMeasurement6D(const std::string& measurementName, const std::string& frameName, const int measurementRate, const double timeStamp, 18 | const Eigen::Matrix4d& measurementPose, const Eigen::Matrix& measurementNoise) 19 | : UnaryMeasurement(measurementName, frameName, measurementRate, timeStamp), 20 | measurementPose_(measurementPose), 21 | measurementNoise_(measurementNoise) {} 22 | 23 | const Eigen::Matrix4d& measurementPose() const { return measurementPose_; } 24 | const Eigen::Matrix& measurementNoise() const { return measurementNoise_; } 25 | 26 | protected: 27 | Eigen::Matrix4d measurementPose_; 28 | Eigen::Matrix measurementNoise_; 29 | }; 30 | 31 | } // namespace graph_msf 32 | 33 | #endif // GRAPH_MSF_UNARYMEASUREMENT_H 34 | -------------------------------------------------------------------------------- /graph_msf/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_msf 4 | 0.0.0 5 | 6 | 7 | State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry and GNSS estimates. 8 | 9 | 10 | Julian Nubert 11 | BSD 12 | Julian Nubert 13 | 14 | catkin 15 | eigen_conversions 16 | cmake_clang_tools 17 | 18 | eigen_conversions 19 | 20 | 21 | -------------------------------------------------------------------------------- /graph_msf/src/lib/Gnss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2017 by Dominic Jud, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #include "graph_msf/gnss/Gnss.h" 9 | 10 | namespace graph_msf { 11 | 12 | Gnss::Gnss() { 13 | calculateConversionParameters(); 14 | } 15 | 16 | void Gnss::calculateConversionParameters() { 17 | // calculate earth radii 18 | const double temp = 1.0 / (1.0 - excentricity2_ * sin(referenceLatitude_ * M_PI / 180.0) * sin(referenceLatitude_ * M_PI / 180.0)); 19 | const double prime_vertical_radius = equatorialRadius_ * sqrt(temp); 20 | earthRadiusN_ = prime_vertical_radius * (1 - excentricity2_) * temp; 21 | earthRadiusE_ = prime_vertical_radius * cos(referenceLatitude_ * M_PI / 180.0); 22 | } 23 | 24 | void Gnss::setReference(const double& referenceLatitude, const double& referenceLongitude, const double& referenceAltitude, 25 | const double& referenceHeading) { 26 | referenceLatitude_ = referenceLatitude; 27 | referenceLongitude_ = referenceLongitude; 28 | referenceAltitude_ = referenceAltitude; 29 | referenceHeading_ = referenceHeading; 30 | 31 | calculateConversionParameters(); 32 | } 33 | 34 | Eigen::Vector3d Gnss::gpsToCartesian(const double& latitudeInDegrees, const double& longitudeInDegrees, const double& altitude) { 35 | const double cn = cos(referenceHeading_); 36 | const double sn = sin(referenceHeading_); 37 | const double kn = 180.0 / earthRadiusN_ / M_PI; 38 | const double ke = 180.0 / earthRadiusE_ / M_PI; 39 | const double lat_tmp = (latitudeInDegrees - referenceLatitude_) / kn; 40 | const double lon_tmp = (longitudeInDegrees - referenceLongitude_) / ke; 41 | 42 | Eigen::Vector3d position; 43 | position(0) = cn * lat_tmp + sn * lon_tmp; 44 | position(1) = sn * lat_tmp - cn * lon_tmp; 45 | position(2) = altitude - referenceAltitude_; 46 | 47 | return position; 48 | } 49 | 50 | Eigen::Vector3d Gnss::cartesianToGps(const Eigen::Matrix position) { 51 | Eigen::Vector3d gpsCoordinates; 52 | gpsCoordinates(0) = 53 | referenceLatitude_ + (cos(referenceHeading_) * position(0) + sin(referenceHeading_) * position(1)) / earthRadiusN_ * 180.0 / M_PI; 54 | gpsCoordinates(1) = 55 | referenceLongitude_ - (-sin(referenceHeading_) * position(0) + cos(referenceHeading_) * position(1)) / earthRadiusE_ * 180.0 / M_PI; 56 | gpsCoordinates(2) = referenceAltitude_ + position(2); 57 | 58 | return gpsCoordinates; 59 | } 60 | 61 | void Gnss::setMercatorReferenceFrame(const Eigen::Vector3d newReferencePoint) { 62 | xyzOffset_ = newReferencePoint; 63 | } 64 | 65 | Eigen::Vector3d Gnss::besselEllipsoidToMercator(const double& latitudeInRad, const double& longitudeInRad, const double& altitude) { 66 | // Ellipsoid to Sphere (Gauss) 67 | const double S = alpha_ * log(tan(M_PI_4 + latitudeInRad / 2.0)) - 68 | alpha_ * sqrt(e2_) / 2.0 * log((1 + sqrt(e2_) * sin(latitudeInRad)) / (1 - sqrt(e2_) * sin(latitudeInRad))) + k_; 69 | const double b = 2 * (atan(exp(S)) - M_PI_4); 70 | const double l = alpha_ * (longitudeInRad - lambda0_); 71 | 72 | // Equator to Pseudoequator (Rotation) 73 | const double lHat = atan(sin(l) / (sin(b0_) * tan(b) + cos(b0_) * cos(l))); 74 | const double bHat = asin(cos(b0_) * sin(b) - sin(b0_) * cos(b) * cos(l)); 75 | 76 | // Sphere to Plane (Mercator) 77 | const double Y = r_ * lHat; 78 | const double X = r_ / 2.0 * log((1 + sin(bHat)) / (1 - sin(bHat))); 79 | 80 | return Eigen::Vector3d(Y, X, altitude) - xyzOffset_; // Yes, this is correct. A point in swiss coordinates is denoted as (y,x). See 81 | // https://en.wikipedia.org/wiki/Swiss_coordinate_system 82 | } 83 | 84 | } // namespace graph_msf 85 | -------------------------------------------------------------------------------- /graph_msf/src/lib/GnssHandler.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // C++ 9 | #include 10 | // Package 11 | #include "graph_msf/gnss/GnssHandler.h" 12 | 13 | namespace graph_msf { 14 | 15 | // Public ------------------------------------------------------------------- 16 | GnssHandler::GnssHandler() { 17 | std::cout << YELLOW_START << "GnssHandler" << GREEN_START << " Created Gnss Handler instance." << COLOR_END << std::endl; 18 | } 19 | 20 | void GnssHandler::initHandler(const Eigen::Vector3d& accumulatedLeftCoordinates, const Eigen::Vector3d& accumulatedRightCoordinates) { 21 | std::cout << YELLOW_START << "GnssHandler" << GREEN_START << " Initializing the handler." << COLOR_END << std::endl; 22 | 23 | // Initialize Gnss converter 24 | if (usingGnssReferenceFlag) { 25 | gnssSensor_.setReference(gnssReferenceLatitude_, gnssReferenceLongitude_, gnssReferenceAltitude_, gnssReferenceHeading_); 26 | } else { 27 | gnssSensor_.setReference(accumulatedLeftCoordinates(0), accumulatedLeftCoordinates(1), accumulatedLeftCoordinates(2), 0.0); 28 | } 29 | 30 | // Get Positions 31 | Eigen::Vector3d leftPosition, rightPosition; 32 | convertNavSatToPositions(accumulatedLeftCoordinates, accumulatedRightCoordinates, leftPosition, rightPosition); 33 | 34 | // Get heading (assuming that connection between antennas is perpendicular to heading) 35 | Eigen::Vector3d W_t_heading = computeHeading_(leftPosition, rightPosition); 36 | std::cout << YELLOW_START << "GnssHandler" << GREEN_START << " Heading read from the Gnss is the following: " << W_t_heading << std::endl; 37 | 38 | // Get initial global yaw 39 | globalAttitudeYaw_ = computeYawFromHeadingVector_(W_t_heading); 40 | std::cout << YELLOW_START << "GnssHandler" << GREEN_START << " Initial global yaw is: " << 180 / M_PI * globalAttitudeYaw_ << std::endl; 41 | 42 | // Initial Gnss position 43 | W_t_W_GnssL0_ = leftPosition; 44 | } 45 | 46 | void GnssHandler::convertNavSatToPositions(const Eigen::Vector3d& leftGnssCoordinate, const Eigen::Vector3d& rightGnssCoordinate, 47 | Eigen::Vector3d& leftPosition, Eigen::Vector3d& rightPosition) { 48 | /// Left 49 | leftPosition = gnssSensor_.gpsToCartesian(leftGnssCoordinate(0), leftGnssCoordinate(1), leftGnssCoordinate(2)); 50 | 51 | /// Right 52 | rightPosition = gnssSensor_.gpsToCartesian(rightGnssCoordinate(0), rightGnssCoordinate(1), rightGnssCoordinate(2)); 53 | } 54 | 55 | // Heading is defined as the orthogonal vector pointing from gnssPos2 to gnssPos1, projected to x,y-plane 56 | // Hence, if left and right gnss, then gnssPos1=leftGnss, gnssPos2=rightGnss 57 | double GnssHandler::computeYaw(const Eigen::Vector3d& gnssPos1, const Eigen::Vector3d& gnssPos2) { 58 | Eigen::Vector3d robotHeading = computeHeading_(gnssPos1, gnssPos2); 59 | return computeYawFromHeadingVector_(robotHeading); 60 | } 61 | 62 | // Private ------------------------------------------------------------------ 63 | 64 | // Heading is defined as the orthogonal vector pointing from gnssPos2 to gnssPos1, projected to x,y-plane 65 | Eigen::Vector3d GnssHandler::computeHeading_(const Eigen::Vector3d& gnssPos1, const Eigen::Vector3d& gnssPos2) { 66 | // Compute connecting unity vector 67 | Eigen::Vector3d W_t_GnssR_GnssL = (gnssPos1 - gnssPos2).normalized(); 68 | 69 | // Compute forward pointing vector 70 | Eigen::Vector3d zUnityVector = Eigen::Vector3d::UnitZ(); 71 | Eigen::Vector3d W_t_heading = W_t_GnssR_GnssL.cross(zUnityVector).normalized(); 72 | 73 | return W_t_heading; 74 | } 75 | 76 | double GnssHandler::computeYawFromHeadingVector_(const Eigen::Vector3d& headingVector) { 77 | return atan2(headingVector(1), headingVector(0)); 78 | } 79 | 80 | } // namespace graph_msf 81 | -------------------------------------------------------------------------------- /graph_msf/src/lib/GraphMsf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #include "graph_msf/GraphMsf.h" 9 | 10 | namespace graph_msf { 11 | 12 | // Public ----------------------------------------------------------- 13 | /// Constructor ----------- 14 | GraphMsf::GraphMsf() { 15 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Instance created." << COLOR_END << std::endl; 16 | } 17 | 18 | /// Setup ------------ 19 | bool GraphMsf::setup(std::shared_ptr graphConfigPtr, std::shared_ptr staticTransformsPtr) { 20 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Setting up." << COLOR_END << std::endl; 21 | 22 | // Graph Config 23 | graphConfigPtr_ = graphConfigPtr; 24 | staticTransformsPtr_ = staticTransformsPtr; 25 | 26 | graphMgrPtr_ = std::make_shared(graphConfigPtr_); 27 | 28 | // Configs 29 | graphMgrPtr_->getIsamParamsReference().findUnusedFactorSlots = graphConfigPtr_->findUnusedFactorSlots; 30 | graphMgrPtr_->getIsamParamsReference().setEnableDetailedResults(graphConfigPtr_->enableDetailedResults); 31 | graphMgrPtr_->getIsamParamsReference().setRelinearizeSkip(graphConfigPtr_->relinearizeSkip); 32 | graphMgrPtr_->getIsamParamsReference().setEnableRelinearization(graphConfigPtr_->enableRelinearization); 33 | graphMgrPtr_->getIsamParamsReference().setEvaluateNonlinearError(graphConfigPtr_->evaluateNonlinearError); 34 | graphMgrPtr_->getIsamParamsReference().setCacheLinearizedFactors(graphConfigPtr_->cacheLinearizedFactors); 35 | graphMgrPtr_->getIsamParamsReference().setEnablePartialRelinearizationCheck(graphConfigPtr_->enablePartialRelinearizationCheck); 36 | 37 | /// Initialize helper threads 38 | optimizeGraphThread_ = std::thread(&GraphMsf::optimizeGraph_, this); 39 | std::cout << YELLOW_START << "GMsf" << COLOR_END << " Initialized thread for optimizing the graph in parallel." << std::endl; 40 | 41 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Set up successfully." << COLOR_END << std::endl; 42 | return true; 43 | } 44 | 45 | bool GraphMsf::yawAndPositionInited() { 46 | return foundInitialYawAndPositionFlag_; 47 | } 48 | 49 | void GraphMsf::activateFallbackGraph() { 50 | if (graphConfigPtr_->usingFallbackGraphFlag) { 51 | graphMgrPtr_->activateFallbackGraph(); 52 | } else { 53 | std::cout << YELLOW_START << "GMsf" << RED_START << " Not activating fallback graph, disabled in config." << COLOR_END << std::endl; 54 | } 55 | } 56 | 57 | bool GraphMsf::initYawAndPosition(const double yaw_W_frame1, const std::string& frame1, const Eigen::Vector3d& W_t_W_frame2, 58 | const std::string& frame2) { 59 | // Transform yaw to imu frame 60 | gtsam::Rot3 yawR_W_frame1 = gtsam::Rot3::Yaw(yaw_W_frame1); 61 | gtsam::Rot3 yawR_W_I0; 62 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Setting yaw in " << frame1 << " frame." << COLOR_END << std::endl; 63 | yawR_W_I0 = 64 | yawR_W_frame1 * gtsam::Pose3(staticTransformsPtr_->rv_T_frame1_frame2(frame1, staticTransformsPtr_->getImuFrame())).rotation(); 65 | 66 | // Locking 67 | const std::lock_guard initYawAndPositionLock(initYawAndPositionMutex_); 68 | if (not alignedImuFlag_) { 69 | std::cout << YELLOW_START << "GMsf" << RED_START << " Tried to set initial yaw, but initial attitude is not yet set." << COLOR_END 70 | << std::endl; 71 | return false; 72 | } else if (not yawAndPositionInited()) { 73 | yaw_W_I0_ = yawR_W_I0.yaw(); 74 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Initial global yaw of imu in world has been set to (deg) " 75 | << 180.0 * yaw_W_I0_ / M_PI << "." << COLOR_END << std::endl; 76 | 77 | gtsam::Rot3 R_W_I0 = gtsam::Rot3::Ypr(yawR_W_I0.yaw(), imuAttitudePitch_, imuAttitudeRoll_); 78 | // Set Member Variables 79 | W_t_W_I0_ = W_t_W_Frame1_to_W_t_W_Frame2_(W_t_W_frame2, frame2, staticTransformsPtr_->getImuFrame(), R_W_I0); 80 | 81 | foundInitialYawAndPositionFlag_ = true; 82 | return true; 83 | } else { 84 | std::cout << YELLOW_START << "GMsf" << RED_START << " Tried to set initial yaw, but it has been set before." << COLOR_END << std::endl; 85 | return false; 86 | } 87 | } 88 | 89 | bool GraphMsf::initYawAndPosition(const Eigen::Matrix4d& T_O_frame, const std::string& frameName) { 90 | gtsam::Pose3 T_O_frame_gtsam(T_O_frame); 91 | return initYawAndPosition(T_O_frame_gtsam.rotation().yaw(), frameName, T_O_frame.block<3, 1>(0, 3), frameName); 92 | } 93 | 94 | // Private --------------------------------------------------------------- 95 | /// Callbacks ----------------------- 96 | bool GraphMsf::addImuMeasurement(const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, const double imuTimeK, 97 | std::shared_ptr& predictionPtr) { 98 | // Static Members 99 | static gtsam::Pose3 T_W_I_km1__; 100 | static int imuCabinCallbackCounter__ = -1; 101 | 102 | // Increase counter 103 | ++imuCabinCallbackCounter__; 104 | 105 | double imuTimeKm1 = imuTimeK_; 106 | imuTimeK_ = imuTimeK; 107 | 108 | // Filter out imu messages with same time stamp 109 | if (std::abs(imuTimeK_ - imuTimeKm1) < 1e-8) { 110 | std::cout << YELLOW_START << " GMsf" << RED_START << " Imu time " << std::setprecision(14) << imuTimeK << " was repeated." << COLOR_END 111 | << std::endl; 112 | return false; 113 | } 114 | 115 | // Add measurement to buffer 116 | graphMgrPtr_->addToIMUBuffer(imuTimeK, linearAcc, angularVel); 117 | 118 | // Variable of odometry 119 | gtsam::Pose3 T_O_Ik; 120 | gtsam::NavState T_W_Ik_nav; 121 | 122 | // Loop variables 123 | bool relocalizationFlag = false; // Edited by graphMgrPtr_->addImuFactorAndGetState 124 | 125 | // If IMU not yet gravity aligned 126 | if (!alignedImuFlag_) { // Not yet gravity aligned 127 | // Try to align 128 | if (!alignImu_()) { 129 | // Print only once per second 130 | if (imuCabinCallbackCounter__ % int(graphConfigPtr_->imuRate) == 0) { 131 | std::cout << YELLOW_START << "GMsf" << COLOR_END << " NOT ENOUGH IMU MESSAGES TO INITIALIZE POSE. WAITING FOR MORE..." << std::endl; 132 | } 133 | return false; 134 | } else { 135 | gtsam::Rot3 R_W_I0 = gtsam::Rot3::Ypr(0.0, imuAttitudePitch_, imuAttitudeRoll_); 136 | T_W_Ik_ = gtsam::Pose3(R_W_I0, gtsam::Point3(0.0, 0.0, 0.0)); 137 | T_W_O_ = gtsam::Pose3(); 138 | I_v_W_I_ = gtsam::Vector3(0.0, 0.0, 0.0); 139 | I_w_W_I_ = gtsam::Vector3(0.0, 0.0, 0.0); 140 | alignedImuFlag_ = true; 141 | 142 | return false; 143 | } 144 | } else if (!yawAndPositionInited()) { // Gravity Aligned but not yaw-aligned 145 | if (imuCabinCallbackCounter__ % int(graphConfigPtr_->imuRate) == 0) { 146 | std::cout << YELLOW_START << "GMsf" << COLOR_END << " IMU callback waiting for initialization of global yaw and initial position." 147 | << std::endl; 148 | } 149 | T_W_Ik_nav = gtsam::NavState(T_W_Ik_, I_v_W_I_); 150 | } else if (!initedGraphFlag_) { // Initialization 151 | initGraph_(imuTimeK); 152 | // if (!graphConfigPtr_->usingGnssFlag) { 153 | // // TODO 154 | // //activateFallbackGraph(); 155 | // } 156 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " ...graph is initialized." << COLOR_END << std::endl; 157 | T_W_Ik_nav = gtsam::NavState(T_W_Ik_, I_v_W_I_); 158 | relocalizationFlag = true; 159 | } else { // Normal operation 160 | normalOperationFlag_ = true; 161 | // Add IMU factor and get propagated state 162 | T_W_Ik_nav = graphMgrPtr_->addImuFactorAndGetState(imuTimeK, linearAcc, angularVel, relocalizationFlag); 163 | } 164 | 165 | // Assign poses and velocities --------------------------------------------------- 166 | T_W_Ik_ = T_W_Ik_nav.pose(); 167 | I_v_W_I_ = T_W_Ik_nav.bodyVelocity(); 168 | I_w_W_I_ = graphMgrPtr_->getIMUBias().correctGyroscope(angularVel); 169 | imuAttitudeRoll_ = T_W_Ik_.rotation().roll(); 170 | imuAttitudePitch_ = T_W_Ik_.rotation().pitch(); 171 | 172 | // If relocalization happens --> write to map->odom ------------------------------------ 173 | if (relocalizationFlag) { 174 | // Print 175 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Relocalization is needed. Publishing to map->odom." << COLOR_END << std::endl; 176 | // For this computation step assume T_O_Ik ~ T_O_Ikm1 177 | gtsam::Pose3 T_I_O_km1 = T_W_I_km1__.inverse() * T_W_O_; 178 | T_W_O_ = T_W_Ik_ * T_I_O_km1; 179 | } 180 | 181 | // Convert to odom frame ---------------------------------------------------------------- 182 | T_O_Ik = T_W_O_.inverse() * T_W_Ik_; 183 | 184 | // Estimate state 185 | // if (receivedOdometryFlag_) {} 186 | 187 | // Publish 188 | predictionPtr = 189 | std::make_shared(T_W_O_.matrix(), T_O_Ik.matrix(), Eigen::Vector3d(I_v_W_I_), Eigen::Vector3d(I_w_W_I_)); 190 | 191 | // Write for next iteration 192 | T_W_I_km1__ = T_W_Ik_; 193 | return true; 194 | } 195 | 196 | void GraphMsf::addOdometryMeasurement(const BinaryMeasurement6D& delta) {} // TODO 197 | 198 | void GraphMsf::addUnaryPoseMeasurement(const UnaryMeasurement6D& unary) { 199 | // Only take actions if graph has been initialized 200 | if (!initedGraphFlag_) { 201 | return; 202 | } 203 | 204 | gtsam::Pose3 T_W_frame(unary.measurementPose()); 205 | gtsam::Pose3 T_W_I = 206 | T_W_frame * gtsam::Pose3(staticTransformsPtr_->rv_T_frame1_frame2(unary.frameName(), staticTransformsPtr_->getImuFrame())); 207 | 208 | if (initedGraphFlag_) { 209 | graphMgrPtr_->addPoseUnaryFactorToGlobalGraph(unary.timeK(), unary.measurementRate(), unary.measurementNoise(), T_W_I); 210 | graphMgrPtr_->addPoseUnaryFactorToFallbackGraph(unary.timeK(), unary.measurementRate(), unary.measurementNoise(), T_W_I); 211 | 212 | // Optimize 213 | { 214 | // Mutex for optimizeGraph Flag 215 | const std::lock_guard optimizeGraphLock(optimizeGraphMutex_); 216 | optimizeGraphFlag_ = true; 217 | } 218 | } 219 | if (!receivedOdometryFlag_) { 220 | receivedOdometryFlag_ = true; 221 | } 222 | } 223 | 224 | void GraphMsf::addDualOdometryMeasurement(const UnaryMeasurement6D& odometryKm1, const UnaryMeasurement6D& odometryK, 225 | const Eigen::Matrix& poseBetweenNoise) { 226 | // Only take actions if graph has been initialized 227 | if (!initedGraphFlag_) { 228 | return; 229 | } 230 | 231 | // Static variables 232 | static bool lidarUnaryFactorInitialized__ = false; 233 | static Eigen::Matrix4d T_Wl_Lj__; 234 | static gtsam::Pose3 T_W_Ij_Graph__; 235 | static gtsam::Key lastDeltaMeasurementKey__; 236 | 237 | // Create Pseudo Unary Factor 238 | if (graphMgrPtr_->globalGraphActiveFlag()) { 239 | /// Reset LiDAR Unary factor intiialization 240 | lidarUnaryFactorInitialized__ = false; 241 | // Write current odometry pose to latest delta pose 242 | T_Wl_Lj__ = odometryK.measurementPose(); 243 | } // 244 | else if (graphMgrPtr_->fallbackGraphActiveFlag()) { 245 | if (!lidarUnaryFactorInitialized__) { 246 | // Calculate state still from globalGraph 247 | T_W_Ij_Graph__ = graphMgrPtr_->calculateActiveStateAtKey(lastDeltaMeasurementKey__).pose(); 248 | std::cout << YELLOW_START << "GMsf" << GREEN_START " Initialized LiDAR unary factors." << COLOR_END << std::endl; 249 | lidarUnaryFactorInitialized__ = true; 250 | } 251 | /// Delta pose 252 | gtsam::Pose3 T_Ij_Ik(staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), odometryKm1.frameName()) * 253 | T_Wl_Lj__.inverse() * odometryK.measurementPose() * 254 | staticTransformsPtr_->rv_T_frame1_frame2(odometryK.frameName(), staticTransformsPtr_->getImuFrame())); 255 | gtsam::Pose3 pseudo_T_W_Ik = T_W_Ij_Graph__ * T_Ij_Ik; 256 | graphMgrPtr_->addPoseUnaryFactorToFallbackGraph(odometryK.timeK(), odometryK.measurementRate(), odometryK.measurementNoise(), 257 | pseudo_T_W_Ik); 258 | } 259 | /// Delta pose 260 | Eigen::Matrix4d T_Lkm1_Lk = odometryKm1.measurementPose().inverse() * odometryK.measurementPose(); 261 | Eigen::Matrix4d T_Ikm1_Ik = staticTransformsPtr_->rv_T_frame1_frame2(staticTransformsPtr_->getImuFrame(), odometryKm1.frameName()) * 262 | T_Lkm1_Lk * 263 | staticTransformsPtr_->rv_T_frame1_frame2(odometryK.frameName(), staticTransformsPtr_->getImuFrame()); 264 | lastDeltaMeasurementKey__ = graphMgrPtr_->addPoseBetweenFactorToGlobalGraph( 265 | odometryKm1.timeK(), odometryK.timeK(), odometryK.measurementRate(), poseBetweenNoise, gtsam::Pose3(T_Ikm1_Ik)); 266 | 267 | // Optimize 268 | { 269 | // Mutex for optimizeGraph Flag 270 | const std::lock_guard optimizeGraphLock(optimizeGraphMutex_); 271 | optimizeGraphFlag_ = true; 272 | } 273 | 274 | if (!receivedOdometryFlag_) { 275 | receivedOdometryFlag_ = true; 276 | } 277 | } 278 | 279 | void GraphMsf::addDualGnssPositionMeasurement(const UnaryMeasurement3D& W_t_W_frame, const Eigen::Vector3d& W_t_W_frame_km1, 280 | const Eigen::Vector3d& estCovarianceXYZ) { 281 | // Only take actions if graph has been initialized 282 | if (!initedGraphFlag_) { 283 | return; 284 | } 285 | 286 | // Read covariance 287 | bool gnssCovarianceViolatedFlag = estCovarianceXYZ(0) > GNSS_COVARIANCE_VIOLATION_THRESHOLD || 288 | estCovarianceXYZ(1) > GNSS_COVARIANCE_VIOLATION_THRESHOLD || 289 | estCovarianceXYZ(2) > GNSS_COVARIANCE_VIOLATION_THRESHOLD; 290 | if (gnssCovarianceViolatedFlag && !gnssCovarianceViolatedFlag_) { 291 | std::cout << YELLOW_START << "GMsf" << RED_START << " Gnss measurments now ABSENT due to too big covariance." << std::endl; 292 | gnssCovarianceViolatedFlag_ = true; 293 | } else if (!gnssCovarianceViolatedFlag && gnssCovarianceViolatedFlag_) { 294 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Gnss returned. Low covariance." << std::endl; 295 | gnssCovarianceViolatedFlag_ = false; 296 | } 297 | 298 | // Gnss jumping? 299 | double jumpingDistance = (W_t_W_frame_km1 - W_t_W_frame.measurementVector()).norm(); 300 | if (jumpingDistance < graphConfigPtr_->gnssOutlierThresold) { // gnssOutlierThreshold_) { 301 | ++gnssNotJumpingCounter_; 302 | if (gnssNotJumpingCounter_ == REQUIRED_GNSS_NUM_NOT_JUMPED) { 303 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Gnss was not jumping recently. Jumping counter valid again." << COLOR_END 304 | << std::endl; 305 | } 306 | } else { 307 | if (gnssNotJumpingCounter_ >= REQUIRED_GNSS_NUM_NOT_JUMPED) { 308 | std::cout << YELLOW_START << "GMsf" << RED_START << " Gnss was jumping: Distance is " << jumpingDistance << "m, larger than allowed " 309 | << 1.0 // gnssOutlierThreshold_ 310 | << "m. Reset outlier counter." << COLOR_END << std::endl; 311 | } 312 | gnssNotJumpingCounter_ = 0; 313 | } 314 | 315 | // Case: Gnss is good --> Write to graph and perform logic 316 | if (!gnssCovarianceViolatedFlag_ && (gnssNotJumpingCounter_ >= REQUIRED_GNSS_NUM_NOT_JUMPED)) { 317 | // Position factor 318 | addGnssPositionMeasurement(W_t_W_frame); 319 | graphMgrPtr_->activateGlobalGraph(); 320 | } 321 | // Case: Gnss is bad --> Do not write to graph, set flags for odometry unary factor to true 322 | else if (usingFallbackGraphFlag_) { 323 | graphMgrPtr_->activateFallbackGraph(); 324 | } 325 | } 326 | 327 | void GraphMsf::addGnssPositionMeasurement(const UnaryMeasurement3D& W_t_W_frame) { 328 | // Only take actions if graph has been initialized 329 | if (!initedGraphFlag_) { 330 | return; 331 | } 332 | 333 | // Add unary factor 334 | gtsam::Point3 W_t_W_I = W_t_W_Frame1_to_W_t_W_Frame2_(W_t_W_frame.measurementVector(), W_t_W_frame.frameName(), 335 | staticTransformsPtr_->getImuFrame(), T_W_Ik_.rotation()); 336 | if (graphMgrPtr_->getStateKey() == 0) { 337 | return; 338 | } 339 | graphMgrPtr_->addGnssPositionUnaryFactor(W_t_W_frame.timeK(), W_t_W_frame.measurementRate(), W_t_W_frame.measurementNoise(), W_t_W_I); 340 | { 341 | // Mutex for optimizeGraph Flag 342 | const std::lock_guard optimizeGraphLock(optimizeGraphMutex_); 343 | optimizeGraphFlag_ = true; 344 | } 345 | } 346 | 347 | void GraphMsf::addGnssHeadingMeasurement(const UnaryMeasurement1D& yaw_W_frame) { 348 | // Only take actions if graph has been initialized 349 | if (!initedGraphFlag_) { 350 | return; 351 | } 352 | 353 | // Transform yaw to imu frame 354 | gtsam::Rot3 yawR_W_frame = gtsam::Rot3::Yaw(yaw_W_frame.measurementValue()); 355 | gtsam::Rot3 yawR_W_I0 = 356 | yawR_W_frame * 357 | gtsam::Pose3(staticTransformsPtr_->rv_T_frame1_frame2(yaw_W_frame.frameName(), staticTransformsPtr_->getImuFrame())).rotation(); 358 | 359 | if (!gnssCovarianceViolatedFlag_ && (gnssNotJumpingCounter_ >= REQUIRED_GNSS_NUM_NOT_JUMPED)) { 360 | graphMgrPtr_->addGnssHeadingUnaryFactor(yaw_W_frame.timeK(), yaw_W_frame.measurementRate(), yaw_W_frame.measurementNoise(), 361 | yawR_W_I0.yaw()); 362 | { 363 | // Mutex for optimizeGraph Flag 364 | const std::lock_guard optimizeGraphLock(optimizeGraphMutex_); 365 | optimizeGraphFlag_ = true; 366 | } 367 | } 368 | } 369 | 370 | /// Worker Functions ----------------------- 371 | bool GraphMsf::alignImu_() { 372 | gtsam::Rot3 imuAttitude; 373 | static int alignImuCounter__ = -1; 374 | ++alignImuCounter__; 375 | if (graphMgrPtr_->estimateAttitudeFromImu(graphConfigPtr_->imuGravityDirection, imuAttitude, gravityConstant_, 376 | graphMgrPtr_->getInitGyrBiasReference())) { 377 | imuAttitudeRoll_ = imuAttitude.roll(); 378 | imuAttitudePitch_ = imuAttitude.pitch(); 379 | std::cout << YELLOW_START << "GMsf" << COLOR_END 380 | << " Attitude of IMU is initialized. Determined Gravity Magnitude: " << gravityConstant_ << std::endl; 381 | return true; 382 | } else { 383 | return false; 384 | } 385 | } 386 | 387 | // Graph initialization for roll & pitch from starting attitude, assume zero yaw 388 | void GraphMsf::initGraph_(const double timeStamp_k) { 389 | // Calculate initial attitude; 390 | gtsam::Rot3 yawR_W_I0 = gtsam::Rot3::Yaw(yaw_W_I0_); 391 | // gtsam::Rot3 yawR_W_I0 = yawR_W_C0 * tfToPose3(staticTransformsPtr_->T_C_Ic()).rotation(); 392 | gtsam::Rot3 R_W_I0 = gtsam::Rot3::Ypr(yawR_W_I0.yaw(), imuAttitudePitch_, imuAttitudeRoll_); 393 | 394 | std::cout << YELLOW_START << "GMsf" << GREEN_START 395 | << " Total initial IMU attitude is Yaw/Pitch/Roll(deg): " << R_W_I0.ypr().transpose() * (180.0 / M_PI) << COLOR_END 396 | << std::endl; 397 | 398 | // Gravity 399 | graphMgrPtr_->initImuIntegrators(gravityConstant_, graphConfigPtr_->imuGravityDirection); 400 | /// Add initial IMU translation based on intial orientation 401 | gtsam::Pose3 T_W_I0; 402 | 403 | T_W_I0 = gtsam::Pose3(R_W_I0, W_t_W_I0_); 404 | 405 | /// Initialize graph node 406 | graphMgrPtr_->initPoseVelocityBiasGraph(timeStamp_k, T_W_I0); 407 | // if (graphConfigPtr_->usingGnssFlag && usingFallbackGraphFlag_) { 408 | // graphMgrPtr_->activateFallbackGraph(); 409 | // } 410 | // Read initial pose from graph 411 | T_W_I0 = gtsam::Pose3(graphMgrPtr_->getGraphState().navState().pose().matrix()); 412 | std::cout << YELLOW_START << "GMsf " << GREEN_START << " INIT t(x,y,z): " << T_W_I0.translation().transpose() 413 | << ", RPY(deg): " << T_W_I0.rotation().rpy().transpose() * (180.0 / M_PI) << COLOR_END << std::endl; 414 | std::cout << YELLOW_START << "GMsf" << COLOR_END << " Factor graph key of very first node: " << graphMgrPtr_->getStateKey() << std::endl; 415 | // Write in tf member variable 416 | T_W_I0_ = T_W_I0; 417 | // Initialize global pose 418 | T_W_Ik_ = T_W_I0_; 419 | imuTimeK_ = timeStamp_k; 420 | // Set flag 421 | initedGraphFlag_ = true; 422 | } 423 | 424 | void GraphMsf::optimizeGraph_() { 425 | // Timing 426 | std::chrono::time_point startLoopTime; 427 | std::chrono::time_point endLoopTime; 428 | 429 | // While loop 430 | std::cout << YELLOW_START << "GMsf" << COLOR_END << " Thread for updating graph is ready." << std::endl; 431 | while (true) { 432 | bool optimizeGraphFlag = false; 433 | // Mutex for optimizeGraph Flag 434 | { 435 | // Lock 436 | const std::lock_guard optimizeGraphLock(optimizeGraphMutex_); 437 | if (optimizeGraphFlag_) { 438 | optimizeGraphFlag = optimizeGraphFlag_; 439 | optimizeGraphFlag_ = false; 440 | } 441 | } 442 | 443 | if (optimizeGraphFlag) { 444 | // Get result 445 | startLoopTime = std::chrono::high_resolution_clock::now(); 446 | double currentTime; 447 | gtsam::NavState optimizedNavState = graphMgrPtr_->updateActiveGraphAndGetState(currentTime); 448 | endLoopTime = std::chrono::high_resolution_clock::now(); 449 | 450 | if (graphConfigPtr_->verboseLevel > 0) { 451 | std::cout << YELLOW_START << "GMsf" << GREEN_START << " Whole optimization loop took " 452 | << std::chrono::duration_cast(endLoopTime - startLoopTime).count() << " milliseconds." 453 | << COLOR_END << std::endl; 454 | } 455 | // Transform pose 456 | // TODO proper datatype 457 | T_W_I_opt_ = optimizedNavState.pose(); 458 | optTime_ = currentTime; 459 | 460 | long seconds = long(currentTime); 461 | 462 | } // else just sleep for a short amount of time before polling again 463 | else { 464 | std::this_thread::sleep_for(std::chrono::milliseconds(1)); 465 | } 466 | } 467 | } 468 | 469 | gtsam::Vector3 GraphMsf::W_t_W_Frame1_to_W_t_W_Frame2_(const gtsam::Point3& W_t_W_frame1, const std::string& frame1, 470 | const std::string& frame2, const gtsam::Rot3& R_W_frame2) { 471 | // Static transforms 472 | const Eigen::Matrix4d& T_frame1_frame2 = staticTransformsPtr_->rv_T_frame1_frame2(frame1, frame2); 473 | const Eigen::Matrix4d& T_frame2_frame1 = staticTransformsPtr_->rv_T_frame1_frame2(frame2, frame1); 474 | Eigen::Vector3d frame1_t_frame1_frame2 = T_frame1_frame2.block<3, 1>(0, 3); 475 | 476 | /// Global rotation 477 | gtsam::Rot3 R_W_frame1 = R_W_frame2 * gtsam::Pose3(T_frame2_frame1).rotation(); 478 | 479 | /// Translation in global frame 480 | Eigen::Vector3d W_t_frame1_frame2 = R_W_frame1 * frame1_t_frame1_frame2; 481 | 482 | /// Shift observed Gnss position to IMU frame (instead of Gnss antenna) 483 | return W_t_W_frame1 + W_t_frame1_frame2; 484 | } 485 | 486 | } // namespace graph_msf 487 | -------------------------------------------------------------------------------- /graph_msf/src/lib/GraphMsfInterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Package 9 | #include "graph_msf/GraphMsfInterface.h" 10 | #include "graph_msf/GraphMsf.h" 11 | 12 | namespace graph_msf { 13 | 14 | // Public ------------------------------------------------------------------------------------- 15 | GraphMsfInterface::GraphMsfInterface() { 16 | std::cout << YELLOW_START << "GMsf-Interface" << GREEN_START << " Instance created." << COLOR_END << std::endl; 17 | } 18 | 19 | // Protected ------------------------------------------------------------------------------------ 20 | bool GraphMsfInterface::setup_() { 21 | std::cout << YELLOW_START << "GMsf-Interface" << GREEN_START << " Setting up." << COLOR_END << std::endl; 22 | 23 | if (!graphConfigPtr_ || !staticTransformsPtr_) { 24 | std::runtime_error("GMsf-Interface::setup_(): graphConfigPtr_ or staticTransformsPtr_ is not set."); 25 | } 26 | 27 | graphMsfPtr_ = std::make_shared(); 28 | graphMsfPtr_->setup(graphConfigPtr_, staticTransformsPtr_); 29 | 30 | std::cout << YELLOW_START << "GMsf-Interface" << GREEN_START << " Set up successfully." << COLOR_END << std::endl; 31 | return true; 32 | } 33 | 34 | bool GraphMsfInterface::initYawAndPosition_(const double yaw_W_frame1, const std::string& frame1, const Eigen::Vector3d& W_t_W_frame2, 35 | const std::string& frame2) { 36 | return graphMsfPtr_->initYawAndPosition(yaw_W_frame1, frame1, W_t_W_frame2, frame2); 37 | } 38 | 39 | bool GraphMsfInterface::initYawAndPosition_(const Eigen::Matrix4d& T_W_frame, const std::string& frameName) { 40 | return graphMsfPtr_->initYawAndPosition(T_W_frame, frameName); 41 | } 42 | 43 | bool GraphMsfInterface::areYawAndPositionInited_() { 44 | return graphMsfPtr_->yawAndPositionInited(); 45 | } 46 | 47 | void GraphMsfInterface::activateFallbackGraph() { 48 | graphMsfPtr_->activateFallbackGraph(); 49 | } 50 | 51 | void GraphMsfInterface::addImuMeasurementAndPublishState_(const Eigen::Vector3d& linearAcc, const Eigen::Vector3d& angularVel, 52 | const double imuTimeK) { 53 | static int imuCabinCallbackCounter__ = -1; 54 | 55 | ++imuCabinCallbackCounter__; 56 | 57 | std::shared_ptr predictionPtr; 58 | 59 | bool success = graphMsfPtr_->addImuMeasurement(linearAcc, angularVel, imuTimeK, predictionPtr); 60 | 61 | if (success) { 62 | publishStateAndMeasureTime_(imuTimeK, predictionPtr->T_W_O, predictionPtr->T_O_Ik, predictionPtr->I_v_W_I, predictionPtr->I_w_W_I); 63 | } 64 | } 65 | 66 | void GraphMsfInterface::addOdometryMeasurement_(const BinaryMeasurement6D& delta) { 67 | graphMsfPtr_->addOdometryMeasurement(delta); 68 | } 69 | 70 | void GraphMsfInterface::addUnaryPoseMeasurement_(const UnaryMeasurement6D& unary) { 71 | graphMsfPtr_->addUnaryPoseMeasurement(unary); 72 | } 73 | 74 | void GraphMsfInterface::addDualOdometryMeasurement_(const UnaryMeasurement6D& odometryKm1, const UnaryMeasurement6D& odometryK, 75 | const Eigen::Matrix& poseBetweenNoise) { 76 | graphMsfPtr_->addDualOdometryMeasurement(odometryKm1, odometryK, poseBetweenNoise); 77 | } 78 | 79 | void GraphMsfInterface::addDualGnssPositionMeasurement_(const UnaryMeasurement3D& W_t_W_frame, const Eigen::Vector3d& lastPosition, 80 | const Eigen::Vector3d& estCovarianceXYZ) { 81 | graphMsfPtr_->addDualGnssPositionMeasurement(W_t_W_frame, lastPosition, estCovarianceXYZ); 82 | } 83 | 84 | void GraphMsfInterface::addGnssPositionMeasurement_(const UnaryMeasurement3D& W_t_W_frame) { 85 | graphMsfPtr_->addGnssPositionMeasurement(W_t_W_frame); 86 | } 87 | 88 | void GraphMsfInterface::addGnssHeadingMeasurement_(const UnaryMeasurement1D& yaw_W_frame) { 89 | graphMsfPtr_->addGnssHeadingMeasurement(yaw_W_frame); 90 | } 91 | 92 | void GraphMsfInterface::publishStateAndMeasureTime_(const double imuTimeK, const Eigen::Matrix4d& T_W_O, const Eigen::Matrix4d& T_O_Ik, 93 | const Eigen::Vector3d& I_v_W_I, const Eigen::Vector3d& I_w_W_I) { 94 | // Define variables for timing 95 | std::chrono::time_point startLoopTime; 96 | std::chrono::time_point endLoopTime; 97 | 98 | startLoopTime = std::chrono::high_resolution_clock::now(); 99 | publishState_(imuTimeK, T_W_O, T_O_Ik, I_v_W_I, I_w_W_I); 100 | endLoopTime = std::chrono::high_resolution_clock::now(); 101 | if (std::chrono::duration_cast(endLoopTime - startLoopTime).count() > (1e6 / graphConfigPtr_->imuRate / 2.0)) { 102 | std::cout << YELLOW_START << "GMsf-Interface" << RED_START << " Publishing state took " 103 | << std::chrono::duration_cast(endLoopTime - startLoopTime).count() 104 | << " microseconds, which is slower than double IMU-rate (" << (1e6 / graphConfigPtr_->imuRate / 2.0) << " microseconds)." 105 | << COLOR_END << std::endl; 106 | } 107 | } 108 | 109 | bool GraphMsfInterface::isInNormalOperation() const { 110 | return graphMsfPtr_->getNormalOperationFlag(); 111 | } 112 | 113 | } // namespace graph_msf 114 | -------------------------------------------------------------------------------- /graph_msf/src/lib/ImuBuffer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Implementation 9 | #include "graph_msf/ImuBuffer.hpp" 10 | 11 | // CPP 12 | #include 13 | 14 | namespace graph_msf { 15 | 16 | // Public -------------------------------------------------------- 17 | void ImuBuffer::addToIMUBuffer(double ts, double accX, double accY, double accZ, double gyrX, double gyrY, double gyrZ) { 18 | // Check that imuBufferLength was set 19 | if (imuBufferLength_ < 0) { 20 | throw std::runtime_error("GMsfImuBuffer: imuBufferLength has to be set by the user."); 21 | } 22 | 23 | // Convert to gtsam type 24 | gtsam::Vector6 imuMeas; 25 | imuMeas << accX, accY, accZ, gyrX, gyrY, gyrZ; 26 | 27 | // Add to buffer 28 | { 29 | // Writing to IMU buffer --> acquire mutex 30 | const std::lock_guard writeInBufferLock(writeInBufferMutex_); 31 | timeToImuBuffer_[ts] = imuMeas; 32 | } 33 | if (ts > tLatestInBuffer_) { 34 | tLatestInBuffer_ = ts; 35 | } 36 | 37 | // If IMU buffer is too large, remove first element 38 | if (timeToImuBuffer_.size() > imuBufferLength_) { 39 | timeToImuBuffer_.erase(timeToImuBuffer_.begin()); 40 | } 41 | 42 | if (timeToImuBuffer_.size() > imuBufferLength_) { 43 | std::ostringstream errorStream; 44 | errorStream << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " IMU Buffer has grown too large. It contains " 45 | << timeToImuBuffer_.size() << " measurements instead of " << imuBufferLength_ << "."; 46 | throw std::runtime_error(errorStream.str()); 47 | } 48 | } 49 | 50 | void ImuBuffer::addToKeyBuffer(double ts, gtsam::Key key) { 51 | if (verboseLevel_ >= 5) { 52 | std::cout << YELLOW_START << "GMsf-Imu-Buffer" << COLOR_END << " Adding key " << key << " to timeToKeyBuffer for time " << ts 53 | << std::endl; 54 | } 55 | { 56 | // Writing to IMU buffer --> acquire mutex 57 | const std::lock_guard writeInBufferLock(writeInBufferMutex_); 58 | timeToKeyBuffer_[ts] = key; 59 | } 60 | 61 | // If Key buffer is too large, remove first element 62 | if (timeToKeyBuffer_.size() > imuBufferLength_) { 63 | timeToKeyBuffer_.erase(timeToKeyBuffer_.begin()); 64 | } 65 | 66 | if (timeToKeyBuffer_.size() > imuBufferLength_) { 67 | std::ostringstream errorStream; 68 | errorStream << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Key Buffer has grown too large. It contains " 69 | << timeToKeyBuffer_.size() << " measurements instead of " << imuBufferLength_ << "."; 70 | throw std::runtime_error(errorStream.str()); 71 | } 72 | } 73 | 74 | void ImuBuffer::getLastTwoMeasurements(TimeToImuMap& imuMap) { 75 | TimeToImuMap::iterator endItr = --(timeToImuBuffer_.end()); 76 | TimeToImuMap::iterator previousItr = --(--(timeToImuBuffer_.end())); 77 | 78 | // Write into IMU Map 79 | imuMap[previousItr->first] = previousItr->second; 80 | imuMap[endItr->first] = endItr->second; 81 | } 82 | 83 | bool ImuBuffer::getClosestKeyAndTimestamp(double& tInGraph, gtsam::Key& key, const std::string& callingName, 84 | const double maxSearchDeviation, const double tK) { 85 | std::_Rb_tree_iterator> upperIterator; 86 | { 87 | // Read frp, IMU buffer --> acquire mutex 88 | const std::lock_guard writeInBufferLock(writeInBufferMutex_); 89 | upperIterator = timeToKeyBuffer_.upper_bound(tK); 90 | } 91 | 92 | auto lowerIterator = upperIterator; 93 | --lowerIterator; 94 | 95 | // Keep key which is closer to tLidar 96 | tInGraph = std::abs(tK - lowerIterator->first) < std::abs(upperIterator->first - tK) ? lowerIterator->first : upperIterator->first; 97 | key = std::abs(tK - lowerIterator->first) < std::abs(upperIterator->first - tK) ? lowerIterator->second : upperIterator->second; 98 | double timeDeviation = tInGraph - tK; 99 | 100 | if (verboseLevel_ >= 2) { 101 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " " << callingName << std::setprecision(14) 102 | << " searched time step: " << tK << std::endl; 103 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " " << callingName << std::setprecision(14) 104 | << " found time step: " << tInGraph << " at key " << key << std::endl; 105 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Time Deviation (t_graph-t_request): " << 1000 * timeDeviation << " ms" 106 | << std::endl; 107 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Latest IMU timestamp: " << tLatestInBuffer_ 108 | << ", hence absolut delay of measurement is " << 1000 * (tLatestInBuffer_ - tK) << "ms." << std::endl; 109 | } 110 | 111 | // Check for error and warn user 112 | if (std::abs(timeDeviation) > maxSearchDeviation) { 113 | std::cerr << YELLOW_START << "GMsf-ImuBuffer " << RED_START << callingName << " Time deviation at key " << key << " is " 114 | << 1000 * timeDeviation << " ms, being larger than admissible deviation of " << 1000 * maxSearchDeviation << " ms" 115 | << COLOR_END << std::endl; 116 | return false; 117 | } 118 | 119 | return true; 120 | } 121 | 122 | bool ImuBuffer::getIMUBufferIteratorsInInterval(const double& ts_start, const double& ts_end, TimeToImuMap::iterator& s_itr, 123 | TimeToImuMap::iterator& e_itr) { 124 | // Check if timestamps are in correct order 125 | if (ts_start >= ts_end) { 126 | std::cerr << YELLOW_START << "GMsf-ImuBuffer" << RED_START << " IMU Lookup Timestamps are not correct ts_start(" << std::fixed 127 | << ts_start << ") >= ts_end(" << ts_end << ")\n"; 128 | return false; 129 | } 130 | 131 | // Get Iterator Belonging to ts_start 132 | s_itr = timeToImuBuffer_.lower_bound(ts_start); 133 | // Get Iterator Belonging to ts_end 134 | e_itr = timeToImuBuffer_.lower_bound(ts_end); 135 | 136 | // Check if it is first value in the buffer which means there is no value before to interpolate with 137 | if (s_itr == timeToImuBuffer_.begin()) { 138 | std::cerr << YELLOW_START << "GMsf-ImuBuffer" << RED_START 139 | << " Lookup requires first message of IMU buffer, cannot Interpolate back, " 140 | "Lookup Start/End: " 141 | << std::fixed << ts_start << "/" << ts_end << ", Buffer Start/End: " << timeToImuBuffer_.begin()->first << "/" 142 | << timeToImuBuffer_.rbegin()->first << std::endl; 143 | return false; 144 | } 145 | 146 | // Check if lookup start time is ahead of buffer start time 147 | if (s_itr == timeToImuBuffer_.end()) { 148 | std::cerr << YELLOW_START << "GMsf-ImuBuffer" << RED_START 149 | << " IMU Lookup start time ahead latest IMU message in the buffer, lookup: " << ts_start 150 | << ", latest IMU: " << timeToImuBuffer_.rbegin()->first << std::endl; 151 | return false; 152 | } 153 | 154 | // Check if last value is valid 155 | if (e_itr == timeToImuBuffer_.end()) { 156 | std::cerr << YELLOW_START << "GMsf-ImuBuffer" << RED_START << " Lookup is past IMU buffer, with lookup Start/End: " << std::fixed 157 | << ts_start << "/" << ts_end << " and latest IMU: " << timeToImuBuffer_.rbegin()->first << std::endl; 158 | e_itr = timeToImuBuffer_.end(); 159 | --e_itr; 160 | } 161 | 162 | // Check if two IMU messages are different 163 | if (s_itr == e_itr) { 164 | std::cerr << YELLOW_START << "GMsf-ImuBuffer" << RED_START 165 | << " Not Enough IMU values between timestamps , with Start/End: " << std::fixed << ts_start << "/" << ts_end 166 | << ", with diff: " << ts_end - ts_start << std::endl; 167 | return false; 168 | } 169 | 170 | // If everything is good 171 | return true; 172 | } 173 | 174 | bool ImuBuffer::estimateAttitudeFromImu(const std::string& imuGravityDirection, gtsam::Rot3& initAttitude, double& gravityMagnitude, 175 | Eigen::Vector3d& gyrBias) { 176 | // Make sure that imuBuffer is long enough 177 | if (imuBufferLength_ < (imuRate_ * imuPoseInitWaitSecs_)) { 178 | throw std::runtime_error("ImuBufferLength is not large enough for initialization. Must be at least 1 second."); 179 | } 180 | 181 | // Get timestamp of first message for lookup 182 | if (timeToImuBuffer_.size() < (imuRate_ * imuPoseInitWaitSecs_)) { 183 | return false; 184 | } else { 185 | // Accumulate Acceleration part of IMU Messages 186 | Eigen::Vector3d initAccMean(0.0, 0.0, 0.0), initGyrMean(0.0, 0.0, 0.0); 187 | for (auto& itr : timeToImuBuffer_) { 188 | initAccMean += itr.second.head<3>(); 189 | initGyrMean += itr.second.tail<3>(); 190 | } 191 | 192 | // Average IMU measurements and set assumed gravity direction 193 | initAccMean /= timeToImuBuffer_.size(); 194 | gravityMagnitude = initAccMean.norm(); 195 | Eigen::Vector3d gUnitVec; 196 | if (imuGravityDirection == "up") { 197 | gUnitVec = Eigen::Vector3d(0.0, 0.0, 1.0); // ROS convention 198 | } else if (imuGravityDirection == "down") { 199 | gUnitVec = Eigen::Vector3d(0.0, 0.0, -1.0); 200 | } else { 201 | throw std::runtime_error("Gravity direction must be either 'up' or 'down'."); 202 | } 203 | // Normalize gravity vectors to remove the affect of gravity magnitude from place-to-place 204 | initAccMean.normalize(); 205 | initAttitude = gtsam::Rot3(Eigen::Quaterniond().setFromTwoVectors(initAccMean, gUnitVec)); 206 | 207 | // Gyro 208 | initGyrMean /= timeToImuBuffer_.size(); 209 | gyrBias = initGyrMean; 210 | 211 | // Calculate robot initial orientation using gravity vector. 212 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Gravity Magnitude: " << gravityMagnitude << std::endl; 213 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Mean IMU Acceleration Vector(x,y,z): " << initAccMean.transpose() 214 | << " - Gravity Unit Vector(x,y,z): " << gUnitVec.transpose() << std::endl; 215 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << GREEN_START 216 | << " Yaw/Pitch/Roll(deg): " << initAttitude.ypr().transpose() * (180.0 / M_PI) << COLOR_END << std::endl; 217 | std::cout << YELLOW_START << "GMsf-ImuBuffer" << COLOR_END << " Gyro bias(x,y,z): " << initGyrMean.transpose() << std::endl; 218 | } 219 | return true; 220 | } 221 | } // namespace graph_msf -------------------------------------------------------------------------------- /graph_msf_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(graph_msf_ros) 3 | 4 | set(CATKIN_PACKAGE_DEPENDENCIES 5 | roscpp 6 | graph_msf 7 | kdl_parser 8 | nav_msgs 9 | tf 10 | ) 11 | 12 | find_package(catkin REQUIRED COMPONENTS 13 | ${CATKIN_PACKAGE_DEPENDENCIES} 14 | ) 15 | 16 | find_package(Eigen3 REQUIRED) 17 | message("Eigen Version:" ${EIGEN3_VERSION_STRING}) 18 | message("Eigen Path:" ${Eigen3_DIR}) 19 | 20 | link_directories(${PCL_LIBRARY_DIRS}) 21 | add_definitions(${PCL_DEFINITIONS}) 22 | 23 | #generate_messages( 24 | # DEPENDENCIES 25 | # std_msgs 26 | #) 27 | 28 | catkin_package( 29 | CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} 30 | DEPENDS EIGEN3 PCL 31 | INCLUDE_DIRS include 32 | LIBRARIES ${PROJECT_NAME} 33 | ) 34 | 35 | ########### 36 | ## Build ## 37 | ########### 38 | set(CMAKE_CXX_STANDARD 14) 39 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 40 | 41 | include_directories( 42 | include 43 | ${catkin_INCLUDE_DIRS} 44 | ${GTSAM_INCLUDE_DIR} 45 | ${PCL_INCLUDE_DIRS} 46 | ${Python3_INCLUDE_DIRS} 47 | ) 48 | 49 | # Library 50 | add_library(${PROJECT_NAME} 51 | src/lib/StaticTransformsTf.cpp 52 | src/lib/StaticTransformsUrdf.cpp 53 | src/lib/GraphMsfRos.cpp 54 | ) 55 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 56 | 57 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 58 | 59 | # Add clang tooling 60 | find_package(cmake_clang_tools QUIET) 61 | if(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) 62 | add_clang_tooling( 63 | TARGET ${PROJECT_NAME} 64 | SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include 65 | CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include 66 | CF_FIX 67 | ) 68 | endif(cmake_clang_tools_FOUND AND NOT DEFINED NO_CLANG_TOOLING) 69 | 70 | ############# 71 | ## Install ## 72 | ############# 73 | 74 | install(TARGETS ${PROJECT_NAME} 75 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 78 | ) 79 | 80 | install(DIRECTORY include/${PROJECT_NAME}/ 81 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 82 | ) 83 | 84 | ## Mark cpp header files for installation 85 | install(DIRECTORY launch config rviz 86 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 87 | ) 88 | -------------------------------------------------------------------------------- /graph_msf_ros/config/extrinsic_params.yaml: -------------------------------------------------------------------------------- 1 | #External Prior/Transform Input 2 | extrinsics: 3 | mapFrame: map 4 | odomFrame: odom 5 | lidarFrame: lidar_top #LiDAR frame name - used to lookup LiDAR-to-ExternalSensor Frame 6 | imuFrame: imu_link #Frame of IMU used in integrator - Used for lookup transform to lidarFrame 7 | leftGnssFrame: GNSS_L 8 | rightGnssFrame: GNSS_R -------------------------------------------------------------------------------- /graph_msf_ros/config/gnss_params.yaml: -------------------------------------------------------------------------------- 1 | gnss: 2 | use_reference: true 3 | reference_latitude: 47.4084860363 # Wangen a.A.: 47.234593265310046, Hoengg: 47.4084860363, Oberglatt: 47.4801402 4 | reference_longitude: 8.50435818058 # Wangen a.A.: 7.681415329904487, Hoengg: 8.50435818058, Oberglatt: ... 5 | reference_altitude: 565.0 #475.2 # Wangen a.A.: 472.58400000000074, Hoengg: 565.0, Oberglatt: ... 6 | reference_heading: 0.0 # radians, Oberglatt: 2.6305 -------------------------------------------------------------------------------- /graph_msf_ros/config/graph_params.yaml: -------------------------------------------------------------------------------- 1 | #Common 2 | common_params: 3 | verbosity: 4 #Debug Output, 0: only important events, 1: optimization duration, 2: added factors 4 | logPlots: true 5 | 6 | # Sensor Params 7 | sensor_params: 8 | imuRate: 400 #Rate of IMU input (Hz) 9 | imuBufferLength: 800 10 | lidarRate: 10 11 | gnssRate: 20 12 | imuTimeOffset: 0.0 # Offset between IMU and LiDAR Measurements: can be determined with rqt_multiplot 13 | 14 | # Factor Graph 15 | graph_params: 16 | smootherLag: 2.0 #Lag of fixed lag smoother[seconds], not needed for ISAM2 17 | additonalIterations: 0 #Additional iterations of graph optimizer after update with new factors 18 | findUnusedFactorSlots: false 19 | enableDetailedResults: false 20 | usingFallbackGraph: false 21 | 22 | # Outliere Rejection 23 | outlier_params: 24 | gnssOutlierThreshold: 0.3 # in meters, if jumping more than this, it is considered as abscent gnss, occures between two gnss measurements 25 | 26 | # Noise Parameters 27 | noise_params: 28 | ## IMU 29 | ### Accelerometer 30 | accNoiseDensity: 7.225e-07 #Continuous-time "Covariance" of accelerometer, microstrain: sigma^2=7.225e-7 31 | accBiasRandomWalk: 1.0e-06 #gnss:1.0e-06 #lidar: 1e-01 #Continuous-time "Covariance" describing accelerometer bias random walk, default: 1.0e-06 32 | accBiasPrior: 0.0 #Prior/starting value of accelerometer bias 33 | ### o 34 | gyrNoiseDensity: 1.71e-08 #Continuous-time "Covariance" of gyroscope measurements, microstrain: sigma^2=1.71-08 35 | gyrBiasRandomWalk: 9.33e-08 #gnss:9.33e-08 #lidar: 9.33e-01 #Continuous-time "Covariance" describing gyroscope bias random walk, default: 9.33e-08 36 | gyrBiasPrior: 0.0 #Prior/starting value of gyroscope bias 37 | ### Preintegration 38 | integrationNoiseDensity: 1.0e-08 #continuous-time "Covariance" describing integration uncertainty, default: 1.0e-06 39 | biasAccOmegaPreint: 1.0e-08 #covariance of bias used for preintegration, default: 1.0e-2 40 | ## LiDAR 41 | poseBetweenNoise: [ 10.0, 10.0, 10.0, 0.01, 0.01, 0.01 ] # gnss [ 10.0, 10.0, 10.0, 2.0, 2.0, 2.0 ] # lidar: [ 1000.0, 1000.0, 1000.0, 100.0, 100.0, 100.0 ] #Noise of add between factor -ORDER RPY(rad) - XYZ(meters) 42 | poseUnaryNoise: [ 100.0, 100.0, 100.0, 0.01, 0.01, 0.01 ] # ORDER RPY(rad) - XYZ(meters) First tests: [ 1000.0, 1000.0, 1000.0, 2.0, 2.0, 2.0 ] 43 | ## GNSS 44 | gnssPositionUnaryNoise: 1.0 # x, y, z of global position 45 | gnssHeadingUnaryNoise: 1000.0 # x,y,z of heading vector 46 | 47 | # Relinearization 48 | relinearization_params: 49 | positionReLinTh: 5.0e-2 #Position linearization threshold 50 | rotationReLinTh: 1.0e-2 #Rotation linearization threshold 51 | velocityReLinTh: 1.0e-2 #Linear Velocity linearization threshold 52 | accBiasReLinTh: 1.0e-2 #Accelerometer bias linearization threshold 53 | gyrBiasReLinTh: 1.0e-2 #Gyroscope bias linearization threshold 54 | relinearizeSkip: 1 55 | enableRelinearization: true 56 | evaluateNonlinearError: true 57 | cacheLinearizedFactors: true 58 | enablePartialRelinearizationCheck: true 59 | -------------------------------------------------------------------------------- /graph_msf_ros/include/graph_msf_ros/GraphMsfRos.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef CompslamSeRos_H 9 | #define CompslamSeRos_H 10 | 11 | // std 12 | #include 13 | 14 | // ROS 15 | #include 16 | #include 17 | #include 18 | 19 | // Workspace 20 | #include "graph_msf/GraphMsfInterface.h" 21 | 22 | namespace graph_msf { 23 | 24 | class GraphMsfRos : public GraphMsfInterface { 25 | public: 26 | GraphMsfRos() {} 27 | 28 | protected: 29 | // Functions that need implementation 30 | virtual void initializePublishers_(std::shared_ptr& privateNodePtr) = 0; 31 | virtual void initializeSubscribers_(std::shared_ptr& privateNodePtr) = 0; 32 | 33 | // Commodity Functions to be shared 34 | static void addToPathMsg(nav_msgs::PathPtr pathPtr, const std::string& frameName, const ros::Time& stamp, const Eigen::Vector3d& t, 35 | const int maxBufferLength); 36 | static void addToOdometryMsg(nav_msgs::OdometryPtr msgPtr, const std::string& fixedFrame, const std::string& movingFrame, 37 | const ros::Time& stamp, const Eigen::Matrix4d& T, const Eigen::Vector3d& W_v_W_F, 38 | const Eigen::Vector3d& W_w_W_F); 39 | long secondsSinceStart_(); 40 | 41 | // Time 42 | std::chrono::time_point startTime_; 43 | std::chrono::time_point currentTime_; 44 | }; 45 | } // namespace graph_msf 46 | #endif // end M545ESTIMATORGRAPH_H 47 | -------------------------------------------------------------------------------- /graph_msf_ros/include/graph_msf_ros/extrinsics/ElementToRoot.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef ELEMENTTOROOT_H 9 | #define ELEMENTTOROOT_H 10 | 11 | #include "tf/tf.h" 12 | 13 | namespace graph_msf { 14 | 15 | class ElementToRoot final { 16 | public: 17 | /// Constructor 18 | explicit ElementToRoot(const tf::Transform& T, const std::string& rootName_, const std::string& elementName_) 19 | : T_root_element(T), rootName(rootName_), elementName(elementName_) {} 20 | 21 | tf::Transform T_root_element; ///< The KDL segment 22 | std::string rootName; ///< The name of the root element to which this link is attached 23 | std::string elementName; ///< The name of the element 24 | }; 25 | 26 | } // namespace graph_msf 27 | 28 | #endif // ELEMENTTOROOT_H 29 | -------------------------------------------------------------------------------- /graph_msf_ros/include/graph_msf_ros/extrinsics/StaticTransformsTf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef StaticTransformsUrdf_H 9 | #define StaticTransformsUrdf_H 10 | 11 | // ROS 12 | #include 13 | 14 | // Workspace 15 | #include "graph_msf/StaticTransforms.h" 16 | 17 | namespace graph_msf { 18 | 19 | class StaticTransformsTf : public StaticTransforms { 20 | public: 21 | StaticTransformsTf(ros::NodeHandle& privateNode); 22 | 23 | protected: 24 | virtual void findTransformations() = 0; 25 | 26 | // Members 27 | tf::TransformListener listener_; 28 | }; 29 | } // namespace graph_msf 30 | #endif // end StaticTransformsUrdf_H 31 | -------------------------------------------------------------------------------- /graph_msf_ros/include/graph_msf_ros/extrinsics/StaticTransformsUrdf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef StaticTransformsUrdf_H 9 | #define StaticTransformsUrdf_H 10 | 11 | // ROS 12 | #include 13 | #include 14 | #include "urdf/model.h" 15 | 16 | // Workspace 17 | #include "graph_msf/StaticTransforms.h" 18 | #include "graph_msf_ros/extrinsics/ElementToRoot.h" 19 | 20 | namespace graph_msf { 21 | 22 | class StaticTransformsUrdf : public StaticTransforms { 23 | public: 24 | StaticTransformsUrdf(ros::NodeHandle& privateNode); 25 | void setup(); 26 | 27 | protected: 28 | // Names 29 | /// Description 30 | std::string urdfDescriptionName_; 31 | 32 | // Robot Models 33 | urdf::Model urdfModel_; 34 | std::unique_ptr model_; 35 | KDL::Tree tree_; 36 | 37 | /// A map of dynamic segment names to SegmentPair structures 38 | std::map segments_; 39 | 40 | // Methods 41 | void getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName = ""); 42 | tf::Transform kdlToTransform(const KDL::Frame& k); 43 | 44 | private: 45 | virtual void findTransformations() = 0; 46 | ros::NodeHandle privateNode_; 47 | }; 48 | } // namespace graph_msf 49 | #endif // end StaticTransformsUrdf_H 50 | -------------------------------------------------------------------------------- /graph_msf_ros/include/graph_msf_ros/util/conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | #ifndef MENZI_SIM_WS_202111_EIGEN_CONVERSIONS_H 9 | #define MENZI_SIM_WS_202111_EIGEN_CONVERSIONS_H 10 | 11 | #include 12 | #include 13 | 14 | namespace graph_msf { 15 | 16 | inline void odomMsgToEigen(const nav_msgs::Odometry& odomLidar, Eigen::Matrix4d& T) { 17 | tf::Quaternion tf_q; 18 | tf::quaternionMsgToTF(odomLidar.pose.pose.orientation, tf_q); 19 | Eigen::Vector3d t(odomLidar.pose.pose.position.x, odomLidar.pose.pose.position.y, odomLidar.pose.pose.position.z); 20 | Eigen::Quaternion q(tf_q.getW(), tf_q.getX(), tf_q.getY(), tf_q.getZ()); 21 | T.setIdentity(); 22 | T.block<3, 3>(0, 0) = q.matrix(); 23 | T.block<3, 1>(0, 3) = t; 24 | } 25 | 26 | inline void odomMsgToTf(const nav_msgs::Odometry& odomLidar, tf::Transform& T) { 27 | tf::Quaternion tf_q; 28 | tf::quaternionMsgToTF(odomLidar.pose.pose.orientation, tf_q); 29 | tf::Vector3 tf_t(odomLidar.pose.pose.position.x, odomLidar.pose.pose.position.y, odomLidar.pose.pose.position.z); 30 | T = tf::Transform(tf_q, tf_t); 31 | } 32 | 33 | inline tf::Transform matrix3ToTf(const Eigen::Matrix3d& R) { 34 | Eigen::Quaterniond q(R); 35 | tf::Transform tf_R; 36 | tf_R.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w())); 37 | tf_R.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); 38 | return tf_R; 39 | } 40 | 41 | inline tf::Transform matrix4ToTf(const Eigen::Matrix4d& T) { 42 | Eigen::Quaterniond q(T.block<3, 3>(0, 0)); 43 | tf::Transform tf_T; 44 | tf_T.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w())); 45 | tf_T.setOrigin(tf::Vector3(T(0, 3), T(1, 3), T(2, 3))); 46 | return tf_T; 47 | } 48 | 49 | inline void tfToMatrix4(const tf::Transform& tf_T, Eigen::Matrix4d& T) { 50 | tf::Quaternion tf_q = tf_T.getRotation(); 51 | Eigen::Quaternion q(tf_q.getW(), tf_q.getX(), tf_q.getY(), tf_q.getZ()); 52 | Eigen::Vector3d t(tf_T.getOrigin().getX(), tf_T.getOrigin().getY(), tf_T.getOrigin().getZ()); 53 | T.setIdentity(); 54 | T.block<3, 3>(0, 0) = q.matrix(); 55 | T.block<3, 1>(0, 3) = t; 56 | } 57 | 58 | inline tf::Transform pose3ToTf(const Eigen::Matrix3d& T) { 59 | Eigen::Quaterniond q(T); 60 | tf::Transform tf_T; 61 | tf_T.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w())); 62 | tf_T.setOrigin(tf::Vector3(T(0, 3), T(1, 3), T(2, 3))); 63 | return tf_T; 64 | } 65 | 66 | } // namespace graph_msf 67 | 68 | #endif // MENZI_SIM_WS_202111_EIGEN_CONVERSIONS_H 69 | -------------------------------------------------------------------------------- /graph_msf_ros/launch/compslam_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /graph_msf_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | graph_msf_ros 4 | 0.0.0 5 | 6 | 7 | State estimation based on factor graphs, utilizing GTSAM functionality. Fusion of IMU, LiDAR Mapping Odometry and GNSS estimates. 8 | 9 | 10 | Julian Nubert 11 | BSD 12 | Julian Nubert 13 | 14 | catkin 15 | roscpp 16 | graph_msf 17 | kdl_parser 18 | nav_msgs 19 | tf 20 | 21 | roscpp 22 | graph_msf 23 | kdl_parser 24 | nav_msgs 25 | tf 26 | 27 | rostest 28 | rosbag 29 | 30 | 31 | -------------------------------------------------------------------------------- /graph_msf_ros/rviz/vis.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Odometry1 10 | - /Odometry1/Shape1 11 | - /Odometry2 12 | - /Odometry2/Shape1 13 | Splitter Ratio: 0.5 14 | Tree Height: 1079 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Angle Tolerance: 0.10000000149011612 60 | Class: rviz/Odometry 61 | Covariance: 62 | Orientation: 63 | Alpha: 0.5 64 | Color: 255; 255; 127 65 | Color Style: Unique 66 | Frame: Local 67 | Offset: 1 68 | Scale: 1 69 | Value: true 70 | Position: 71 | Alpha: 0.30000001192092896 72 | Color: 204; 51; 204 73 | Scale: 1 74 | Value: true 75 | Value: false 76 | Enabled: true 77 | Keep: 100 78 | Name: Odometry 79 | Position Tolerance: 0.10000000149011612 80 | Queue Size: 10 81 | Shape: 82 | Alpha: 0.5 83 | Axes Length: 1 84 | Axes Radius: 0.10000000149011612 85 | Color: 255; 25; 0 86 | Head Length: 0.10000000149011612 87 | Head Radius: 0.10000000149011612 88 | Shaft Length: 0.20000000298023224 89 | Shaft Radius: 0.05000000074505806 90 | Value: Arrow 91 | Topic: /crl_rzr/liopard/lio/odometry 92 | Unreliable: false 93 | Value: true 94 | - Class: rviz/TF 95 | Enabled: false 96 | Frame Timeout: 15 97 | Frames: 98 | All Enabled: true 99 | Marker Alpha: 1 100 | Marker Scale: 1 101 | Name: TF 102 | Show Arrows: true 103 | Show Axes: true 104 | Show Names: true 105 | Tree: 106 | {} 107 | Update Interval: 0 108 | Value: false 109 | - Angle Tolerance: 0.10000000149011612 110 | Class: rviz/Odometry 111 | Covariance: 112 | Orientation: 113 | Alpha: 0.5 114 | Color: 255; 255; 127 115 | Color Style: Unique 116 | Frame: Local 117 | Offset: 1 118 | Scale: 1 119 | Value: true 120 | Position: 121 | Alpha: 0.30000001192092896 122 | Color: 204; 51; 204 123 | Scale: 1 124 | Value: true 125 | Value: true 126 | Enabled: true 127 | Keep: 100 128 | Name: Odometry 129 | Position Tolerance: 0.10000000149011612 130 | Queue Size: 10 131 | Shape: 132 | Alpha: 1 133 | Axes Length: 1 134 | Axes Radius: 0.10000000149011612 135 | Color: 238; 238; 236 136 | Head Length: 0.10000000149011612 137 | Head Radius: 0.10000000149011612 138 | Shaft Length: 0.20000000298023224 139 | Shaft Radius: 0.05000000074505806 140 | Value: Arrow 141 | Topic: /graph_msf/transform_odom_lidar 142 | Unreliable: false 143 | Value: true 144 | Enabled: true 145 | Global Options: 146 | Background Color: 48; 48; 48 147 | Default Light: true 148 | Fixed Frame: crl_rzr/map 149 | Frame Rate: 30 150 | Name: root 151 | Tools: 152 | - Class: rviz/Interact 153 | Hide Inactive Objects: true 154 | - Class: rviz/MoveCamera 155 | - Class: rviz/Select 156 | - Class: rviz/FocusCamera 157 | - Class: rviz/Measure 158 | - Class: rviz/SetInitialPose 159 | Theta std deviation: 0.2617993950843811 160 | Topic: /initialpose 161 | X std deviation: 0.5 162 | Y std deviation: 0.5 163 | - Class: rviz/SetGoal 164 | Topic: /move_base_simple/goal 165 | - Class: rviz/PublishPoint 166 | Single click: true 167 | Topic: /clicked_point 168 | Value: true 169 | Views: 170 | Current: 171 | Angle: -1.5699995756149292 172 | Class: rviz/TopDownOrtho 173 | Enable Stereo Rendering: 174 | Stereo Eye Separation: 0.05999999865889549 175 | Stereo Focal Distance: 1 176 | Swap Stereo Eyes: false 177 | Value: false 178 | Invert Z Axis: false 179 | Name: Current View 180 | Near Clip Distance: 0.009999999776482582 181 | Scale: 94.30439758300781 182 | Target Frame: 183 | X: 0 184 | Y: 0 185 | Saved: ~ 186 | Window Geometry: 187 | Displays: 188 | collapsed: false 189 | Height: 1376 190 | Hide Left Dock: false 191 | Hide Right Dock: false 192 | QMainWindow State: 000000ff00000000fd00000004000000000000018a000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004a8000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 193 | Selection: 194 | collapsed: false 195 | Time: 196 | collapsed: false 197 | Tool Properties: 198 | collapsed: false 199 | Views: 200 | collapsed: false 201 | Width: 1869 202 | X: 691 203 | Y: 3 204 | -------------------------------------------------------------------------------- /graph_msf_ros/src/lib/GraphMsfRos.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Implementation 9 | #include "graph_msf_ros/GraphMsfRos.h" 10 | 11 | // ROS 12 | #include 13 | 14 | // Workspace 15 | #include "graph_msf_ros/util/conversions.h" 16 | 17 | namespace graph_msf { 18 | 19 | void GraphMsfRos::addToPathMsg(nav_msgs::PathPtr pathPtr, const std::string& frameName, const ros::Time& stamp, const Eigen::Vector3d& t, 20 | const int maxBufferLength) { 21 | geometry_msgs::PoseStamped pose; 22 | pose.header.frame_id = frameName; 23 | pose.header.stamp = stamp; 24 | pose.pose.position.x = t(0); 25 | pose.pose.position.y = t(1); 26 | pose.pose.position.z = t(2); 27 | pathPtr->header.frame_id = frameName; 28 | pathPtr->header.stamp = stamp; 29 | pathPtr->poses.push_back(pose); 30 | if (pathPtr->poses.size() > maxBufferLength) { 31 | pathPtr->poses.erase(pathPtr->poses.begin()); 32 | } 33 | } 34 | 35 | void GraphMsfRos::addToOdometryMsg(nav_msgs::OdometryPtr msgPtr, const std::string& fixedFrame, const std::string& movingFrame, 36 | const ros::Time& stamp, const Eigen::Matrix4d& T, const Eigen::Vector3d& W_v_W_F, 37 | const Eigen::Vector3d& W_w_W_F) { 38 | msgPtr->header.frame_id = fixedFrame; 39 | msgPtr->child_frame_id = movingFrame; 40 | msgPtr->header.stamp = stamp; 41 | tf::poseTFToMsg(matrix4ToTf(T), msgPtr->pose.pose); 42 | msgPtr->twist.twist.linear.x = W_v_W_F(0); 43 | msgPtr->twist.twist.linear.y = W_v_W_F(1); 44 | msgPtr->twist.twist.linear.z = W_v_W_F(2); 45 | msgPtr->twist.twist.angular.x = W_w_W_F(0); 46 | msgPtr->twist.twist.angular.y = W_w_W_F(1); 47 | msgPtr->twist.twist.angular.z = W_w_W_F(2); 48 | } 49 | 50 | long GraphMsfRos::secondsSinceStart_() { 51 | return std::chrono::duration_cast(currentTime_ - startTime_).count(); 52 | } 53 | 54 | } // namespace graph_msf -------------------------------------------------------------------------------- /graph_msf_ros/src/lib/StaticTransformsTf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // Implementation 9 | #include "graph_msf_ros/extrinsics/StaticTransformsTf.h" 10 | 11 | // ROS 12 | #include 13 | 14 | // Workspace 15 | #include "graph_msf_ros/util/conversions.h" 16 | 17 | namespace graph_msf { 18 | 19 | StaticTransformsTf::StaticTransformsTf(ros::NodeHandle& privateNode) { 20 | std::cout << YELLOW_START << "StaticTransformsTf" << GREEN_START << " Initializing static transforms..." << COLOR_END << std::endl; 21 | } 22 | 23 | } // namespace graph_msf 24 | -------------------------------------------------------------------------------- /graph_msf_ros/src/lib/StaticTransformsUrdf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright 2022 by Julian Nubert, Robotic Systems Lab, ETH Zurich. 3 | All rights reserved. 4 | This file is released under the "BSD-3-Clause License". 5 | Please see the LICENSE file that has been included as part of this package. 6 | */ 7 | 8 | // ROS 9 | #include 10 | #include 11 | 12 | // Workspace 13 | #include "graph_msf_ros/extrinsics/StaticTransformsUrdf.h" 14 | 15 | namespace graph_msf { 16 | 17 | StaticTransformsUrdf::StaticTransformsUrdf(ros::NodeHandle& privateNode) { 18 | std::cout << YELLOW_START << "StaticTransformsUrdf" << GREEN_START << " Initializing..." << COLOR_END << std::endl; 19 | privateNode_ = privateNode; 20 | } 21 | 22 | void StaticTransformsUrdf::setup() { 23 | std::cout << YELLOW_START << "StaticTransformsUrdf" << GREEN_START << " Setting up for description name '" << urdfDescriptionName_ << "'." 24 | << COLOR_END << std::endl; 25 | 26 | // Check whether description contains information 27 | std::string urdfDescriptionContent; 28 | privateNode_.getParam(std::string("/") + urdfDescriptionName_, urdfDescriptionContent); 29 | if (urdfDescriptionContent.empty()) { 30 | throw std::runtime_error("StaticTransformsUrdf - Could not load description, description empty."); 31 | } 32 | 33 | // load excavator model from URDF 34 | double timeStep; 35 | urdfModel_.initParam(urdfDescriptionName_); 36 | 37 | // Initialize the KDL tree 38 | if (!kdl_parser::treeFromUrdfModel(urdfModel_, tree_)) { 39 | throw std::runtime_error("Failed to extract kdl tree from robot description"); 40 | } 41 | 42 | KDL::SegmentMap segments_map = tree_.getSegments(); 43 | KDL::Chain chain; 44 | 45 | // walk the tree and add segments to segments_ 46 | segments_.clear(); 47 | getRootTransformations(tree_.getRootSegment()); 48 | 49 | // Summary 50 | std::cout << YELLOW_START << "StaticTransformsUrdf" << GREEN_START << " Initialized." << COLOR_END << std::endl; 51 | } 52 | 53 | void StaticTransformsUrdf::getRootTransformations(const KDL::SegmentMap::const_iterator element, std::string rootName) { 54 | const std::string& elementName = GetTreeElementSegment(element->second).getName(); 55 | if (rootName == "") { 56 | rootName = elementName; 57 | } 58 | // Iterate through children 59 | std::vector children = GetTreeElementChildren(element->second); 60 | for (unsigned int i = 0; i < children.size(); i++) { 61 | // Go through children 62 | const KDL::SegmentMap::const_iterator child = children[i]; 63 | // Get kinematic chain from current child to root 64 | KDL::Chain chain; 65 | tree_.getChain(rootName, child->second.segment.getName(), chain); 66 | // Compute trafo to root 67 | tf::Transform T_root_element = tf::Transform::getIdentity(); 68 | for (int i = 0; i < chain.getNrOfSegments(); ++i) { 69 | KDL::Frame frameToRoot = chain.getSegment(i).getFrameToTip(); 70 | double x, y, z, w; 71 | frameToRoot.M.GetQuaternion(x, y, z, w); 72 | tf::Quaternion q(x, y, z, w); 73 | tf::Vector3 t(frameToRoot.p[0], frameToRoot.p[1], frameToRoot.p[2]); 74 | T_root_element = T_root_element * tf::Transform(q, t); 75 | } 76 | // Write into buffer 77 | ElementToRoot elementToRoot(T_root_element, rootName, child->second.segment.getName()); 78 | // Insert into segments 79 | segments_.insert(std::make_pair(child->second.segment.getName(), elementToRoot)); 80 | // Call recursively 81 | getRootTransformations(child, rootName); 82 | } 83 | } 84 | 85 | tf::Transform StaticTransformsUrdf::kdlToTransform(const KDL::Frame& k) { 86 | tf::Transform tf_T; 87 | tf_T.setOrigin(tf::Vector3(k.p.x(), k.p.y(), k.p.z())); 88 | double qx, qy, qz, qw; 89 | k.M.GetQuaternion(qx, qy, qz, qw); 90 | tf_T.setRotation(tf::Quaternion(qx, qy, qz, qw)); 91 | 92 | return tf_T; 93 | } 94 | 95 | } // namespace graph_msf 96 | --------------------------------------------------------------------------------