Repository: HKUST-Aerial-Robotics/RIO Branch: main Commit: 0ede946fe8ee Files: 85 Total size: 45.6 MB Directory structure: gitextract_0gbuko_e/ ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── ars548_driver/ │ ├── CMakeLists.txt │ ├── data_type/ │ │ ├── RadarConfiguration.hpp │ │ ├── RadarDetection.hpp │ │ ├── RadarObject.hpp │ │ ├── RadarStatus.hpp │ │ ├── StatusDef.hpp │ │ ├── TypeConverter.hpp │ │ └── VehicleInfo.hpp │ ├── msg/ │ │ ├── AccelerationLateralCog.msg │ │ ├── AccelerationLongitudinalCog.msg │ │ ├── CharacteristicSpeed.msg │ │ ├── DetectionList.msg │ │ ├── Detections.msg │ │ ├── DrivingDirection.msg │ │ ├── ObjectList.msg │ │ ├── Objects.msg │ │ ├── RadarBasicStatus.msg │ │ ├── SteeringAngleFrontAxle.msg │ │ ├── VelocityVehicle.msg │ │ └── YawRate.msg │ ├── node/ │ │ └── ARS548_node.cpp │ ├── package.xml │ └── src/ │ ├── ARS548.cpp │ ├── ARS548.hpp │ ├── ARS548Coder.cpp │ ├── ARS548Coder.hpp │ └── UdpIO.cpp ├── dataset/ │ ├── .gitignore │ ├── coloradar_trim/ │ │ └── colo_trim_outdoors_run_0.bag │ └── exp/ │ ├── Sequence_1.bag │ ├── Sequence_2.bag │ └── Sequence_3.bag ├── docker/ │ ├── Dockerfile │ ├── docker.sh │ └── run.py └── rio/ ├── CMakeLists.txt ├── config/ │ ├── RIO.rviz │ ├── ars548.yaml │ └── coloradar.yaml ├── launch/ │ └── TestRadar.launch ├── node/ │ ├── rio.cpp │ ├── rosWarper.cpp │ └── rosWarper.hpp ├── package.xml └── src/ ├── data_manager/ │ ├── IMUManager.cpp │ ├── IMUManager.hpp │ ├── RadarManager.cpp │ ├── RadarManager.hpp │ └── readme.md ├── factor/ │ ├── ImuPreintegrationManager.cpp │ ├── ImuPreintegrationManager.hpp │ ├── RadarFeatureManager.cpp │ └── RadarFeatureManager.hpp ├── frame_type/ │ ├── IMUFrame.hpp │ └── RadarFrame.hpp ├── frontend/ │ ├── RadarPreprocessor.cpp │ ├── RadarPreprocessor.hpp │ ├── RadarTracker.cpp │ ├── RadarTracker.hpp │ └── readme.md ├── manifold/ │ ├── EigenQuaternionManifold.cpp │ └── EigenQuaternionManifold.hpp ├── residual/ │ ├── IMUResidual.cpp │ ├── IMUResidual.hpp │ ├── Preintegration.cpp │ ├── Preintegration.hpp │ ├── RadarImuVelResidual.cpp │ ├── RadarImuVelResidual.hpp │ ├── RadarPointResidual.cpp │ ├── RadarPointResidual.hpp │ └── readme.md ├── state/ │ ├── IMUState.hpp │ ├── R3State.hpp │ ├── RadarPointFeatureState.hpp │ ├── SE3State.hpp │ ├── StateManager.cpp │ └── StateManager.hpp └── utility/ ├── MathUtility.hpp ├── TicToc.hpp ├── Timestamp.hpp └── Utility.hpp ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ /CMakeLists.txt ================================================ FILE: .pre-commit-config.yaml ================================================ repos: - repo: https://github.com/cpp-linter/cpp-linter-hooks rev: v0.4.0 # Use the ref you want to point at hooks: - id: clang-format args: [--style=Google] # Other coding style: LLVM, GNU, Chromium, Microsoft, Mozilla, WebKit. # - id: clang-tidy # args: [--checks='all', -p, build] ================================================ FILE: LICENSE ================================================ MIT License Copyright (c) 2024 HKUST Aerial Robotics Group Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ # RIO Optimization Based and Point Uncertainty Aware Radar-inertial Odometry for 4D Radar System ## Prerequisites Please refer to the `docker/Dockerfile` for the detailed dependencies. ## Dcoker ### Start the docker container ```bash ./docker/docker.sh -b # build the docker image ./docker/docker.sh -r # run the docker container ``` ### Outside the docker container ```bash # Allow the docker container to connect to the X server xhost + ``` ### Inside the docker container ```bash # In the first terminal roscore & rviz # config file: rio/config/RIO.rviz # In the second terminal cd /ws catkin_make # Run the RIO with the sample dataset python3 /ws/src/docker/run.py -a -n rio -c /ws/src/rio/config/ars548.yaml -d /ws/src/dataset/exp/Sequence_1.bag -r 1 -p 1 ``` Then you can see the odometry and the point cloud in rviz. ## System Overview ![](https://wpcos-1300629776.cos.ap-chengdu.myqcloud.com/wpcos-1300629776/Galleryrio-factor.jpg) ## Experiment Platform Our platform consists of a 4D FMCW Radar ARS548RDI manufactured by Continental and an IMU BMI088 manufactured by Bosch. The radar sensor is mounted on the front of the platform, while the IMU is mounted on the bottom. ![](https://wpcos-1300629776.cos.ap-chengdu.myqcloud.com/wpcos-1300629776/Galleryplatform.png) ## Trajectories on self-collected and ColoRadar dataset ![](https://wpcos-1300629776.cos.ap-chengdu.myqcloud.com/wpcos-1300629776/Galleryablation_pic.png) **Red** trajectory is the proposed full system. **Blue** one is the system without point uncertainty model, and **black** one is the ground truth trajectory. We present the results on four sequences in two different datasets. ## Self-collected dataset - **Sequence 1** : involved relatively low-speed movements. - **Sequence 2** : involved relatively high-speed movements. - **Sequence 3** : involved relatively high-speed movements with high-speed rotations. ### Data Format | Field Name | Data Type | Count | Offset (Bytes) | Remarks | |--------------|----------------------------------|-------|----------------|--------------------------------| | azimuth | `sensor_msgs::PointField::FLOAT32` | 1 | 0 | Angle in the horizontal plane | | azimuthSTD | `sensor_msgs::PointField::FLOAT32` | 1 | 4 | Standard deviation of azimuth | | elevation | `sensor_msgs::PointField::FLOAT32` | 1 | 8 | Angle in the vertical plane | | elevationSTD | `sensor_msgs::PointField::FLOAT32` | 1 | 12 | Standard deviation of elevation| | range | `sensor_msgs::PointField::FLOAT32` | 1 | 16 | Distance to the target | | rangeSTD | `sensor_msgs::PointField::FLOAT32` | 1 | 20 | Standard deviation of range | | velocity | `sensor_msgs::PointField::FLOAT32` | 1 | 24 | Speed of the target | | velocitySTD | `sensor_msgs::PointField::FLOAT32` | 1 | 28 | Standard deviation of velocity | | rcs | `sensor_msgs::PointField::INT8` | 1 | 32 | Radar cross-section | ## ColoRadar dataset It consists of 52 sequences, recorded in mines, built environments, and in an urban creek path, totaling more than 145 minutes of 3D FMCW radar, 3D lidar, and IMU data. The full dataset, including sensor data, calibration sequences, and evaluation scripts. It is available at [ColoRadar](https://arpg.github.io/coloradar/). ## Citation If you find our work useful in your research, please consider citing: ```bibtex @article{huang2024morephysicalenhancedradarinertialodometry, title={Less is More: Physical-enhanced Radar-Inertial Odometry}, author={Qiucan Huang and Yuchen Liang and Zhijian Qiao and Shaojie Shen and Huan Yin}, booktitle={ICRA}, year={2024}, } ``` ```bibtex @ARTICLE{10833750, author={Xu, Yang and Huang, Qiucan and Shen, Shaojie and Yin, Huan}, journal={IEEE Robotics and Automation Letters}, title={Incorporating Point Uncertainty in Radar SLAM}, year={2025}, volume={10}, number={3}, pages={2168-2175}, keywords={Radar;Uncertainty;Simultaneous localization and mapping;Radar measurements;Noise;Doppler radar;Doppler effect;Sensors;Velocity measurement;Robots;SLAM;Sensor fusion;Range Sensing}, doi={10.1109/LRA.2025.3527344} } ``` ## License MIT License (see [LICENSE](LICENSE)). ================================================ FILE: ars548_driver/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.0.2) project(ars548_driver) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) SET(CMAKE_BUILD_TYPE "Debug") ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED roscpp) find_package(PCL) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) # find_package(PCAP REQUIRED) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## ################################################ ## To declare and build messages, services or actions from within this ## package, follow these steps: ## * Let MSG_DEP_SET be the set of packages whose message types you use in ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). ## * In the file package.xml: ## * add a build_depend tag for "message_generation" ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in ## but can be declared for certainty nonetheless: ## * add a exec_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to ## find_package(catkin REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to ## catkin_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # ) ## Generate actions in the 'action' folder # add_action_files( # FILES # Action1.action # Action2.action # ) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs # Or other packages containing msgs # ) ################################################ ## Declare ROS dynamic reconfigure parameters ## ################################################ ## To declare and build dynamic reconfigure parameters within this ## package, follow these steps: ## * In the file package.xml: ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" ## * In this file (CMakeLists.txt): ## * add "dynamic_reconfigure" to ## find_package(catkin REQUIRED COMPONENTS ...) ## * uncomment the "generate_dynamic_reconfigure_options" section below ## and list every .cfg file to be processed ## Generate dynamic reconfigure parameters in the 'cfg' folder # generate_dynamic_reconfigure_options( # cfg/DynReconf1.cfg # cfg/DynReconf2.cfg # ) ################################### ## catkin specific configuration ## ################################### ## The catkin_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES ars548_driver # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( data_type src ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ) ## Declare a C++ library # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/ars548_driver.cpp # ) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide add_executable(ARS548_node node/ARS548_node.cpp src/ARS548.cpp src/ARS548Coder.cpp src/UdpIO.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against target_link_libraries(ARS548_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} -lpcap ) ############# ## Install ## ############# # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # catkin_install_python(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html # install(TARGETS ${PROJECT_NAME}_node # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html # install(TARGETS ${PROJECT_NAME} # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} # ) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES # # myfile1 # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_ars548_driver.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) ================================================ FILE: ars548_driver/data_type/RadarConfiguration.hpp ================================================ #pragma once #include namespace Driver { struct RadarConfiguration { // -100 to 100 m float longitudinal; // -100 to 100 m float lateral; // -10 to 10 m float vertical; // -3.14159 to 3.14159 rad float yaw; // -1.5707 to -1.5707 rad float pitch; // 0 to 255, ARS548PlugOrientation uint8_t plugOrientation; // 0.01 to 100, this is vehicle setup float length; // 0.01 to 100, this is vehicle setup float width; // 0.01 to 100, this is vehicle setup float height; // 0.01 to 100, this is vehicle setup float wheelbase; // 93 to 1514 meter uint16_t maximumDistance; // 0 to 255, ARS548FrequencySlot uint8_t frequencySlot; // 0 to 255 represents for 50 to 100 uint8_t cycleTime; // 0 to 255 represents for 10 to 99 uint8_t timeSlot; // 0 to 255, country code, ARS548CountryCode uint8_t hcc; // 0 to 255, ARS548PowerSaveStandstillMode uint8_t powerSaveStandstill; // (0-255).(0-255).(0-255).(0-255) uint32_t sensorIPAddress0; // Reserved uint32_t sensorIPAddress1; // 0 to 255, ARS548ParameterConfiguration uint8_t newSensorMounting; // 0 to 255, ARS548ParameterConfiguration uint8_t newVehicleParameters; // 0 to 255, ARS548ParameterConfiguration uint8_t newRadarParameters; // 0 to 255, ARS548ParameterConfiguration uint8_t newNetworkConfiguration; }; } // namespace Driver ================================================ FILE: ars548_driver/data_type/RadarDetection.hpp ================================================ #pragma once #include namespace Driver { struct ARS548RadarDetection { // -3.14 to 3.14 in rad float azimuthAngle; // 0 to 1 float azimuthSTD; // 0 to 255, ARS548RadarDetectionFlags uint8_t invalidFlags; // -3.14 to 3.14 in rad float elevationAngle; // 0 to 1 float elevationSTD; // 0 to 1500 in meter float range; // 0 to 1 float rangeSTD; // -100 to 100 in meter/seconds float rangeRate; // 0 to 1 float rangeRateSTD; // radar cross-section, -128 to 127 in dBm^2 int8_t rcs; // 0 to 65535 uint16_t measurementID; // 0 to 100, % uint8_t positivePredictiveValue; // 0 to 255, ARS548RadarDetectionClasses uint8_t classification; // 0 to 100, & uint8_t multiTargetProbability; // 0 to 65535 uint16_t objectID; // 0 to 100, %, Probability for resolved velocity ambiguity uint8_t ambiguityFlag; // TBD uint16_t sortIndex; }; struct ARS548RadarDetectionList { // header uint16_t serviceID; uint16_t methodID; uint32_t dataLength; uint16_t clientID; uint16_t sessionID; uint8_t protocolVersion; uint8_t interfaceVersion; uint8_t messageType; uint8_t returnCode; // body uint64_t crc; uint32_t length; // Sequence counter incremented with each message. uint32_t sqc; uint32_t dataID; // 0 to 999999999 uint32_t timestampNs; // 0 to 4294967295 uint32_t timestampS; // 0 to 255, ARS548SyncState, unused uint8_t timestampSyncStatus; // 0 to 4294967295, unused uint32_t eventDataQualifier; // 0 to 255, unused uint8_t extendedQualifier; // 0 to 65535, unused uint16_t originInvalidFlags; // -10 to 10, meter, x position of radar float originX; // TBD, unused float originXSTD; // -10 to 10, meter, y position of radar float originY; // TBD, unused float originYSTD; // -10 to 10, meter, z position of radar float originZ; // TBD, unused float originZSTD; // -3.14 to 3.14 rad, unused float originRoll; // 0 to 0.1, unused float originRollSTD; // -3.14 to 3.14 rad float originPitch; // 0 to 0.1 float originPitchSTD; // -3.14 to 3.14 rad float originYaw; // 0 to 0.1 float originYawSTD; // 0 to 255, unused uint8_t listInvalidFlags; ARS548RadarDetection detectionArray[800]; // -100 to 100, m/s , min ambiguity free Doppler velocity range float listRadVelDomainMin; // -100 to 100, m/s , max ambiguity free Doppler velocity range float listRadVelDomainMax; // 0 to 2047 uint32_t listNumOfDetections; // -3.14 to 3.14, rad float alnAzimuthCorrection; // -3.14 to 3.14, rad float alnElevationCorrection; // alignment status, ARS548AlignmentStatus uint8_t alnStatus; }; } // namespace Driver ================================================ FILE: ars548_driver/data_type/RadarObject.hpp ================================================ #pragma once #include namespace Driver { struct ARS548RadarObject { // 0 to 65535, TBD uint16_t statusSensor; // 0 to 4294967295 uint32_t id; // 0 to 65535 uint16_t age; // 0 to 255, ARS548ObjectStatus uint8_t statusMeasurement; // 0 to 255, ARS548MovementStatus uint8_t statusMovement; // 0 to 65535, TBD uint16_t positionInvalidFlags; // 0 to 255, ARS548PositionReference uint8_t positionReference; // meter float positionX; // meter float positionXSTD; // meter float positionY; // meter float positionYSTD; // meter float positionZ; // meter float positionZSTD; // covariance m^2 float positionCovarianceXY; // rad float positionOrientation; // rad float positionOrientationSTD; // 0 to 255, unused uint8_t existenceInvalidFlags; // 0 to 100, % float existenceProbability; // 0 to 100, %, unused float existencePPV; // 0 to 100, % uint8_t classificationCar; // 0 to 100, % uint8_t classificationTruck; // 0 to 100, % uint8_t classificationMotorcycle; // 0 to 100, % uint8_t classificationBicycle; // 0 to 100, % uint8_t classificationPedestrian; // 0 to 100, % uint8_t classificationAnimal; // 0 to 100, % uint8_t classificationHazard; // 0 to 100, % uint8_t classificationUnknown; // 0 to 100, %, unused uint8_t classificationOverdrivable; // 0 to 100, %, unused uint8_t classificationUnderdrivable; // 0 to 255, unused uint8_t dynamicsAbsVelInvalidFlags; // m/s float dynamicsAbsVelX; // m/s float dynamicsAbsVelXSTD; // m/s float dynamicsAbsVelY; // m/s float dynamicsAbsVelYSTD; // (m/s)^2 float dynamicsAbsVelCovarianceXY; // 0 to 255, unused uint8_t dynamicsRelVelInvalidFlags; // m/s float dynamicsRelVelX; // m/s float dynamicsRelVelXSTD; // m/s float dynamicsRelVelY; // m/s float dynamicsRelVelYSTD; // (m/s)^2 float dynamicsRelVelCovarianceXY; // 0 to 255, unused uint8_t dynamicsAbsAccelInvalidFlags; // m/s^2 float dynamicsAbsAccelX; // m/s^2 float dynamicsAbsAccelXSTD; // m/s^2 float dynamicsAbsAccelY; // m/s^2 float dynamicsAbsAccelYSTD; // (m/s^2)^2 float dynamicsAbsAccelCovarianceXY; // 0 to 255, unused uint8_t dynamicsRelAccelInvalidFlags; // m/s^2 float dynamicsRelAccelX; // m/s^2 float dynamicsRelAccelXSTD; // m/s^2 float dynamicsRelAccelY; // m/s^2 float dynamicsRelAccelYSTD; // (m/s^2)^2 float dynamicsRelAccelCovarianceXY; // 0 to 255, unused uint8_t dynamicsOrientationInvalidFlags; // rad/s float dynamicsOrientationRateMean; // rad/s float dynamicsOrientationRateSTD; // 0 to 4294967295, ARS548ShapeStatus, unused uint32_t shapeLengthStatus; // 0 to 255, unused uint8_t shapeLengthEdgeInvalidFlags; // meter float shapeLengthEdgeMean; // meter unused float shapeLengthEdgeSTD; // 0 to 4294967295, ARS548ShapeStatus, unused uint32_t shapeWidthStatus; // 0 to 255, unused uint8_t shapeWidthEdgeInvalidFlags; // meter float shapeWidthEdgeMean; // meter unused float shapeWidthEdgeSTD; }; struct ARS548RadarObjectList { // header uint16_t serviceID; uint16_t methodID; uint32_t dataLength; uint16_t clientID; uint16_t sessionID; uint8_t protocolVersion; uint8_t interfaceVersion; uint8_t messageType; uint8_t returnCode; // body int64_t crc; uint32_t length; // Sequence counter incremented with each message. uint32_t sqc; uint32_t dataID; // 0 to 999999999, ns uint32_t timestampNs; // 0 to 4294967295 uint32_t timestampS; // 0 to 255, ARS548SyncState, unused uint8_t timestampSyncStatus; // 0 to 4294967295, unused uint32_t eventDataQualifier; // 0 to 255, unused uint8_t extendedQualifier; uint8_t objectListNumOfObjects; ARS548RadarObject objectArray[50]; }; } // namespace Driver ================================================ FILE: ars548_driver/data_type/RadarStatus.hpp ================================================ #pragma once #include namespace Driver { struct ARS548RadarStatus { // -100 to 100 m float longitudinal; // -100 to 100 m float lateral; // -10 to 10 m float vertical; // -3.14159 to 3.14159 rad float yaw; // -1.5707 to -1.5707 rad float pitch; // 0 to 255, ARS548PlugOrientation uint8_t plugOrientation; // 0.01 to 100, this is vehicle setup float length; // 0.01 to 100, this is vehicle setup float width; // 0.01 to 100, this is vehicle setup float height; // 0.01 to 100, this is vehicle setup float wheelbase; // 93 to 1514 meter uint16_t maximumDistance; // 0 to 255, ARS548FrequencySlot uint8_t frequencySlot; // 0 to 255 represents for 50 to 100 uint8_t cycleTime; // 0 to 255 represents for 10 to 99 uint8_t timeSlot; // 0 to 255, country code, ARS548CountryCode uint8_t hcc; // 0 to 255, ARS548PowerSaveStandstillMode uint8_t powerSaveStandstill; // (0-255).(0-255).(0-255).(0-255) uint32_t sensorIPAddress0; // Reserved uint32_t sensorIPAddress1; // increase after configuration accepted uint8_t configurationCounter; // 0 to 255, ARS548VDYStatus uint8_t statusLongitudinalVelocity; // 0 to 255, ARS548VDYStatus uint8_t statusLongitudinalAcceleration; // 0 to 255, ARS548VDYStatus uint8_t statusLateralAcceleration; // 0 to 255, ARS548VDYStatus uint8_t statusYawRate; // 0 to 255, ARS548VDYStatus uint8_t statusSteeringAngle; // 0 to 255, ARS548VDYStatus uint8_t statusDrivingDirection; // 0 to 255, ARS548VDYStatus uint8_t statusCharacteristicSpeed; // 0 to 255, ARS548SensorStatus uint8_t statusRadar; }; } // namespace Driver ================================================ FILE: ars548_driver/data_type/StatusDef.hpp ================================================ #pragma once namespace Driver { enum ARS548RadarDetectionFlags { VALID_ALL = 0x00, INVALID_DISTANCE = 0x01, INVALID_DISTANCE_STD = 0x02, INVALID_AZIMUTH = 0x04, INVALID_AZIMUTH_STD = 0x08, INVALID_ELEVATION = 0x10, INVALID_ELEVATION_STD = 0x20, INVALID_RANGE_RATE = 0x40, INVALID_RANGE_RATE_STD = 0x80 }; enum ARS548RadarDetectionClasses { NO_CLASSIFICATION = 0, CLASS_NOISE = 1, CLASS_GROUND = 2, CLASS_TRAVERSABLE_UNDER = 3, CLASS_OBSTACLE = 4, CLASS_INVALID = 255 }; enum ARS548SyncStatus { SYNC_OK = 1, SYNC_NEVER_SYNC = 2, SYNC_LOST = 3, }; enum ARS548AlignmentStatus { ALIGNMENT_INIT = 0, ALIGNMENT_OK = 1, ALIGNMENT_NOTOK = 2, }; enum ARS548ObjectStatus { OBJECT_MEASURED = 0, OBJECT_NEW = 1, OBJECT_PREDICTED = 2, OBJECT_MEASUREMENT_INVALID = 255, }; enum ARS548MovementStatus { OBJECT_MOVED = 0, OBJECT_STATIONARY = 1, OBJECT_MOVEMENT_INVALID = 255, }; enum ARS548PositionReference { REFERENCE_CORNER_FRONT_LEFT = 0, REFERENCE_MIDDLE_FRONT = 1, REFERENCE_CORNER_FRONT_RIGHT = 2, REFERENCE_MIDDLE_SIDE_RIGHT = 3, REFERENCE_CORNER_REAR_RIGHT = 4, REFERENCE_MIDDLE_REAR = 5, REFERENCE_CORNER_REAR_LEFT = 6, REFERENCE_MIDDLE_SIDE_LEFT = 7, REFERENCE_SIGNAL_UNFILLED = 255, }; enum ARS548ShapeStatus { SHAPE_COMPLETELY_VISIBLE = 0, SHAPE_PARTIALLY_OCCLUDED = 1, SHAPE_COMPLETELY_OCCLUDED = 2, SHAPE_INVALID = 255, }; enum ARS548PlugOrientation { PLUG_RIGHT = 0, PLUG_LEFT = 1, }; enum ARS548FrequencySlot { // 76.23 GHz LOW = 0, // 76.48 GHz MID = 1, // 76.73 GHz HIGH = 2, }; enum ARS548CountryCode { WORLDWIDE = 1, JAPAN = 2, }; enum ARS548PowerSaveStandstillMode { OFF = 0, ON = 1, }; enum ARS548VDYStatus { VDY_OK = 0, VDY_NOTOK = 1, }; enum ARS548SensorStatus { SENSOR_STATE_INIT = 0, SENSOR_STATE_OK = 1, SENSOR_STATE_INVALID = 2, }; enum ARS548ParameterConfiguration { IGNORE_PARAMETERS = 0, USE_PARAMETERS = 1, }; enum ARS548VehicleDirection { DIRECTION_VOID = 0, DIRECTION_FORWARD = 1, DIRECTION_BACKWARDS = 2, }; } // namespace Driver ================================================ FILE: ars548_driver/data_type/TypeConverter.hpp ================================================ #pragma once #include #include inline uint8_t byteArrayToUint8(const uint8_t *array) { return *array; } inline int8_t byteArrayToInt8(const uint8_t *array) { return *reinterpret_cast(array); } inline uint16_t byteArrayToUint16(const uint8_t *array) { return ntohs(*reinterpret_cast(array)); } inline int16_t byteArrayToInt16(const uint8_t *array) { uint16_t temp = ntohs(*reinterpret_cast(array)); return *reinterpret_cast(&temp); } inline uint32_t byteArrayToUint32(const uint8_t *array) { return ntohl(*reinterpret_cast(array)); } inline int32_t byteArrayToInt32(const uint8_t *array) { uint32_t temp = ntohl(*reinterpret_cast(array)); return *reinterpret_cast(&temp); } inline uint64_t byteArrayToUint64(const uint8_t *array) { return static_cast(*array) << 56 | static_cast(*(array + 1)) << 48 | static_cast(*(array + 2)) << 40 | static_cast(*(array + 3)) << 32 | static_cast(*(array + 4)) << 24 | static_cast(*(array + 5)) << 16 | static_cast(*(array + 6)) << 8 | static_cast(*(array + 7)); } inline int64_t byteArrayToInt64(const uint8_t *array) { uint64_t temp = static_cast(*array) << 56 | static_cast(*(array + 1)) << 48 | static_cast(*(array + 2)) << 40 | static_cast(*(array + 3)) << 32 | static_cast(*(array + 4)) << 24 | static_cast(*(array + 5)) << 16 | static_cast(*(array + 6)) << 8 | static_cast(*(array + 7)); return *reinterpret_cast(&temp); } inline float byteArrayToFloat(const uint8_t *array) { uint32_t temp = ntohl(*reinterpret_cast(array)); return *reinterpret_cast(&temp); } inline double byteArrayToDouble(const uint8_t *array) { uint64_t temp = static_cast(*array) << 56 | static_cast(*(array + 1)) << 48 | static_cast(*(array + 2)) << 40 | static_cast(*(array + 3)) << 32 | static_cast(*(array + 4)) << 24 | static_cast(*(array + 5)) << 16 | static_cast(*(array + 6)) << 8 | static_cast(*(array + 7)); return *reinterpret_cast(&temp); } // inline void uint8ToByteArray(uint8_t value, uint8_t *array) { *array = value; // } // inline void int8ToByteArray(int8_t value, uint8_t *array) // { // *reinterpret_cast(array) = value; // } // inline void uint16ToByteArray(uint16_t value, uint8_t *array) // { // *reinterpret_cast(array) = value; // } // inline void int16ToByteArray(int16_t value, uint8_t *array) // { // *reinterpret_cast(array) = value; // } // inline void uint32ToByteArray(uint32_t value, uint8_t *array) // { // *reinterpret_cast(array) = value; // } // inline void int32ToByteArray(int32_t value, uint8_t *array) // { // *reinterpret_cast(array) = value; // } // inline void floatToByteArray(float value, uint8_t *array) // { // *reinterpret_cast(array) = value; // } ================================================ FILE: ars548_driver/data_type/VehicleInfo.hpp ================================================ #pragma once #include namespace Driver { struct VehicleAccLateral { // unused float accelerationLateralErrAmp; // unused uint8_t accelerationLateralErrAmpInvalidFlag; // unused uint8_t qualifierAccelerationLateral; // -65 TO 65 m/s^2 float accelerationLateral; // unused uint8_t accelerationLateralInvalidFlag; // unused uint8_t accelerationLateralEventDataQualifier; }; struct VehicleAccLongitudinal { // unused float accelerationLongitudinalErrAmp; // unused uint8_t accelerationLongitudinalErrAmpInvalidFlag; // unused uint8_t qualifierAccelerationLongitudinal; // -65 TO 65 m/s^2 float accelerationLongitudinal; // unused uint8_t accelerationLongitudinalInvalidFlag; // unused uint8_t accelerationLongitudinalEventDataQualifier; }; struct VehicleCharacteristicSpeed { // unused uint8_t characteristicSpeedErrAmp; // unused uint8_t qualifierCharacteristicSpeed; // 0 to 255, km/h uint8_t characteristicSpeed; }; struct VehicleDrivingDirection { // unused uint8_t drivingDirectionUnconfirmed; // ARS548VehicleDirection uint8_t drivingDirectionConfirmed; }; struct VehicleSteeringAngle { // unused uint8_t qualifierSteeringAngleFrontAxle; // unused float steeringAngleFrontAxleErrAmp; // unused uint8_t steeringAngleFrontAxleErrAmpInvalidFlag; // -90 to 90, degree float steeringAngleFrontAxle; // unused uint8_t steeringAngleFrontAxleInvalidFlag; // unused uint8_t steeringAngleFrontAxleEventDataQualifier; }; struct VehicleVelocity { // unused uint8_t statusVelocityNearStandstill; // unused uint8_t qualifierVelocityVehicle; // unused uint8_t velocityVehicleEventDataQualifier; // 0 to 350 km/h float velocityVehicle; // unused uint8_t velocityVehicleInvalidFlag; }; struct VehicleYawRate { // unused float yawRateErrAmp; // unused uint8_t yawRateErrAmpInvalidFlag; // unused uint8_t qualifierYawRate; // -163.84 to 163.83 deg/s float yawRate; // unused uint8_t yawRateInvalidFlag; // unused uint8_t yawRateEventDataQualifier; }; } // namespace Driver ================================================ FILE: ars548_driver/msg/AccelerationLateralCog.msg ================================================ std_msgs/Header header float32 accelerationLateralErrAmp uint8 accelerationLateralErrAmpInvalidFlag uint8 qualifierAccelerationLateral float32 accelerationLateral uint8 accelerationLateralInvalidFlag uint8 accelerationLateralEventDataQualifier ================================================ FILE: ars548_driver/msg/AccelerationLongitudinalCog.msg ================================================ std_msgs/Header header float32 accelerationLongitudinalErrAmp uint8 accelerationLongitudinalErrAmpInvalidFlag uint8 qualifierAccelerationLongitudinal float32 accelerationLongitudinal uint8 accelerationLongitudinalInvalidFlag uint8 accelerationLongitudinalEventDataQualifier ================================================ FILE: ars548_driver/msg/CharacteristicSpeed.msg ================================================ std_msgs/Header header uint8 characteristicSpeedErrAmp uint8 qualifierCharacteristicSpeed uint8 characteristicSpeed ================================================ FILE: ars548_driver/msg/DetectionList.msg ================================================ uint16 serviceID uint16 MethodID uint32 dataLength uint16 clientID uint16 sessionID uint8 protocolVersion uint8 interfaceVersion uint8 messageType uint8 returnCode uint64 crc uint32 length uint32 sqc uint32 dataID uint32 timestampNanoseconds uint32 timestampSeconds uint8 timestampSyncStatus uint32 eventDataQualifier uint8 extendedQualifier uint16 originInvalidFlags float32 originXPos float32 originXSTD float32 originYPos float32 originYSTD float32 originZPos float32 originZSTD float32 originRoll float32 originRollSTD float32 originPitch float32 originPitchSTD float32 originYaw float32 originYawSTD uint8 listInvalidFlags detections[] detectionArray float32 listRadVelDomainMin float32 listRadVelDomainMax uint32 listNumOfDetections float32 alnAzimuthCorrection float32 alnElevationCorrection uint8 alnStatus ================================================ FILE: ars548_driver/msg/Detections.msg ================================================ std_msgs/Header header float32 azimuthAngle float32 azimuthSTD float32 elevationAngle float32 elevationSTD float32 range float32 rangeSTD float32 rangeRate float32 rangeRateSTD uint8 InvalidFlags int8 rcs uint16 measurementID uint8 positivePredictiveValue uint8 classification uint8 multiTargetProbability uint16 objectID uint8 ambiguityFlag uint16 sortIndex ================================================ FILE: ars548_driver/msg/DrivingDirection.msg ================================================ std_msgs/Header header uint8 drivingDirectionUnconfirmed uint8 drivingDirectionConfirmed ================================================ FILE: ars548_driver/msg/ObjectList.msg ================================================ uint16 serviceID uint16 methodID uint32 dataLength uint16 clientID uint16 sessionID uint8 protocolVersion uint8 interfaceVersion uint8 messageType uint8 returnCode uint64 crc uint32 length uint32 sqc uint32 dataID uint32 timestampNanoseconds uint32 timestampSeconds uint8 timestampSyncStatus uint32 eventDataQualifier uint8 extendedQualifier uint8 objectListNumOfObjects objects[] objectArray ================================================ FILE: ars548_driver/msg/Objects.msg ================================================ std_msgs/Header header uint16 statusSensor uint32 id uint16 age uint8 statusMeasurement uint8 statusMovement uint16 positionInvalidFlags uint8 position_Reference float32 positionX float32 positionXSTD float32 positionY float32 positionYSTD float32 positionZ float32 positionZSTD float32 positionCovarianceXY float32 positionOrientation float32 positionOrientationSTD uint8 existenceInvalidFlags float32 existenceProbability float32 existencePPV uint8 classificationCar uint8 classificationTruck uint8 classificationMotorcycle uint8 classificationBicycle uint8 classificationPedestrian uint8 classificationAnimal uint8 classificationHazard uint8 classificationUnknown uint8 classificationOverdrivable uint8 classificationUnderdrivable uint8 dynamicsAbsVelInvalidFlags float32 dynamicsAbsVelX float32 dynamicsAbsVelXSTD float32 dynamicsAbsVelY float32 dynamicsAbsVelYSTD float32 dynamicsAbsVelCovarianceXY uint8 dynamicsRelVelInvalidFlags float32 dynamicsRelVelX float32 dynamicsRelVelXSTD float32 dynamicsRelVelY float32 dynamicsRelVelYSTD float32 dynamicsRelVelCovarianceXY uint8 dynamics_AbsAccel_InvalidFlags float32 dynamicsAbsAccelX float32 dynamicsAbsAccelXSTD float32 dynamicsAbsAccelY float32 dynamicsAbsAccelYSTD float32 dynamicsAbsAccelCovarianceXY uint8 dynamicsRelAccelInvalidFlags float32 dynamicsRelAccelX float32 dynamicsRelAccelXSTD float32 dynamicsRelAccelY float32 dynamicsRelAccelYSTD float32 dynamicsRelAccelCovarianceXY uint8 dynamicsOrientationInvalidFlags float32 dynamicsOrientationRateMean float32 dynamicsOrientationRateSTD uint32 shapeLengthStatus uint8 shapeLengthEdgeInvalidFlags float32 shapeLengthEdgeMean float32 shapeLengthEdgeSTD uint32 shapeWidthStatus uint8 shapeWidthEdgeInvalidFlags float32 shapeWidthEdgeMean float32 shapeWidthEdgeSTD ================================================ FILE: ars548_driver/msg/RadarBasicStatus.msg ================================================ std_msgs/Header header float32 longitudinal float32 lateral float32 vertical float32 yaw float32 pitch uint8 plugOrientation float32 length float32 width float32 height float32 wheelbase uint16 maximumDistance uint8 frequencySlot uint8 cycleTime uint8 timeSlot uint8 hcc uint8 powerSaveStandstill uint32 sensorIPAddress0 uint32 sensorIPAddress1 uint8 configurationCounter uint8 statusLongitudinalVelocity uint8 statusLongitudinalAcceleration uint8 statusLateralAcceleration uint8 statusYawRate uint8 statusSteeringAngle uint8 statusDrivingDirection uint8 statusCharacteristicSpeed uint8 statusRadar ================================================ FILE: ars548_driver/msg/SteeringAngleFrontAxle.msg ================================================ std_msgs/Header header uint8 qualifierSteeringAngleFrontAxle float32 steeringAngleFrontAxleErrAmp uint8 steeringAngleFrontAxleErrAmpInvalidFlag float32 steeringAngleFrontAxle uint8 steeringAngleFrontAxleInvalidFlag uint8 steeringAngleFrontAxleEventDataQualifier ================================================ FILE: ars548_driver/msg/VelocityVehicle.msg ================================================ std_msgs/Header header uint8 statusVelocityNearStandstill uint8 qualifierVelocityVehicle uint8 velocityVehicleEventDataQualifier float32 velocityVehicle uint8 velocityVehicleInvalidFlag ================================================ FILE: ars548_driver/msg/YawRate.msg ================================================ std_msgs/Header header float32 yawRateErrAmp uint8 yawRateErrAmpInvalidFlag uint8 qualifierYawRate float32 yawRate uint8 yawRateInvalidFlag uint8 yawRateEventDataQualifier ================================================ FILE: ars548_driver/node/ARS548_node.cpp ================================================ #include #include #include #include #include #include "ARS548.hpp" #include "ros/publisher.h" #include "ros/ros.h" #include "ros/time.h" #include "sensor_msgs/PointCloud2.h" #include "sensor_msgs/PointField.h" // #include "sensor_msgs/PointCloud.h" Driver::ARS548 ars548{}; bool timeInit = false; ros::Time firstReceiveTime; ros::Time firstTimeStamp; int main(int argc, char **argv) { ros::init(argc, argv, "ARS548_Driver"); ros::NodeHandle nodeHandle("~"); ros::Publisher pointCloudPub = nodeHandle.advertise("/radar_raw_message", 10); sensor_msgs::PointField fieldAzimuth; fieldAzimuth.name = "azimuth"; fieldAzimuth.count = 1; fieldAzimuth.offset = 0; fieldAzimuth.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldAzimuthSTD; fieldAzimuthSTD.name = "azimuthSTD"; fieldAzimuthSTD.count = 1; fieldAzimuthSTD.offset = 4; fieldAzimuthSTD.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldElevation; fieldElevation.name = "elevation"; fieldElevation.count = 1; fieldElevation.offset = 8; fieldElevation.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldElevationSTD; fieldElevationSTD.name = "elevationSTD"; fieldElevationSTD.count = 1; fieldElevationSTD.offset = 12; fieldElevationSTD.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldRange; fieldRange.name = "range"; fieldRange.count = 1; fieldRange.offset = 16; fieldRange.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldRangeSTD; fieldRangeSTD.name = "rangeSTD"; fieldRangeSTD.count = 1; fieldRangeSTD.offset = 20; fieldRangeSTD.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldVelocity; fieldVelocity.name = "velocity"; fieldVelocity.count = 1; fieldVelocity.offset = 24; fieldVelocity.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldVelocitySTD; fieldVelocitySTD.name = "velocitySTD"; fieldVelocitySTD.count = 1; fieldVelocitySTD.offset = 28; fieldVelocitySTD.datatype = sensor_msgs::PointField::FLOAT32; sensor_msgs::PointField fieldRCS; fieldRCS.name = "rcs"; fieldRCS.count = 1; fieldRCS.offset = 32; fieldRCS.datatype = sensor_msgs::PointField::INT8; sensor_msgs::PointCloud2::_fields_type fields; fields.emplace_back(fieldAzimuth); fields.emplace_back(fieldAzimuthSTD); fields.emplace_back(fieldElevation); fields.emplace_back(fieldElevationSTD); fields.emplace_back(fieldRange); fields.emplace_back(fieldRangeSTD); fields.emplace_back(fieldVelocity); fields.emplace_back(fieldVelocitySTD); fields.emplace_back(fieldRCS); ars548.init("eno1", "224.0.2.2", 42102, "10.13.1.113", 42401, 42101); while (ros::ok()) { ros::Time ars548TimeStamp; ars548.receiveMsg(); if (ars548.getDetectionListReady()) { auto &detectionsArray = ars548.getDetectionList(); if (!timeInit) { firstReceiveTime = ros::Time::now(); firstTimeStamp.sec = detectionsArray.timestampS; firstTimeStamp.nsec = detectionsArray.timestampNs; timeInit = true; } ars548TimeStamp.sec = detectionsArray.timestampS; ars548TimeStamp.nsec = detectionsArray.timestampNs; sensor_msgs::PointCloud2 pubFrame; pubFrame.header.frame_id = "world"; pubFrame.header.stamp = firstReceiveTime + (ars548TimeStamp - firstTimeStamp); pubFrame.fields = fields; pubFrame.is_bigendian = sensor_msgs::PointCloud2::_is_bigendian_type(false); pubFrame.is_dense = sensor_msgs::PointCloud2::_is_dense_type(true); pubFrame.point_step = 33; for (int index = 0; index < detectionsArray.listNumOfDetections; index++) { auto &detection = detectionsArray.detectionArray[index]; if (detection.invalidFlags == 0) { uint32_t byteArray = 0; byteArray = *reinterpret_cast(&detection.azimuthAngle); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast(&detection.azimuthSTD); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast( &detection.elevationAngle); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast(&detection.elevationSTD); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast(&detection.range); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast(&detection.rangeSTD); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast(&detection.rangeRate); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); byteArray = *reinterpret_cast(&detection.rangeRateSTD); pubFrame.data.emplace_back( static_cast(byteArray & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 8) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 16) & 0xFF)); pubFrame.data.emplace_back( static_cast((byteArray >> 24) & 0xFF)); pubFrame.data.emplace_back(detection.rcs); pubFrame.row_step++; } } pointCloudPub.publish(pubFrame); } // if (ars548.getObjectListReady()) // { // // // } // if (ars548.getRadarStatusReady()) // { // // // } ars548.clearReadyFlag(); } return 0; } ================================================ FILE: ars548_driver/package.xml ================================================ ars548_driver 0.0.0 The ars548_driver package root TODO catkin roscpp roscpp roscpp ================================================ FILE: ars548_driver/src/ARS548.cpp ================================================ #include "ARS548.hpp" namespace Driver { ARS548::ARS548(std::string device, std::string multicastIP, int multicastPort, std::string unicastIP, int unicastSrcPort, int unicastDstPort) { init(device, multicastIP, multicastPort, unicastIP, unicastSrcPort, unicastDstPort); } void ARS548::init(std::string device, std::string multicastIP, int multicastPort, std::string unicastIP, int unicastSrcPort, int unicastDstPort) { if (!io.initDevice(device)) std::cout << "device init error" << std::endl; if (!io.initUdpMulticastServer(multicastIP, multicastPort)) std::cout << "multicast server init error" << std::endl; if (!io.initUdpUnicastClient(unicastIP, unicastDstPort, unicastSrcPort)) std::cout << "unicast client init error" << std::endl; } void ARS548::receiveMsg() { long length = io.receiveFromRadar(data); // std::cout << "length:" << length << std::endl; switch (length) { case 9401: { // std::cout << "receive object list" << std::endl; objectList = coder.decodeObjectListMessage(data); objectListReady = true; } break; case 35336: { // std::cout << "receive detection list" << std::endl; detectionList = coder.decodeDetectionListMessage(data); detectionListReady = true; } break; case 69: { // std::cout << "receive radar status" << std::endl; radarStatus = coder.decodeBasicStatusMessage(data); radarStatusReady = true; } break; default: std::cout << "unknown message" << std::endl; break; } } void ARS548::sendMsg() { // int n; // n = sendto(socket_client_fd, // data, // len, // 0, // (struct sockaddr *)&addr_serv, // sizeof(addr_serv)); // return (n); } bool ARS548::getDetectionListReady() const { return detectionListReady; } bool ARS548::getObjectListReady() const { return objectListReady; } bool ARS548::getRadarStatusReady() const { return radarStatusReady; } void ARS548::clearReadyFlag() { detectionListReady = false; objectListReady = false; radarStatusReady = false; } ARS548RadarDetectionList &ARS548::getDetectionList() { return detectionList; } ARS548RadarObjectList &ARS548::getObjectList() { return objectList; } ARS548RadarStatus &ARS548::getRadarStatus() { return radarStatus; } ARS548::~ARS548() { io.closeIO(); } }; // namespace Driver ================================================ FILE: ars548_driver/src/ARS548.hpp ================================================ #pragma once #include #include #include #include #include #include #include #include #include #include "ARS548Coder.hpp" namespace Driver { class ARS548 { public: ARS548() = default; ARS548(std::string device, std::string multicastIP, int multicastPort, std::string unicastIP, int unicastSrcPort, int unicastDstPort); ~ARS548(); ARS548(ARS548 &another) = delete; ARS548(ARS548 &&another) = delete; ARS548 &operator=(const ARS548 &another) = delete; ARS548 &operator=(ARS548 &&another) = delete; void init(std::string device, std::string multicastIP, int multicastPort, std::string unicastIP, int unicastSrcPort, int unicastDstPort); void receiveMsg(); void sendMsg(); bool getDetectionListReady() const; bool getObjectListReady() const; bool getRadarStatusReady() const; void clearReadyFlag(); ARS548RadarDetectionList &getDetectionList(); ARS548RadarObjectList &getObjectList(); ARS548RadarStatus &getRadarStatus(); private: class UdpIO { public: UdpIO() = default; ~UdpIO() = default; UdpIO(UdpIO &another) = delete; UdpIO(UdpIO &&another) = delete; UdpIO &operator=(const UdpIO &another) = delete; UdpIO &operator=(UdpIO &&another) = delete; bool initDevice(std::string device); bool initUdpMulticastServer(std::string ipAddress, int port); bool initUdpUnicastClient(std::string dstIpAddress, int dstPort, int localPort); static void listAllDevice(); long receiveFromRadar(uint8_t *data); long sendToRadar(uint8_t *data, int len); bool closeIO() const; static bool decodeIPHeader(const uint8_t *data, bool &noFragment, bool &moreFragment, int &offsetFragment, int &frameID, int &dataOffset, int &dataLength); private: pcap_t *deviceHandler = nullptr; } io{}; uint8_t data[40000]{}; ARS548Coder coder = {}; bool detectionListReady = false; bool objectListReady = false; bool radarStatusReady = false; ARS548RadarDetectionList detectionList = {}; ARS548RadarObjectList objectList = {}; ARS548RadarStatus radarStatus = {}; }; } // namespace Driver ================================================ FILE: ars548_driver/src/ARS548Coder.cpp ================================================ #include #include #include "ARS548Coder.hpp" namespace Driver { ARS548RadarObjectList ARS548Coder::decodeObjectListMessage( const uint8_t *data) const { ARS548RadarObjectList result{}; // decode header result.serviceID = byteArrayToUint16(data); // methodID should be 329 result.methodID = byteArrayToUint16(data + 2); // dataLength should be 9401 - 8 result.dataLength = byteArrayToUint32(data + 4); result.clientID = byteArrayToUint16(data + 8); result.sessionID = byteArrayToUint16(data + 10); result.protocolVersion = byteArrayToUint8(data + 12); result.interfaceVersion = byteArrayToUint8(data + 13); result.messageType = byteArrayToUint8(data + 14); result.returnCode = byteArrayToUint8(data + 15); // decoder body result.crc = byteArrayToInt64(data + 16); result.length = byteArrayToUint32(data + 24); result.sqc = byteArrayToUint32(data + 28); result.dataID = byteArrayToUint32(data + 32); result.timestampNs = byteArrayToUint32(data + 36); result.timestampS = byteArrayToUint32(data + 40); result.timestampSyncStatus = byteArrayToUint8(data + 44); result.eventDataQualifier = byteArrayToUint32(data + 45); result.extendedQualifier = byteArrayToUint8(data + 49); // number should less than 51 result.objectListNumOfObjects = byteArrayToUint8(data + 50); // object list uint8_t *base = nullptr; for (int i = 0; i < result.objectListNumOfObjects; i++) result.objectArray[i] = decodeObjectMessage(data + static_cast(51 + i * 187)); return result; } ARS548RadarObject ARS548Coder::decodeObjectMessage(const uint8_t *data) const { ARS548RadarObject result{}; result.statusSensor = byteArrayToUint16(data); result.id = byteArrayToUint32(data + 2); result.age = byteArrayToUint16(data + 6); result.statusMeasurement = byteArrayToUint8(data + 8); result.statusMovement = byteArrayToUint8(data + 9); result.positionInvalidFlags = byteArrayToUint16(data + 10); result.positionReference = byteArrayToUint8(data + 12); result.positionX = byteArrayToFloat(data + 13); result.positionXSTD = byteArrayToFloat(data + 17); result.positionY = byteArrayToFloat(data + 21); result.positionYSTD = byteArrayToFloat(data + 25); result.positionZ = byteArrayToFloat(data + 29); result.positionZSTD = byteArrayToFloat(data + 33); result.positionCovarianceXY = byteArrayToFloat(data + 37); result.positionOrientation = byteArrayToFloat(data + 41); result.positionOrientationSTD = byteArrayToFloat(data + 45); result.existenceInvalidFlags = byteArrayToUint8(data + 49); result.existenceProbability = byteArrayToFloat(data + 50); result.existencePPV = byteArrayToFloat(data + 54); result.classificationCar = byteArrayToUint8(data + 58); result.classificationTruck = byteArrayToUint8(data + 59); result.classificationMotorcycle = byteArrayToUint8(data + 60); result.classificationBicycle = byteArrayToUint8(data + 61); result.classificationPedestrian = byteArrayToUint8(data + 62); result.classificationAnimal = byteArrayToUint8(data + 63); result.classificationHazard = byteArrayToUint8(data + 64); result.classificationUnknown = byteArrayToUint8(data + 65); result.classificationOverdrivable = byteArrayToUint8(data + 66); result.classificationUnderdrivable = byteArrayToUint8(data + 67); result.dynamicsAbsVelInvalidFlags = byteArrayToUint8(data + 68); result.dynamicsAbsVelX = byteArrayToFloat(data + 69); result.dynamicsAbsVelXSTD = byteArrayToFloat(data + 73); result.dynamicsAbsVelY = byteArrayToFloat(data + 77); result.dynamicsAbsVelYSTD = byteArrayToFloat(data + 81); result.dynamicsAbsVelCovarianceXY = byteArrayToFloat(data + 85); result.dynamicsRelVelInvalidFlags = byteArrayToUint8(data + 89); result.dynamicsRelVelX = byteArrayToFloat(data + 90); result.dynamicsRelVelXSTD = byteArrayToFloat(data + 94); result.dynamicsRelVelY = byteArrayToFloat(data + 98); result.dynamicsRelVelYSTD = byteArrayToFloat(data + 102); result.dynamicsRelVelCovarianceXY = byteArrayToFloat(data + 106); result.dynamicsAbsAccelInvalidFlags = byteArrayToUint8(data + 110); result.dynamicsAbsAccelX = byteArrayToFloat(data + 111); result.dynamicsAbsAccelXSTD = byteArrayToFloat(data + 115); result.dynamicsAbsAccelY = byteArrayToFloat(data + 119); result.dynamicsAbsAccelYSTD = byteArrayToFloat(data + 123); result.dynamicsAbsAccelCovarianceXY = byteArrayToFloat(data + 127); result.dynamicsRelAccelInvalidFlags = byteArrayToUint8(data + 131); result.dynamicsRelAccelX = byteArrayToFloat(data + 132); result.dynamicsRelAccelXSTD = byteArrayToFloat(data + 136); result.dynamicsRelAccelY = byteArrayToFloat(data + 140); result.dynamicsRelAccelYSTD = byteArrayToFloat(data + 144); result.dynamicsRelAccelCovarianceXY = byteArrayToFloat(data + 148); result.dynamicsOrientationInvalidFlags = byteArrayToUint8(data + 152); result.dynamicsOrientationRateMean = byteArrayToFloat(data + 153); result.dynamicsOrientationRateSTD = byteArrayToFloat(data + 157); result.shapeLengthStatus = byteArrayToUint32(data + 161); result.shapeLengthEdgeInvalidFlags = byteArrayToUint8(data + 165); result.shapeLengthEdgeMean = byteArrayToFloat(data + 166); result.shapeLengthEdgeSTD = byteArrayToFloat(data + 170); result.shapeWidthStatus = byteArrayToUint32(data + 174); result.shapeWidthEdgeInvalidFlags = byteArrayToUint8(data + 178); result.shapeWidthEdgeMean = byteArrayToFloat(data + 179); result.shapeWidthEdgeSTD = byteArrayToFloat(data + 183); return result; } ARS548RadarDetectionList ARS548Coder::decodeDetectionListMessage( const uint8_t *data) const { ARS548RadarDetectionList result{}; // header result.serviceID = byteArrayToUint16(data); // method ID should be 336 result.methodID = byteArrayToUint16(data + 2); // dataLength should be 35336 - 8 result.dataLength = byteArrayToUint32(data + 4); result.clientID = byteArrayToUint16(data + 8); result.sessionID = byteArrayToUint16(data + 10); result.protocolVersion = byteArrayToUint8(data + 12); result.interfaceVersion = byteArrayToUint8(data + 13); result.messageType = byteArrayToUint8(data + 14); result.returnCode = byteArrayToUint8(data + 15); // body result.crc = byteArrayToUint64(data + 16); result.length = byteArrayToUint32(data + 24); result.sqc = byteArrayToUint32(data + 28); result.dataID = byteArrayToUint32(data + 32); result.timestampNs = byteArrayToUint32(data + 36); result.timestampS = byteArrayToUint32(data + 40); result.timestampSyncStatus = byteArrayToUint8(data + 44); result.eventDataQualifier = byteArrayToUint32(data + 45); result.extendedQualifier = byteArrayToUint8(data + 49); result.originInvalidFlags = byteArrayToUint32(data + 50); result.originX = byteArrayToFloat(data + 52); result.originXSTD = byteArrayToFloat(data + 56); result.originY = byteArrayToFloat(data + 60); result.originYSTD = byteArrayToFloat(data + 64); result.originZ = byteArrayToFloat(data + 68); result.originZSTD = byteArrayToFloat(data + 72); result.originRoll = byteArrayToFloat(data + 76); result.originRollSTD = byteArrayToFloat(data + 80); result.originPitch = byteArrayToFloat(data + 84); result.originPitchSTD = byteArrayToFloat(data + 88); result.originYaw = byteArrayToFloat(data + 92); result.originYawSTD = byteArrayToFloat(data + 96); result.listInvalidFlags = byteArrayToUint8(data + 100); result.listRadVelDomainMin = byteArrayToFloat(data + 35301); result.listRadVelDomainMax = byteArrayToFloat(data + 35305); result.listNumOfDetections = byteArrayToUint32(data + 35309); result.alnAzimuthCorrection = byteArrayToFloat(data + 35313); result.alnElevationCorrection = byteArrayToFloat(data + 35317); result.alnStatus = byteArrayToUint8(data + 35321); // detection list for (int i = 0; i < result.listNumOfDetections; i++) result.detectionArray[i] = decodeDetectionMessage(data + static_cast(101 + i * 44)); return result; } ARS548RadarDetection ARS548Coder::decodeDetectionMessage( const uint8_t *data) const { // x = range * cos(elevationAngle) * cos(azimuthAngle); // y = range * cos(elevationAngle) * sin(azimuthAngle); // z = range * sin(elevationAngle); ARS548RadarDetection result{}; result.azimuthAngle = byteArrayToFloat(data); result.azimuthSTD = byteArrayToFloat(data + 4); result.invalidFlags = byteArrayToUint8(data + 8); result.elevationAngle = byteArrayToFloat(data + 9); result.elevationSTD = byteArrayToFloat(data + 13); result.range = byteArrayToFloat(data + 17); result.rangeSTD = byteArrayToFloat(data + 21); result.rangeRate = byteArrayToFloat(data + 25); result.rangeRateSTD = byteArrayToFloat(data + 29); result.rcs = byteArrayToInt8(data + 33); result.measurementID = byteArrayToUint16(data + 34); result.positivePredictiveValue = byteArrayToUint8(data + 36); result.classification = byteArrayToUint8(data + 37); result.multiTargetProbability = byteArrayToUint8(data + 38); result.objectID = byteArrayToUint32(data + 39); result.ambiguityFlag = byteArrayToUint8(data + 41); result.sortIndex = byteArrayToUint32(data + 42); return result; } ARS548RadarStatus ARS548Coder::decodeBasicStatusMessage( const uint8_t *data) const { ARS548RadarStatus result{}; uint16_t serviceID = 0; uint16_t methodID = 0; uint32_t length = 0; serviceID = byteArrayToUint16(data); // methodID should be 380 methodID = byteArrayToUint16(data + 2); // length should be 69 - 8 length = byteArrayToUint32(data + 4); result.longitudinal = byteArrayToFloat(data + 8); result.lateral = byteArrayToFloat(data + 12); result.vertical = byteArrayToFloat(data + 16); result.yaw = byteArrayToFloat(data + 20); result.pitch = byteArrayToFloat(data + 24); result.plugOrientation = byteArrayToUint8(data +28); result.length = byteArrayToFloat(data + 29); result.width = byteArrayToFloat(data + 33); result.height = byteArrayToFloat(data + 37); result.wheelbase = byteArrayToFloat(data + 41); result.maximumDistance = byteArrayToUint16(data + 45); result.frequencySlot = byteArrayToUint8(data +47); result.cycleTime = byteArrayToUint8(data +48); result.timeSlot = byteArrayToUint8(data +49); result.hcc = byteArrayToUint8(data +50); result.powerSaveStandstill = byteArrayToUint8(data +51); result.sensorIPAddress0 = byteArrayToUint32(data + 52); result.sensorIPAddress1 = byteArrayToUint32(data + 56); result.configurationCounter = byteArrayToUint8(data +60); result.statusLongitudinalVelocity = byteArrayToUint8(data +61); result.statusLongitudinalAcceleration = byteArrayToUint8(data +62); result.statusLateralAcceleration = byteArrayToUint8(data +63); result.statusYawRate = byteArrayToUint8(data +64); result.statusSteeringAngle = byteArrayToUint8(data +65); result.statusDrivingDirection = byteArrayToUint8(data +66); result.statusCharacteristicSpeed = byteArrayToUint8(data +67); result.statusRadar = byteArrayToUint8(data +68); return result; } } // namespace Driver ================================================ FILE: ars548_driver/src/ARS548Coder.hpp ================================================ #pragma once #include #include #include "../data_type/RadarDetection.hpp" #include "../data_type/RadarObject.hpp" #include "../data_type/RadarStatus.hpp" #include "../data_type/StatusDef.hpp" #include "../data_type/TypeConverter.hpp" namespace Driver { class ARS548Coder { public: ARS548Coder() = default; ~ARS548Coder() = default; ARS548Coder(ARS548Coder &another) = delete; ARS548Coder(ARS548Coder &&another) = delete; ARS548Coder &operator=(const ARS548Coder &another) = delete; ARS548Coder &operator=(ARS548Coder &&another) = delete; ARS548RadarObjectList decodeObjectListMessage(const uint8_t *data) const; ARS548RadarObject decodeObjectMessage(const uint8_t *data) const; ARS548RadarDetectionList decodeDetectionListMessage(const uint8_t *data) const; ARS548RadarDetection decodeDetectionMessage(const uint8_t *data) const; ARS548RadarStatus decodeBasicStatusMessage(const uint8_t *data) const; }; } // namespace Driver ================================================ FILE: ars548_driver/src/UdpIO.cpp ================================================ #include #include #include #include #include #include #include #include "ARS548.hpp" #include "pcap.h" namespace Driver { bool ARS548::UdpIO::initDevice(std::string device) { char buffer[1024]; deviceHandler = pcap_open_live(device.c_str(), 65535, 1, 20, buffer); if (deviceHandler == nullptr) { std::cout << "open device " << device << " error!" << std::endl << "available device list:" << std::endl; listAllDevice(); return false; } return true; } bool ARS548::UdpIO::initUdpMulticastServer(std::string ipAddress, int port) { bpf_program filter{}; if (pcap_compile(deviceHandler, &filter, "dst host 224.0.2.2", 1, 0) < 0) { std::cout << "compile filter rule error" << std::endl; return false; } if (pcap_setfilter(deviceHandler, &filter) < 0) { std::cout << "set filter error" << std::endl; return false; } return true; } bool ARS548::UdpIO::initUdpUnicastClient(std::string dstIpAddress, int dstPort, int localPort) { // sending message to radar, no support yet. // TODO return true; } void ARS548::UdpIO::listAllDevice() { pcap_if_t *allDev = nullptr; char buffer[1024]; pcap_findalldevs(&allDev, buffer); for (auto *ptr = allDev; ptr != nullptr; ptr = ptr->next) std::cout << ptr->name << std::endl; pcap_freealldevs(allDev); } long ARS548::UdpIO::receiveFromRadar(uint8_t *data) { pcap_pkthdr packetInfo{}; const uint8_t *packetContent = nullptr; packetContent = pcap_next(deviceHandler, &packetInfo); bool noFragment = false; bool moreFragment = false; int offsetFragment = 0; int frameID = 0; int dataOffset = 0; int dataLength = 0; bool res = decodeIPHeader(packetContent, noFragment, moreFragment, offsetFragment, frameID, dataOffset, dataLength); if (!res) return -1; // single frame if ((!noFragment) && (!moreFragment) && (offsetFragment == 0)) { memcpy(data, packetContent + dataOffset, dataLength); return dataLength; } bool startFrameReceived = false; bool endFrameReceived = false; long lengthReceived = 0; // multi-frame memcpy(data + offsetFragment - 8, packetContent + dataOffset, dataLength); lengthReceived += dataLength; while ((!startFrameReceived) || (!endFrameReceived)) { packetContent = pcap_next(deviceHandler, &packetInfo); res = decodeIPHeader(packetContent, noFragment, moreFragment, offsetFragment, frameID, dataOffset, dataLength); if (!res) { std::cout << "error!" << std::endl; return 0; } if (!moreFragment) endFrameReceived = true; // the start frame contains padding only if (offsetFragment == 0) { startFrameReceived = true; break; } memcpy(data + offsetFragment - 8, packetContent + dataOffset, packetInfo.caplen - dataOffset); lengthReceived += packetInfo.caplen - dataOffset; } if (lengthReceived == dataLength) { return lengthReceived; } return 0; } long ARS548::UdpIO::sendToRadar(uint8_t *data, int len) { // sending message to radar, no support yet. // TODO return -1; } bool ARS548::UdpIO::closeIO() const { pcap_close(deviceHandler); return true; } bool ARS548::UdpIO::decodeIPHeader(const uint8_t *data, bool &noFragment, bool &moreFragment, int &offsetFragment, int &frameID, int &dataOffset, int &dataLength) { uint16_t type = 0; int offset = 0; const ethhdr *eth = nullptr; eth = reinterpret_cast(data); type = ntohs(eth->h_proto); offset = sizeof(ethhdr); while (type == ETH_P_8021Q) { type = (*(data + offset + 2) << 8) | *(data + offset + 3); offset += 4; } if (type != ETH_P_IP) return false; const iphdr *ipHeader = reinterpret_cast(data + offset); uint16_t fragmentFlag = ntohs(ipHeader->frag_off); noFragment = (fragmentFlag & 0x4000) != 0; moreFragment = (fragmentFlag & 0x2000) != 0; offsetFragment = (fragmentFlag & 0x1FFF) << 3; frameID = ntohs(ipHeader->id); offset += sizeof(iphdr); if (ipHeader->protocol != IPPROTO_UDP) return false; // single frame or multi-frame start frame if (offsetFragment == 0) { dataOffset = offset + 8; dataLength = (*(data + offset + 4)) << 8 | (*(data + offset + 5)) - 8; } // multi-frame data frame if ((noFragment) && (offsetFragment != 0)) { dataOffset = offset; dataLength = ntohs(ipHeader->tot_len) - 20; } return true; } } // namespace Driver ================================================ FILE: dataset/.gitignore ================================================ */**/*.pcd ================================================ FILE: dataset/exp/Sequence_1.bag ================================================ [File too large to display: 27.2 MB] ================================================ FILE: dataset/exp/Sequence_2.bag ================================================ [File too large to display: 18.2 MB] ================================================ FILE: docker/Dockerfile ================================================ FROM ubuntu:20.04 # Environment variables ENV DEBIAN_FRONTEND=noninteractive ENV TZ=Asia/Beijing ENV WS=/root/ws ENV SHELL=/bin/bash # Arguments for build ARG OPENCV_VERSION=4.6.0 ARG CERES_VERSION=2.2.0 ARG ROS_VERSION=noetic # Install dependencies RUN apt-get update && \ apt-get install -y \ build-essential \ libgtk2.0-dev \ lsb-release \ net-tools \ cmake \ libgoogle-glog-dev \ libgflags-dev \ libatlas-base-dev \ libeigen3-dev \ libsuitesparse-dev \ git \ wget \ curl \ htop \ xterm \ libpcap-dev \ binutils-dev \ libdw-dev \ libdwarf-dev \ gdb && \ apt-get clean # Install ROS RUN sh -c 'echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ `lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list' && \ curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ apt-get update && \ apt-get install -y \ ros-${ROS_VERSION}-ros-base \ ros-${ROS_VERSION}-nav-msgs \ ros-${ROS_VERSION}-sensor-msgs \ ros-${ROS_VERSION}-cv-bridge \ ros-${ROS_VERSION}-rviz \ ros-${ROS_VERSION}-pcl-ros \ ros-${ROS_VERSION}-image-transport-plugins \ ros-${ROS_VERSION}-rqt-gui \ ros-${ROS_VERSION}-rqt-common-plugins \ ros-${ROS_VERSION}-catkin \ python3-rosdep \ python3-rosinstall \ python3-rosinstall-generator \ python3-wstool \ python3-rosdep \ python3-catkin-tools && \ apt-get clean && \ rosdep init && \ rosdep update && \ echo "source /opt/ros/${ROS_VERSION}/setup.bash" >> ~/.bashrc # Install Mavros and Mavlink RUN apt-get update && \ apt-get install -y \ ros-${ROS_VERSION}-mavros \ ros-${ROS_VERSION}-mavros-msgs \ ros-${ROS_VERSION}-mavros-extras \ ros-${ROS_VERSION}-mavlink && \ apt-get clean && \ wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh && \ bash ./install_geographiclib_datasets.sh # Install python3 packages RUN apt-get update && \ apt-get install -y \ python3-pip && \ apt-get clean && \ python3 -m pip install evo --upgrade --no-binary evo && \ python3 -m pip install packaging # Update cmake RUN apt-get update && \ apt-get install -y \ ca-certificates \ gpg \ wget \ software-properties-common && \ wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null && \ echo 'deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ focal main' | tee /etc/apt/sources.list.d/kitware.list >/dev/null && \ apt-get update && \ apt-get install -y cmake && \ apt-get clean #Install OpenCV4 ENV OPENCV_SOURCE_DIR="/root/3rdparty" ENV INSTALL_DIR="/usr/local" RUN mkdir -p $OPENCV_SOURCE_DIR && cd $OPENCV_SOURCE_DIR && \ wget -O opencv.zip https://github.com/Itseez/opencv/archive/${OPENCV_VERSION}.zip && \ wget -O opencv_contrib.zip https://github.com/Itseez/opencv_contrib/archive/${OPENCV_VERSION}.zip && \ apt-get install -y unzip && \ unzip opencv.zip && \ unzip opencv_contrib.zip && \ cd $OPENCV_SOURCE_DIR/opencv-${OPENCV_VERSION} && \ mkdir build && \ cd build && \ cmake -D CMAKE_BUILD_TYPE=RELEASE \ -D CMAKE_INSTALL_PREFIX=${INSTALL_DIR} \ -D ENABLE_FAST_MATH=ON \ -D WITH_CUBLAS=ON \ -D WITH_LIBV4L=ON \ -D WITH_V4L=ON \ -D WITH_GSTREAMER=ON \ -D WITH_GSTREAMER_0_10=OFF \ -D WITH_QT=ON \ -D WITH_OPENGL=ON \ -D BUILD_opencv_python2=OFF \ -D BUILD_opencv_python3=OFF \ -D BUILD_TESTS=OFF \ -D BUILD_PERF_TESTS=OFF \ -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib-${OPENCV_VERSION}/modules \ ../ && \ make -j$(nproc) && \ make install && \ ldconfig && \ rm -rf $OPENCV_SOURCE_DIR/* # ceres-solver-2.2.0 RUN cd $OPENCV_SOURCE_DIR && \ wget http://ceres-solver.org/ceres-solver-${CERES_VERSION}.tar.gz && \ tar zxf ceres-solver-${CERES_VERSION}.tar.gz && \ mkdir ceres-bin && \ cd ceres-bin && \ cmake ../ceres-solver-${CERES_VERSION} && \ make -j$(nproc) && \ make install && \ rm -rf $OPENCV_SOURCE_DIR/* # Install backword-cpp RUN cd $OPENCV_SOURCE_DIR && \ git clone https://github.com/bombela/backward-cpp.git && \ cd backward-cpp && \ mkdir -p build && \ cd build && \ cmake .. && \ make -j$(nproc) && \ make install && \ rm -rf $OPENCV_SOURCE_DIR/* ================================================ FILE: docker/docker.sh ================================================ #!/bin/bash SCRIPT_DIR=$(cd $(dirname $0); pwd) function build() { docker build \ -t rio \ -f $SCRIPT_DIR/Dockerfile \ $SCRIPT_DIR/.. } function run() { docker run -it --rm \ --network host \ --privileged \ -v /dev:/dev \ -v $SCRIPT_DIR/../:/ws/src \ -e DISPLAY=$DISPLAY \ -v /tmp/.X11-unix:/tmp/.X11-unix \ rio \ /bin/bash } function help() { echo "Usage: $0 [build|run]" exit 0 } while [[ $# -gt 0 ]]; do case $1 in -b|--build) build shift ;; -r|--run) run shift ;; -h|--help) help shift ;; *) echo "Unknown option: $1" help exit 1 ;; esac done ================================================ FILE: docker/run.py ================================================ #!/bin/env python3 import argparse import shutil import sys import os BASE_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) def ArgsParser(argv): parser = argparse.ArgumentParser(description='Test RIO on a given dataset') parser.add_argument('-a', '--auto', action='store_true', help='Auto mode') parser.add_argument('-n', '--node', type=str, default='rio', help='Node name') parser.add_argument('-c', '--conf', type=str, default='config/ars548.yaml', help='Config file') parser.add_argument('-d', '--dataset', type=str, default='dataset/exp/Sequence_1.bag', help='Path to the dataset') parser.add_argument('-r', '--round', type=int, default=1, help='Number of rounds') parser.add_argument('-p', '--playSpeed', type=float, default=0.25, help='Play speed') return parser.parse_args(argv) def RunEstimator(dataset, node, conf, round, playSpeed): dataset = os.path.join(BASE_DIR, dataset) CMD = 'bash -c "source /opt/ros/`rosversion -d`/setup.bash && source {}/../devel/setup.bash && roslaunch rio TestRadar.launch round:={} nodeName:={} configFile:={} playBagpath:={} playSpeed:={}"'.format(BASE_DIR, round, node, conf, dataset, playSpeed) os.system(CMD) def runSingleRound(dataset, node, conf, round, playSpeed): RunEstimator(dataset, node, conf, round, playSpeed) def main(argv): args = ArgsParser(argv) # Modify the ground truth topic if args.auto: if 'blocking' in args.dataset or 'square' in args.dataset: args.groundTruth = '/pos_vel_mocap/odom_TA' elif 'colo' in args.dataset: args.groundTruth = '/gt_pose' # Start testing for round in range(args.round): runSingleRound(args.dataset, args.node, args.conf, round, args.playSpeed) if __name__ == '__main__': main(sys.argv[1:]) ================================================ FILE: rio/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.0.2) project(rio) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) add_compile_options(-std=c++17 -O3) find_package(catkin REQUIRED roscpp rospy std_msgs sensor_msgs nav_msgs) find_package(OpenCV REQUIRED) find_package(PCL REQUIRED) find_package(Ceres REQUIRED) find_package(Backward) catkin_package() ########### ## Build ## ########### FILE(GLOB_RECURSE RIO_SOURCE_FILES "src/*.cpp") add_executable(rio node/rio.cpp node/rosWarper.cpp ${RIO_SOURCE_FILES} ) target_include_directories(rio PRIVATE ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} node src/data_manager src/factor src/frame_type src/frontend src/residual src/state src/utility src/manifold ) target_link_libraries(rio ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${CERES_LIBRARIES} ${PCL_LIBRARIES} -lbfd -ldw ) ================================================ FILE: rio/config/RIO.rviz ================================================ Panels: - Class: rviz/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Odometry1 - /PointCloud21 - /Pose2 Splitter Ratio: 0.5 Tree Height: 621 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties Expanded: - /2D Pose Estimate1 - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - Class: rviz/Time Name: Time SyncMode: 0 SyncSource: PointCloud2 Preferences: PromptSaveOnExit: true Toolbars: toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: Alpha: 0.5 Color: 255; 255; 127 Color Style: Unique Frame: Local Offset: 1 Scale: 1 Value: true Position: Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true Value: true Enabled: true Keep: 1 Name: Odometry Position Tolerance: 0.10000000149011612 Queue Size: 10 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Color: 255; 25; 0 Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Value: Axes Topic: /pos_vel_mocap/odom_TA Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 0; 0 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /tracking_frame Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: true Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /sub_map_frame Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes Topic: /estimated_pose Unreliable: false Value: true - Alpha: 1 Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 Name: Path Offset: X: 0 Y: 0 Z: 0 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 Topic: /estimated_path Unreliable: false Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz/PointCloud2 Color: 85; 0; 255 Color Transformer: FlatColor Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 Name: PointCloud2 Position Transformer: XYZ Queue Size: 10 Selectable: false Size (Pixels): 3 Size (m): 0.10000000149011612 Style: Flat Squares Topic: /tracking_map Unreliable: true Use Fixed Frame: true Use rainbow: true Value: true - Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 25; 0 Enabled: true Head Length: 0.30000001192092896 Head Radius: 0.10000000149011612 Name: Pose Queue Size: 10 Shaft Length: 1 Shaft Radius: 0.05000000074505806 Shape: Axes Topic: /gt_pose Unreliable: false Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true Fixed Frame: world Frame Rate: 30 Name: root Tools: - Class: rviz/Interact Hide Inactive Objects: true - Class: rviz/MoveCamera - Class: rviz/Select - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose Theta std deviation: 0.2617993950843811 Topic: /initialpose X std deviation: 0.5 Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint Single click: true Topic: /clicked_point Value: true Views: Current: Class: rviz/Orbit Distance: 36.37040710449219 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.7853981852531433 Focal Point: X: 0.6377802491188049 Y: -1.420575737953186 Z: 1.2562583684921265 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.4297963380813599 Target Frame: Yaw: 2.233621835708618 Saved: ~ Window Geometry: Displays: collapsed: false Height: 912 Hide Left Dock: false Hide Right Dock: false QMainWindow State: 000000ff00000000fd000000040000000000000156000002f6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002f6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002f6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005f60000003efc0100000002fb0000000800540069006d00650100000000000005f60000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000385000002f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1526 X: 117 Y: 70 ================================================ FILE: rio/config/ars548.yaml ================================================ # Algorithm configuration useWeightedResiduals: true useDopplerResidual: true usePoint2PointResidual: true radarType: 0 # 0: ARS548, 1: ColoRadar # Radar sensor configuration SigmaRange: 0.05 SigmaAzimuth: 0.00290888 SigmaElevation: 0.00058178 # Point matching configuration numSigma: 3 # Fusion configuration observationThreshold: 4 maxVelInfoGain: 2000 maxPointInfoGain: 1500 # Without weighted residuals dopplerResidualWeight: 1500 pointResidualWeight: 1000 # ROS configuration radarSubTopic: /radar_raw_message imuSubTopic: /mavros/imu/data nav_gtSubTopic: /pos_vel_mocap/odom_TA posePubTopic: /estimated_pose pathPubTopic: /estimated_path framePubTopic: /radar_frame trackingPubTopic: /tracking_frame subMapPubTopic: /sub_map_frame ================================================ FILE: rio/config/coloradar.yaml ================================================ # Algorithm configuration useWeightedResiduals: true useDopplerResidual: true usePoint2PointResidual: true radarType: 1 # 0: ARS548, 1: ColoRadar # Radar sensor configuration SigmaRange: 0.105 SigmaAzimuth: 0.00890888 SigmaElevation: 0.0088178 # Point matching configuration numSigma: 3 # Fusion configuration observationThreshold: 4 maxVelInfoGain: 1500 maxPointInfoGain: 400 # Without weighted residuals dopplerResidualWeight: 1500 pointResidualWeight: 400 # ROS configuration radarSubTopic: /mmWaveDataHdl/RScan imuSubTopic: /gx5/imu/data nav_gtSubTopic: /lidar_ground_truth geometry_gtSubTopic: /motion_capture_ground_truth posePubTopic: /estimated_pose pathPubTopic: /estimated_path gtPubTopic: /gt_pose framePubTopic: /radar_frame trackingPubTopic: /tracking_frame subMapPubTopic: /sub_map_frame ================================================ FILE: rio/launch/TestRadar.launch ================================================ ================================================ FILE: rio/node/rio.cpp ================================================ #include "rosWarper.hpp" #ifdef DEBUG #define BACKWARD_HAS_BFD 1 #define BACKWARD_HAS_DW 1 #include backward::SignalHandling sh; #endif int main(int argc, char **argv) { ros::init(argc, argv, "rio"); ros::NodeHandle nodeHandle("~"); std::shared_ptr rio = std::make_shared(nodeHandle); ros::spin(); return 0; } ================================================ FILE: rio/node/rosWarper.cpp ================================================ #include "rosWarper.hpp" RIO::RIO(ros::NodeHandle &nh) { initROS(nh); initRIO(); } RIO::~RIO() {} void RIO::initROS(ros::NodeHandle &nh) { nodeHandle = nh; getParam(); } void RIO::getParam() { ros::param::get("~useWeightedResiduals", useWeightedResiduals); ros::param::get("~useDopplerResidual", useDopplerResidual); ros::param::get("~usePoint2PointResidual", usePoint2PointResidual); ros::param::get("~SigmaRange", SigmaRange); ros::param::get("~SigmaAzimuth", SigmaAzimuth); ros::param::get("~SigmaElevation", SigmaElevation); ros::param::get("~SigmaDoppler", SigmaDoppler); ros::param::get("~numSigma", numSigma); ros::param::get("~maxVelInfoGain", maxVelInfoGain); ros::param::get("~maxPointInfoGain", maxPointInfoGain); ros::param::get("~dopplerResidualWeight", dopplerResidualWeight); ros::param::get("~pointResidualWeight", pointResidualWeight); ros::param::get("~radarSubTopic", radarSubTopic); ros::param::get("~imuSubTopic", imuSubTopic); ros::param::get("~nav_gtSubTopic", nav_gtSubTopic); ros::param::get("~geometry_gtSubTopic", geometry_gtSubTopic); ros::param::get("~posePubTopic", posePubTopic); ros::param::get("~pathPubTopic", pathPubTopic); ros::param::get("~gtPubTopic", gtPubTopic); ros::param::get("~framePubTopic", framePubTopic); ros::param::get("~trackingPubTopic", trackingPubTopic); ros::param::get("~subMapPubTopic", subMapPubTopic); int radarTypeInt = 0; ros::param::get("~radarType", radarTypeInt); radarType = static_cast(radarTypeInt); // Print parameters std::cout << "useWeightedResiduals: " << useWeightedResiduals << std::endl << "useRCSFilter: " << useRCSFilter << std::endl << "observationThreshold: " << observationThreshold << std::endl << "useDopplerResidual: " << useDopplerResidual << std::endl << "usePoint2PointResidual: " << usePoint2PointResidual << std::endl << "SigmaRange: " << SigmaRange << std::endl << "SigmaAzimuth: " << SigmaAzimuth << std::endl << "SigmaElevation: " << SigmaElevation << std::endl << "SigmaDoppler: " << SigmaDoppler << std::endl << "numSigma: " << numSigma << std::endl << "maxVelInfoGain: " << maxVelInfoGain << std::endl << "maxPointInfoGain: " << maxPointInfoGain << std::endl << "dopplerResidualWeight: " << dopplerResidualWeight << std::endl << "pointResidualWeight: " << pointResidualWeight << std::endl << "radarSubTopic: " << radarSubTopic << std::endl << "imuSubTopic: " << imuSubTopic << std::endl << "nav_gtSubTopic: " << nav_gtSubTopic << std::endl << "geometry_gtSubTopic: " << geometry_gtSubTopic << std::endl << "posePubTopic: " << posePubTopic << std::endl << "pathPubTopic: " << pathPubTopic << std::endl << "gtPubTopic: " << gtPubTopic << std::endl << "framePubTopic: " << framePubTopic << std::endl << "trackingPubTopic: " << trackingPubTopic << std::endl << "subMapPubTopic: " << subMapPubTopic << std::endl << "radarType: " << radarType << std::endl; } std::vector RIO::decodeRadarMsg_ARS548( const sensor_msgs::PointCloud2 &msg) { std::vector result; int pointBytes = static_cast(msg.point_step); int offsetAzimuth = 0; int offsetAzimuthSTD = 0; int offsetElevation = 0; int offsetElevationSTD = 0; int offsetRange = 0; int offsetRangeSTD = 0; int offsetVelocity = 0; int offsetVelocitySTD = 0; int offsetRCS = 0; const auto &fields = msg.fields; for (auto i = 0; i < fields.size(); i++) { if (fields[i].name == "azimuth") offsetAzimuth = static_cast(fields[i].offset); else if (fields[i].name == "azimuthSTD") offsetAzimuthSTD = static_cast(fields[i].offset); else if (fields[i].name == "elevation") offsetElevation = static_cast(fields[i].offset); else if (fields[i].name == "elevationSTD") offsetElevationSTD = static_cast(fields[i].offset); else if (fields[i].name == "range") offsetRange = static_cast(fields[i].offset); else if (fields[i].name == "rangeSTD") offsetRangeSTD = static_cast(fields[i].offset); else if (fields[i].name == "velocity") offsetVelocity = static_cast(fields[i].offset); else if (fields[i].name == "velocitySTD") offsetVelocitySTD = static_cast(fields[i].offset); else if (fields[i].name == "rcs") offsetRCS = static_cast(fields[i].offset); } pcl::PointCloud pointCloud; for (auto i = 0; i < msg.row_step; i++) { float azimuth = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetAzimuth)); float azimuthSTD = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetAzimuthSTD)); float elevation = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetElevation)); float elevationSTD = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetElevationSTD)); float range = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetRange)); float rangeSTD = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetRangeSTD)); float velocity = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetVelocity)); float velocitySTD = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetVelocitySTD)); int8_t rcs = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetRCS)); Frame::RadarData currentData{}; currentData.azimuth = azimuth; currentData.elevation = elevation; currentData.range = range; currentData.doppler = velocity; currentData.rcs = rcs; Frame::RadarData::anglesToXYZ(currentData); Frame::RadarData::rcsToIntensity(currentData); result.emplace_back(currentData); pcl::PointXYZI point; point.x = range * cos(azimuth) * cos(elevation); point.y = range * sin(azimuth) * cos(elevation); point.z = range * sin(elevation); point.intensity = rcs; pointCloud.emplace_back(point); } // pointCloud to world frame // pcl::PointCloud pointCloudWorld; // pcl::transformPointCloud(pointCloud, pointCloudWorld, gtTransform); // std::string path = "/ws/src/dataset/exp/square_fast/"; // std::string filename = path + std::to_string(msg.header.stamp.toSec()) + // ".pcd"; pcl::io::savePCDFileASCII(filename, pointCloudWorld); sensor_msgs::PointCloud2 pubMsg; pcl::toROSMsg(pointCloud, pubMsg); pubMsg.header.frame_id = "world"; framePub.publish(pubMsg); return result; } std::vector RIO::decodeRadarMsg_ColoRadar( const sensor_msgs::PointCloud2 &msg) { std::vector result; int pointBytes = static_cast(msg.point_step); int offsetX = 0; int offsetY = 0; int offsetZ = 0; int offsetIntensity = 0; int offsetRange = 0; int offsetDoppler = 0; // auto receivedTime = msg.header.stamp; const auto &fields = msg.fields; for (auto i = 0; i < fields.size(); i++) { if (fields[i].name == "x") offsetX = static_cast(fields[i].offset); else if (fields[i].name == "y") offsetY = static_cast(fields[i].offset); else if (fields[i].name == "z") offsetZ = static_cast(fields[i].offset); else if (fields[i].name == "intensity") offsetIntensity = static_cast(fields[i].offset); else if (fields[i].name == "range") offsetRange = static_cast(fields[i].offset); else if (fields[i].name == "doppler") offsetDoppler = static_cast(fields[i].offset); } pcl::PointCloud pointCloud; for (auto i = 0; i < msg.width; i++) { float xValue = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetX)); float yValue = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetY)); float zValue = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetZ)); float range = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetRange)); float velocity = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetDoppler)); float intensity = *reinterpret_cast( msg.data.data() + static_cast(pointBytes * i + offsetIntensity)); Frame::RadarData currentData{}; currentData.x = xValue; currentData.y = yValue; currentData.z = zValue; currentData.range = range; currentData.doppler = velocity; currentData.rcs = intensity; // Frame::RadarData::xyzToAngles(currentData); // Frame::RadarData::intensityToRCS(currentData); result.emplace_back(currentData); pcl::PointXYZI point; point.x = xValue; point.y = yValue; point.z = zValue; point.intensity = intensity; pointCloud.emplace_back(point); } sensor_msgs::PointCloud2 pubMsg; pcl::toROSMsg(pointCloud, pubMsg); pubMsg.header.frame_id = "world"; framePub.publish(pubMsg); return result; } void RIO::factorGraphInit(std::vector &frameRadarData, const ros::Time &timeStamp) { for (auto pts : frameRadarData) { Eigen::Vector3d p{pts.x, pts.y, pts.z}; p = radarExParam.rot * p + radarExParam.vec; pcl::PointXYZI point; point.x = p.x(); point.y = p.y(); point.z = p.z(); point.intensity = pts.rcs; worldMap->emplace_back(point); } states.basicState[0].positionParams[0] = predState.vec.x(); states.basicState[0].positionParams[1] = predState.vec.y(); states.basicState[0].positionParams[2] = predState.vec.z(); states.basicState[0].rotationParams[0] = predState.rot.x(); states.basicState[0].rotationParams[1] = predState.rot.y(); states.basicState[0].rotationParams[2] = predState.rot.z(); states.basicState[0].rotationParams[3] = predState.rot.w(); states.imuState[0].velocityParams[0] = predState.vel.x(); states.imuState[0].velocityParams[1] = predState.vel.y(); states.imuState[0].velocityParams[2] = predState.vel.z(); states.imuState[0].biasAccParams[0] = predState.accBias.x(); states.imuState[0].biasAccParams[1] = predState.accBias.y(); states.imuState[0].biasAccParams[2] = predState.accBias.z(); states.imuState[0].biasGyroParams[0] = predState.gyroBias.x(); states.imuState[0].biasGyroParams[1] = predState.gyroBias.y(); states.imuState[0].biasGyroParams[2] = predState.gyroBias.z(); states.basicStateNum = 1; states.imuStateNum = 1; radarData.data.emplace_back( scan2scanTracker.trackPoints(frameRadarData, timeStamp)); radarData.data.back().gyroData = imuData.data.back().gyroData; radarFeatureFactor.clear(); radarFeatureFactor.pushBack(radarData.data.back()); } Residuals::Preintegration RIO::imuPreintegration(ros::Time start, ros::Time end) { std::vector dataBuffer = imuData.getDataWithinInterval(start, end); if (dataBuffer.empty()) return Residuals::Preintegration{Eigen::Vector3d{0, 0, 0}, Eigen::Vector3d{0, 0, 0}, predState.accBias, predState.gyroBias, 0.1, 0.01, 0.001, 0.0001}; Residuals::Preintegration result{dataBuffer.front().accData, dataBuffer.front().gyroData, predState.accBias, predState.gyroBias, 0.1, 0.01, 0.001, 0.0001}; for (auto data = dataBuffer.begin(); data != dataBuffer.end(); data++) { if (data == dataBuffer.begin()) { result.propagate((data->receivedTime - start).toSec(), data->accData, data->gyroData); } else if (data == (dataBuffer.end() - 1)) { result.propagate((data->receivedTime - (data - 1)->receivedTime).toSec(), data->accData, data->gyroData); result.propagate((end - data->receivedTime).toSec(), data->accData, data->gyroData); } else { result.propagate((data->receivedTime - (data - 1)->receivedTime).toSec(), data->accData, data->gyroData); } } return result; } void RIO::constructFactor(std::vector &frameRadarData, const ros::Time &timeStamp) { Residuals::Preintegration preintegration = imuPreintegration(radarData.data.back().receivedTime, timeStamp); // initial guess curState = predState; // predict predState.vec = curState.rot * preintegration.getDeltaP() + curState.vel * preintegration.getTotalTime() + curState.vec - 0.5 * gravity * preintegration.getTotalTime() * preintegration.getTotalTime(); predState.rot = curState.rot * preintegration.getDeltaQ(); predState.vel = curState.rot * preintegration.getDeltaV() + curState.vel - gravity * preintegration.getTotalTime(); Eigen::Vector3d unbiasedAngularVel = imuData.data.back().gyroData - predState.gyroBias; Eigen::Vector3d tangentVel = MathUtility::skewSymmetric(unbiasedAngularVel) * -radarExParam.vec; Eigen::Vector3d radialVel = predState.rot.inverse() * predState.vel; Eigen::Vector3d velInRadar = radarExParam.rot.inverse() * (radialVel + tangentVel); Eigen::Quaterniond curRotRadar = curState.rot * radarExParam.rot; Eigen::Vector3d curVecRadar = curState.rot * radarExParam.vec + curState.vec; Eigen::Quaterniond predRotRadar = predState.rot * radarExParam.rot; Eigen::Vector3d predVecRadar = predState.rot * radarExParam.vec + predState.vec; Frame::RadarFrame frame; scan2scanTracker.setPrediction(curRotRadar, curVecRadar, predRotRadar, predVecRadar, -velInRadar); frame = scan2scanTracker.trackPoints(frameRadarData, timeStamp); radarData.data.emplace_back(frame); radarData.data.back().gyroData = imuData.data.back().gyroData; radarFeatureFactor.pushBack(radarData.data.back()); imuFeatureFactor.pushBack(preintegration, states.imuStateNum - 1, states.imuStateNum); } void RIO::optimizer() { ceres::Problem problem; ceres::Solver::Options option; ceres::Solver::Summary summary; option.linear_solver_type = ceres::DENSE_SCHUR; option.max_solver_time_in_seconds = 0.2; option.max_num_iterations = 20; option.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; #ifdef DEBUG option.minimizer_progress_to_stdout = true; option.logging_type = ceres::PER_MINIMIZER_ITERATION; #endif initStates(); constructProblem(problem); ceres::Solve(option, &problem, &summary); // std::cout << summary.FullReport() << std::endl; recoverState(ceres::CONVERGENCE); } void RIO::initStates() { states.basicState[states.basicStateNum].positionParams[0] = predState.vec.x(); states.basicState[states.basicStateNum].positionParams[1] = predState.vec.y(); states.basicState[states.basicStateNum].positionParams[2] = predState.vec.z(); states.basicState[states.basicStateNum].rotationParams[0] = predState.rot.x(); states.basicState[states.basicStateNum].rotationParams[1] = predState.rot.y(); states.basicState[states.basicStateNum].rotationParams[2] = predState.rot.z(); states.basicState[states.basicStateNum].rotationParams[3] = predState.rot.w(); states.imuState[states.imuStateNum].velocityParams[0] = predState.vel.x(); states.imuState[states.imuStateNum].velocityParams[1] = predState.vel.y(); states.imuState[states.imuStateNum].velocityParams[2] = predState.vel.z(); states.imuState[states.imuStateNum].biasAccParams[0] = predState.accBias.x(); states.imuState[states.imuStateNum].biasAccParams[1] = predState.accBias.y(); states.imuState[states.imuStateNum].biasAccParams[2] = predState.accBias.z(); states.imuState[states.imuStateNum].biasGyroParams[0] = predState.gyroBias.x(); states.imuState[states.imuStateNum].biasGyroParams[1] = predState.gyroBias.y(); states.imuState[states.imuStateNum].biasGyroParams[2] = predState.gyroBias.z(); states.basicStateNum++; states.imuStateNum++; states.exRadarToImu.positionParams[0] = radarExParam.vec.x(); states.exRadarToImu.positionParams[1] = radarExParam.vec.y(); states.exRadarToImu.positionParams[2] = radarExParam.vec.z(); states.exRadarToImu.rotationParams[0] = radarExParam.rot.x(); states.exRadarToImu.rotationParams[1] = radarExParam.rot.y(); states.exRadarToImu.rotationParams[2] = radarExParam.rot.z(); states.exRadarToImu.rotationParams[3] = radarExParam.rot.w(); } void RIO::recoverState(ceres::TerminationType type) { if (type != ceres::CONVERGENCE) { ROS_ERROR("Optimization failed"); return; } predState.vec.x() = states.basicState[states.basicStateNum - 1].positionParams[0]; predState.vec.y() = states.basicState[states.basicStateNum - 1].positionParams[1]; predState.vec.z() = states.basicState[states.basicStateNum - 1].positionParams[2]; predState.rot.x() = states.basicState[states.basicStateNum - 1].rotationParams[0]; predState.rot.y() = states.basicState[states.basicStateNum - 1].rotationParams[1]; predState.rot.z() = states.basicState[states.basicStateNum - 1].rotationParams[2]; predState.rot.w() = states.basicState[states.basicStateNum - 1].rotationParams[3]; predState.vel.x() = states.imuState[states.basicStateNum - 1].velocityParams[0]; predState.vel.y() = states.imuState[states.basicStateNum - 1].velocityParams[1]; predState.vel.z() = states.imuState[states.basicStateNum - 1].velocityParams[2]; predState.accBias.x() = states.imuState[states.basicStateNum - 1].biasAccParams[0]; predState.accBias.y() = states.imuState[states.basicStateNum - 1].biasAccParams[1]; predState.accBias.z() = states.imuState[states.basicStateNum - 1].biasAccParams[2]; predState.gyroBias.x() = states.imuState[states.basicStateNum - 1].biasGyroParams[0]; predState.gyroBias.y() = states.imuState[states.basicStateNum - 1].biasGyroParams[1]; predState.gyroBias.z() = states.imuState[states.basicStateNum - 1].biasGyroParams[2]; radarExParam.vec.x() = states.exRadarToImu.positionParams[0]; radarExParam.vec.y() = states.exRadarToImu.positionParams[1]; radarExParam.vec.z() = states.exRadarToImu.positionParams[2]; radarExParam.rot.x() = states.exRadarToImu.rotationParams[0]; radarExParam.rot.y() = states.exRadarToImu.rotationParams[1]; radarExParam.rot.z() = states.exRadarToImu.rotationParams[2]; radarExParam.rot.w() = states.exRadarToImu.rotationParams[3]; if (states.basicStateNum > State::MAX_WINDOWS_SIZE) { for (int i = 0; i < states.basicStateNum - 1; i++) { states.basicState[i].positionParams[0] = states.basicState[i + 1].positionParams[0]; states.basicState[i].positionParams[1] = states.basicState[i + 1].positionParams[1]; states.basicState[i].positionParams[2] = states.basicState[i + 1].positionParams[2]; states.basicState[i].rotationParams[0] = states.basicState[i + 1].rotationParams[0]; states.basicState[i].rotationParams[1] = states.basicState[i + 1].rotationParams[1]; states.basicState[i].rotationParams[2] = states.basicState[i + 1].rotationParams[2]; states.basicState[i].rotationParams[3] = states.basicState[i + 1].rotationParams[3]; states.imuState[i].velocityParams[0] = states.imuState[i + 1].velocityParams[0]; states.imuState[i].velocityParams[1] = states.imuState[i + 1].velocityParams[1]; states.imuState[i].velocityParams[2] = states.imuState[i + 1].velocityParams[2]; states.imuState[i].biasAccParams[0] = states.imuState[i + 1].biasAccParams[0]; states.imuState[i].biasAccParams[1] = states.imuState[i + 1].biasAccParams[1]; states.imuState[i].biasAccParams[2] = states.imuState[i + 1].biasAccParams[2]; states.imuState[i].biasGyroParams[0] = states.imuState[i + 1].biasGyroParams[0]; states.imuState[i].biasGyroParams[1] = states.imuState[i + 1].biasGyroParams[1]; states.imuState[i].biasGyroParams[2] = states.imuState[i + 1].biasGyroParams[2]; } states.basicStateNum--; states.imuStateNum--; radarFeatureFactor.popFront(); imuFeatureFactor.popFront(); } } void RIO::constructProblem(ceres::Problem &problem) { addOptimizationVariables(problem); constructImuResiduals(problem); if (useDopplerResidual) { constructDopplerResiduals(problem); } if (usePoint2PointResidual) { constructPoint2PointResiduals(problem); } } void RIO::addOptimizationVariables(ceres::Problem &problem) { // extrinsic problem.AddParameterBlock(states.exRadarToImu.positionParams, 3); problem.AddParameterBlock(states.exRadarToImu.rotationParams, 4); problem.SetManifold(states.exRadarToImu.rotationParams, // NOLINTNEXTLINE : ceres will take ownership of this new Manifold::EigenQuaternionManifold()); problem.SetParameterBlockConstant(states.exRadarToImu.positionParams); problem.SetParameterBlockConstant(states.exRadarToImu.rotationParams); // state for (int offset = 0; offset < states.basicStateNum; offset++) { problem.AddParameterBlock(states.basicState[offset].positionParams, 3); problem.AddParameterBlock(states.basicState[offset].rotationParams, 4); problem.SetManifold(states.basicState[offset].rotationParams, new Manifold::EigenQuaternionManifold()); problem.AddParameterBlock(states.imuState[offset].velocityParams, 3); problem.AddParameterBlock(states.imuState[offset].biasAccParams, 3); problem.AddParameterBlock(states.imuState[offset].biasGyroParams, 3); } problem.SetParameterBlockConstant(states.basicState[0].positionParams); problem.SetParameterBlockConstant(states.basicState[0].rotationParams); problem.SetParameterBlockConstant(states.imuState[0].velocityParams); problem.SetParameterBlockConstant(states.imuState[0].biasAccParams); problem.SetParameterBlockConstant(states.imuState[0].biasGyroParams); } void RIO::constructImuResiduals(ceres::Problem &problem) { for (int offset = 0; offset < states.basicStateNum - 1; offset++) { auto preintegrationTemp = imuFeatureFactor.imuPreintegration[offset]; auto *imuCost = new Residuals::IMUResidual(preintegrationTemp, gravity); problem.AddResidualBlock(imuCost, nullptr, states.basicState[offset].positionParams, states.basicState[offset].rotationParams, states.imuState[offset].velocityParams, states.imuState[offset].biasAccParams, states.imuState[offset].biasGyroParams, states.basicState[offset + 1].positionParams, states.basicState[offset + 1].rotationParams, states.imuState[offset + 1].velocityParams, states.imuState[offset + 1].biasAccParams, states.imuState[offset + 1].biasGyroParams); } } void RIO::constructDopplerResiduals(ceres::Problem &problem) { auto beginFrame = radarData.data.end() - 1 - states.basicStateNum; int staticPointSize = 0; for (int offset = 0; offset < states.basicStateNum; offset++) staticPointSize += (beginFrame + offset)->staticPoint.size(); double *staticPointState = new double[3 * staticPointSize]; int staticPointCount = 0; for (int offset = 0; offset < states.basicStateNum; offset++) { for (auto point : (beginFrame + offset)->staticPoint) { Eigen::Vector3d angularVel = (beginFrame + offset)->gyroData; Eigen::Vector3d targetPos{point.x, point.y, point.z}; staticPointState[3 * staticPointCount] = targetPos.x(); staticPointState[3 * staticPointCount + 1] = targetPos.y(); staticPointState[3 * staticPointCount + 2] = targetPos.z(); problem.AddParameterBlock(staticPointState + 3 * staticPointCount, 3); problem.SetParameterBlockConstant(staticPointState + 3 * staticPointCount); Residuals::RadarImuVelocityResidual *costFn; if (useWeightedResiduals) { costFn = new Residuals::RadarImuVelocityResidual( angularVel, -point.doppler, point.azimuth, point.elevation, SigmaAzimuth, SigmaElevation, maxVelInfoGain); } else { costFn = new Residuals::RadarImuVelocityResidual( angularVel, -point.doppler, dopplerResidualWeight); } problem.AddResidualBlock(costFn, nullptr, states.basicState[offset].rotationParams, states.imuState[offset].velocityParams, states.imuState[offset].biasGyroParams, states.exRadarToImu.positionParams, states.exRadarToImu.rotationParams, staticPointState + 3 * staticPointCount); staticPointCount++; } } } void RIO::constructPoint2PointResiduals(ceres::Problem &problem) { states.pointNum = 0; for (auto &point : radarFeatureFactor.pointRelation) { if (point.frameId.size() < observationThreshold) continue; std::vector stateIndex(point.frameId.size()); for (int i = 0; i < point.frameId.size(); i++) { for (int j = 0; j < radarFeatureFactor.frameRelation.size(); j++) if (radarFeatureFactor.frameRelation[j].radarFrameId == point.frameId[i]) stateIndex[i] = j; } if (!point.init) { point.init = true; Eigen::Vector3d initGuess{point.measurement[0].x, point.measurement[0].y, point.measurement[0].z}; Eigen::Vector3d translation{states.basicState[0].positionParams[0], states.basicState[0].positionParams[1], states.basicState[0].positionParams[2]}; Eigen::Quaterniond rotation{states.basicState[0].rotationParams[3], states.basicState[0].rotationParams[0], states.basicState[0].rotationParams[1], states.basicState[0].rotationParams[2]}; initGuess = rotation * (radarExParam.rot * initGuess + radarExParam.vec) + translation; point.pointState = initGuess; } states.pointState[states.pointNum].positionParams[0] = point.pointState.x(); states.pointState[states.pointNum].positionParams[1] = point.pointState.y(); states.pointState[states.pointNum].positionParams[2] = point.pointState.z(); problem.AddParameterBlock(states.pointState[states.pointNum].positionParams, 3); for (int i = 0; i < point.frameId.size(); i++) { auto data = point.measurement[i]; int observationCount = point.frameId.size(); int pointNum = radarFeatureFactor.pointRelation.size(); auto state = stateIndex[i]; Eigen::Vector3d measurement{data.x, data.y, data.z}; Residuals::RadarPointResidual *costFn; if (useWeightedResiduals) { costFn = new Residuals::RadarPointResidual( measurement, data.range, data.azimuth, data.elevation, SigmaRange, SigmaAzimuth, SigmaElevation, maxPointInfoGain, observationCount); } else { costFn = new Residuals::RadarPointResidual(measurement, pointResidualWeight); } problem.AddResidualBlock( costFn, nullptr, states.basicState[state].positionParams, states.basicState[state].rotationParams, states.exRadarToImu.positionParams, states.exRadarToImu.rotationParams, states.pointState[states.pointNum].positionParams); } states.pointNum++; } } void RIO::publish(const ros::Time &timeStamp) { pcl::PointCloud::Ptr featurePoints( new pcl::PointCloud); int pointsNum = states.pointNum; if (pointsNum > 0) { for (auto &point : radarFeatureFactor.pointRelation) { if (point.frameId.size() < observationThreshold) continue; point.pointState.x() = states.pointState[pointsNum - states.pointNum].positionParams[0]; point.pointState.y() = states.pointState[pointsNum - states.pointNum].positionParams[1]; point.pointState.z() = states.pointState[pointsNum - states.pointNum].positionParams[2]; pcl::PointXYZI mapPt; mapPt.x = point.pointState.x(); mapPt.y = point.pointState.y(); mapPt.z = point.pointState.z(); mapPt.intensity = point.measurement.front().rcs; worldMap->emplace_back(mapPt); pcl::PointXYZ pt; pt.x = point.pointState.x(); pt.y = point.pointState.y(); pt.z = point.pointState.z(); featurePoints->emplace_back(pt); states.pointNum--; } } sensor_msgs::PointCloud2 featureMsg; pcl::toROSMsg(*featurePoints, featureMsg); featureMsg.header.frame_id = "world"; trackingPub.publish(featureMsg); geometry_msgs::PoseStamped pubMsg; pubMsg.header.frame_id = "world"; pubMsg.header.stamp = timeStamp; pubMsg.pose.orientation.w = predState.rot.w(); pubMsg.pose.orientation.x = predState.rot.x(); pubMsg.pose.orientation.y = predState.rot.y(); pubMsg.pose.orientation.z = predState.rot.z(); pubMsg.pose.position.x = predState.vec.x(); pubMsg.pose.position.y = predState.vec.y(); pubMsg.pose.position.z = predState.vec.z(); posePub.publish(pubMsg); // visualize path path.poses.emplace_back(pubMsg); path.header.frame_id = "world"; pathPub.publish(path); // visualize map sensor_msgs::PointCloud2 mapMsg; pcl::toROSMsg(*worldMap, mapMsg); mapMsg.header.frame_id = "world"; subMapPub.publish(mapMsg); } void RIO::radarCallback(const sensor_msgs::PointCloud2 &msg) { if (imuData.size() <= 0) { ROS_ERROR("No imu data"); return; } // Preprocess radar data std::vector frameRadarData; if (radarType == ARS548) frameRadarData = decodeRadarMsg_ARS548(msg); else if (radarType == ColoRadar) frameRadarData = decodeRadarMsg_ColoRadar(msg); else ROS_ERROR("Unknown radar type"); radarPreprocessor.process(frameRadarData); // Initialize factor graph if ((radarData.size() <= 3 || !init)) { factorGraphInit(frameRadarData, msg.header.stamp); return; } // Construct factor constructFactor(frameRadarData, msg.header.stamp); // Optimize optimizer(); // Publish publish(msg.header.stamp); } static int initCounter = 0; void RIO::imuCallback(const sensor_msgs::Imu &msg) { auto frame = Frame::IMUFrame(); Eigen::Vector3d acc; Eigen::Vector3d gyro; if (radarType == ARS548) { acc = Eigen::Vector3d{msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z}; gyro = Eigen::Vector3d{msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z}; } else if (radarType == ColoRadar) { acc = Eigen::Vector3d{msg.linear_acceleration.x, -msg.linear_acceleration.y, -msg.linear_acceleration.z}; gyro = Eigen::Vector3d{msg.angular_velocity.x, -msg.angular_velocity.y, -msg.angular_velocity.z}; } else { ROS_ERROR("Unknown radar type"); } frame.receivedTime = msg.header.stamp; frame.accData = acc; frame.gyroData = gyro; imuData.push(frame); if (!init) { initCounter++; gravity += acc; if (initCounter >= 100) { gravity /= initCounter; curState.vec.setZero(); curState.rot.setIdentity(); curState.vel.setZero(); curState.accBias.setZero(); curState.gyroBias.setZero(); predState.vec.setZero(); predState.rot.setIdentity(); predState.vel.setZero(); predState.accBias.setZero(); predState.gyroBias.setZero(); if (radarType == ARS548) { radarExParam.vec.setZero(); radarExParam.vec.z() = -0.05; radarExParam.rot.setIdentity(); } else if (radarType == ColoRadar) { Eigen::Quaterniond quatImuToBase(0.00020953429822, 0.707105451466, 0.707108048814, 0.000209535067884); Eigen::Vector3d vecImuToBase(0, 0, 0); Eigen::Vector3d vecRadarToBase{-0.145, 0.09, -0.025}; Eigen::Quaterniond quatRadarToBase{0.707388269167, 0.0, 0.0, 0.706825181105}; radarExParam.vec = Eigen::Vector3d{-0.09, -0.145, 0.025}; radarExParam.rot.setIdentity(); } else { ROS_ERROR("Unknown radar type"); } init = true; std::cout << "g:\n" << gravity << std::endl; } } } static Eigen::Quaterniond z90rot(Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ())); static Eigen::Vector3d firstVec; static Eigen::Quaterniond firstRot; static bool firstFrame = true; void RIO::geometry_gtCallback(const geometry_msgs::PoseStamped &msg) { if (firstFrame) { firstVec = Eigen::Vector3d{msg.pose.position.x, msg.pose.position.y, msg.pose.position.z}; firstRot = Eigen::Quaterniond{msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z}; firstFrame = false; } auto vec = Eigen::Vector3d{msg.pose.position.x, msg.pose.position.y, msg.pose.position.z}; auto rot = Eigen::Quaterniond{msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z}; vec = firstRot.inverse() * (vec - firstVec); rot = firstRot.inverse() * rot; geometry_msgs::PoseStamped pubMsg; pubMsg.header.frame_id = "world"; pubMsg.header.stamp = msg.header.stamp; pubMsg.pose.orientation.w = rot.w(); pubMsg.pose.orientation.x = rot.x(); pubMsg.pose.orientation.y = rot.y(); pubMsg.pose.orientation.z = rot.z(); pubMsg.pose.position.x = vec.x(); pubMsg.pose.position.y = vec.y(); pubMsg.pose.position.z = vec.z(); gtPub.publish(pubMsg); } void RIO::nav_gtCallback(const nav_msgs::Odometry &msg) { if (firstFrame) { firstVec = Eigen::Vector3d{msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z}; firstRot = Eigen::Quaterniond{ msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z}; firstFrame = false; } auto vec = Eigen::Vector3d{msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z}; auto rot = Eigen::Quaterniond{ msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z}; vec = firstRot.inverse() * (vec - firstVec); rot = firstRot.inverse() * rot * z90rot; geometry_msgs::PoseStamped pubMsg; pubMsg.header.frame_id = "world"; pubMsg.header.stamp = msg.header.stamp; pubMsg.pose.orientation.w = rot.w(); pubMsg.pose.orientation.x = rot.x(); pubMsg.pose.orientation.y = rot.y(); pubMsg.pose.orientation.z = rot.z(); pubMsg.pose.position.x = vec.x(); pubMsg.pose.position.y = vec.y(); pubMsg.pose.position.z = vec.z(); gtPub.publish(pubMsg); } void RIO::initRIO() { radarSub = nodeHandle.subscribe(radarSubTopic, SUB_QUEUE_SIZE, &RIO::radarCallback, this); imuSub = nodeHandle.subscribe(imuSubTopic, SUB_QUEUE_SIZE, &RIO::imuCallback, this); nav_gtSub = nodeHandle.subscribe(nav_gtSubTopic, SUB_QUEUE_SIZE, &RIO::nav_gtCallback, this); geometry_gtSub = nodeHandle.subscribe(geometry_gtSubTopic, SUB_QUEUE_SIZE, &RIO::geometry_gtCallback, this); posePub = nodeHandle.advertise(posePubTopic, PUB_QUEUE_SIZE); pathPub = nodeHandle.advertise(pathPubTopic, PUB_QUEUE_SIZE); gtPub = nodeHandle.advertise(gtPubTopic, PUB_QUEUE_SIZE); framePub = nodeHandle.advertise(framePubTopic, PUB_QUEUE_SIZE); trackingPub = nodeHandle.advertise(trackingPubTopic, PUB_QUEUE_SIZE); subMapPub = nodeHandle.advertise(subMapPubTopic, PUB_QUEUE_SIZE); // radarPreprocessor if (radarType == ARS548) { radarPreprocessor.addFOVParams(Frontend::RadarPreprocessor::FOVParams{ -14 * DEG_TO_RAD, 14 * DEG_TO_RAD, -50 * DEG_TO_RAD, +50 * DEG_TO_RAD, 0.2, 30}); radarPreprocessor.setVelParams( Frontend::RadarPreprocessor::VelParams{-100, 50}); radarPreprocessor.setDistanceParams(1.5); } else if (radarType == ColoRadar) { radarPreprocessor.addFOVParams(Frontend::RadarPreprocessor::FOVParams{ -14 * DEG_TO_RAD, 14 * DEG_TO_RAD, -28 * DEG_TO_RAD, 28 * DEG_TO_RAD, 1.5, 15}); radarPreprocessor.setVelParams( Frontend::RadarPreprocessor::VelParams{-30, 30}); radarPreprocessor.setDistanceParams(0.5); } // radarTracker scan2scanTracker.setMatchingThreshold(1, 0.3); scan2scanTracker.setMatchingParameters( SigmaRange, SigmaAzimuth, SigmaElevation, numSigma, useRCSFilter); scan2scanTracker.setPredictedVelocityThreshold(0.3); } ================================================ FILE: rio/node/rosWarper.hpp ================================================ #pragma once // Std #include #include // ROS #include "geometry_msgs/PoseStamped.h" #include "nav_msgs/Odometry.h" #include "nav_msgs/Path.h" #include "ros/ros.h" #include "sensor_msgs/Imu.h" #include "sensor_msgs/PointCloud2.h" // Data Manager #include "IMUManager.hpp" #include "RadarManager.hpp" #include "StateManager.hpp" // Frontend #include "Preintegration.hpp" #include "RadarPreprocessor.hpp" #include "RadarTracker.hpp" // Factor #include "ImuPreintegrationManager.hpp" #include "RadarFeatureManager.hpp" // Residual #include "IMUResidual.hpp" #include "RadarImuVelResidual.hpp" #include "RadarPointResidual.hpp" // Utility #include "MathUtility.hpp" // PCL Library #include #include #include #include // Cerse Library #include #include #include #include // Eigen Library #include "EigenQuaternionManifold.hpp" // Const Param constexpr int SUB_QUEUE_SIZE = 1000; constexpr int PUB_QUEUE_SIZE = 100; constexpr float DEG_TO_RAD = 3.1415926 / 180; // Enum enum RadarType { ARS548, ColoRadar }; class RIO { private: // Map pcl::PointCloud::Ptr trackingMap = pcl::PointCloud::Ptr(new pcl::PointCloud); pcl::PointCloud::Ptr worldMap = pcl::PointCloud::Ptr(new pcl::PointCloud); nav_msgs::Path path; // Param RadarType radarType; bool useWeightedResiduals = true; bool useRCSFilter = true; bool useDopplerResidual = false; bool usePoint2PointResidual = false; int observationThreshold = 4; double SigmaRange; double SigmaAzimuth; double SigmaElevation; double SigmaDoppler; double numSigma; double maxVelInfoGain; double maxPointInfoGain; std::string radarSubTopic; std::string imuSubTopic; std::string nav_gtSubTopic; std::string geometry_gtSubTopic; std::string posePubTopic; std::string pathPubTopic; std::string gtPubTopic; std::string framePubTopic; std::string trackingPubTopic; std::string subMapPubTopic; double dopplerResidualWeight; double pointResidualWeight; // ROS ros::NodeHandle nodeHandle; ros::Subscriber radarSub; ros::Subscriber imuSub; ros::Subscriber nav_gtSub; ros::Subscriber geometry_gtSub; ros::Publisher posePub; ros::Publisher pathPub; ros::Publisher gtPub; ros::Publisher framePub; ros::Publisher trackingPub; ros::Publisher subMapPub; // Data Data::IMUManager imuData; Data::RadarManager radarData; // Frontend Frontend::RadarPreprocessor radarPreprocessor; Frontend::RadarTracker scan2scanTracker; // State bool init = false; Eigen::Vector3d gravity; State::StateManager states; struct runtimeState { Eigen::Vector3d vec; Eigen::Quaterniond rot; Eigen::Vector3d vel; Eigen::Vector3d accBias; Eigen::Vector3d gyroBias; }; runtimeState curState; runtimeState predState; struct exParam { Eigen::Vector3d vec; Eigen::Quaterniond rot; }; exParam radarExParam; // Factor Factor::RadarFeatureManager radarFeatureFactor; Factor::ImuPreintegrationManger imuFeatureFactor; // BaseFunc void getParam(); void initROS(ros::NodeHandle &nh); void initRIO(); void publish(const ros::Time &timeStamp); // DataProcess std::vector decodeRadarMsg_ARS548( const sensor_msgs::PointCloud2 &msg); std::vector decodeRadarMsg_ColoRadar( const sensor_msgs::PointCloud2 &msg); // Estimator Residuals::Preintegration imuPreintegration(ros::Time start, ros::Time end); void factorGraphInit(std::vector &frameRadarData, const ros::Time &timeStamp); void constructFactor(std::vector &frameRadarData, const ros::Time &timeStamp); void optimizer(); void initStates(); void recoverState(ceres::TerminationType type); void constructProblem(ceres::Problem &problem); void addOptimizationVariables(ceres::Problem &problem); void constructImuResiduals(ceres::Problem &problem); void constructDopplerResiduals(ceres::Problem &problem); void constructPoint2PointResiduals(ceres::Problem &problem); // Callback void radarCallback(const sensor_msgs::PointCloud2 &msg); void imuCallback(const sensor_msgs::Imu &msg); void nav_gtCallback(const nav_msgs::Odometry &msg); void geometry_gtCallback(const geometry_msgs::PoseStamped &msg); public: RIO(ros::NodeHandle &nh); ~RIO(); RIO(RIO &another) = delete; RIO(RIO &&another) = delete; }; ================================================ FILE: rio/package.xml ================================================ rio 0.0.0 The rio package root TODO catkin roscpp rospy std_msgs sensor_msgs nav_msgs roscpp rospy std_msgs sensor_msgs nav_msgs roscpp rospy std_msgs sensor_msgs nav_msgs ================================================ FILE: rio/src/data_manager/IMUManager.cpp ================================================ #include "IMUManager.hpp" #include #include namespace Data { void IMUManager::push(Frame::IMUFrame frame) { data.emplace_back(frame); } void IMUManager::popFront() { if (size() != 0) data.pop_front(); } void IMUManager::popBack() { if (size() != 0) data.pop_back(); } ros::Time IMUManager::getFirstTimestamp() const { return (size() == 0) ? ros::Time{0, 0} : data.front().receivedTime; } ros::Time IMUManager::getLastTimestamp() const { return (size() == 0) ? ros::Time{0, 0} : data.back().receivedTime; } size_t IMUManager::size() const { return data.size(); } std::vector IMUManager::getDataWithinInterval( ros::Time startTime, ros::Time endTime) { std::vector result; for (auto &candidate : data) if (startTime <= candidate.receivedTime && candidate.receivedTime <= endTime) result.emplace_back(candidate); return result; } }; // namespace Data ================================================ FILE: rio/src/data_manager/IMUManager.hpp ================================================ #pragma once #include #include #include "../frame_type/IMUFrame.hpp" #include "ros/time.h" namespace Data { class IMUManager { public: IMUManager() = default; ~IMUManager() = default; IMUManager(IMUManager &another) = delete; IMUManager(IMUManager &&another) = delete; IMUManager &operator=(const IMUManager &another) = delete; IMUManager &operator=(IMUManager &&another) = delete; void push(Frame::IMUFrame frame); void popFront(); void popBack(); ros::Time getFirstTimestamp() const; ros::Time getLastTimestamp() const; size_t size() const; std::vector getDataWithinInterval(ros::Time startTime, ros::Time endTime); std::deque data; }; } // namespace Data ================================================ FILE: rio/src/data_manager/RadarManager.cpp ================================================ #include "RadarManager.hpp" namespace Data { void RadarManager::push(Frame::RadarFrame frame) { data.emplace_back(frame); } void RadarManager::popFront() { if (size() != 0) data.pop_front(); } void RadarManager::popBack() { if (size() != 0) data.pop_back(); } ros::Time RadarManager::getFirstTimestamp() const { return (size() == 0) ? ros::Time{0, 0} : data.front().receivedTime; } ros::Time RadarManager::getLastTimestamp() const { return (size() == 0) ? ros::Time{0, 0} : data.back().receivedTime; } size_t RadarManager::size() const { return data.size(); } std::vector RadarManager::getDataWithinInterval( ros::Time startTime, ros::Time endTime) { std::vector result; for (auto &candidate : data) if (startTime <= candidate.receivedTime && candidate.receivedTime <= endTime) result.emplace_back(candidate); return result; } } // namespace Data ================================================ FILE: rio/src/data_manager/RadarManager.hpp ================================================ #pragma once #include #include #include "../frame_type/RadarFrame.hpp" #include "ros/time.h" namespace Data { class RadarManager { public: RadarManager() = default; ~RadarManager() = default; RadarManager(RadarManager &another) = delete; RadarManager(RadarManager &&another) = delete; RadarManager &operator=(const RadarManager &another) = delete; RadarManager &operator=(RadarManager &&another) = delete; void push(Frame::RadarFrame frame); void popFront(); void popBack(); ros::Time getFirstTimestamp() const; ros::Time getLastTimestamp() const; size_t size() const; std::vector getDataWithinInterval(ros::Time startTime, ros::Time endTime); std::deque data; }; } // namespace Data ================================================ FILE: rio/src/data_manager/readme.md ================================================ ## DataManager ### Workflow All managers are wrappers, we keep this module just for record all resource. ## IMUManager ### TODO - [ ] move unnecessary function and parameters to private domain - [ ] re-organize const expression - [ ] test - [ ] finish comments - [ ] documentation ### Notices ## ImageManager ### TODO - [ ] move unnecessary function and parameters to private domain - [ ] re-organize const expression - [ ] optimize - [ ] test - [ ] finish comments - [ ] documentation ### Notices ================================================ FILE: rio/src/factor/ImuPreintegrationManager.cpp ================================================ #include "ImuPreintegrationManager.hpp" namespace Factor { void ImuPreintegrationManger::pushBack(Residuals::Preintegration data, int startIndex, int endIndex) { imuPreintegration.emplace_back(data); relations.emplace_back(Relation{startIndex, endIndex}); } void ImuPreintegrationManger::popBack() { imuPreintegration.pop_back(); relations.pop_back(); } void ImuPreintegrationManger::popFront() { imuPreintegration.pop_front(); relations.pop_front(); } void ImuPreintegrationManger::shiftStateIndex(int number) { for (auto &data : relations) { data.startStateIndex -= number; data.endStateIndex -= number; } } } // namespace Factor ================================================ FILE: rio/src/factor/ImuPreintegrationManager.hpp ================================================ #pragma once #include #include #include #include #include #include "Preintegration.hpp" namespace Factor { class ImuPreintegrationManger { public: struct Relation { int startStateIndex; int endStateIndex; }; std::deque imuPreintegration; std::deque relations; void pushBack(Residuals::Preintegration data, int startIndex, int endIndex); void popBack(); void popFront(); void shiftStateIndex(int number); }; } // namespace Factor ================================================ FILE: rio/src/factor/RadarFeatureManager.cpp ================================================ #include "RadarFeatureManager.hpp" #include namespace Factor { void RadarFeatureManager::pushBack(Frame::RadarFrame &frame) { int index = 0; RadarFrameRelation radarRelation; for (auto data : frame.matchedPoint) { radarRelation.featureId.emplace_back(data.first); while ((index < pointRelation.size()) && pointRelation[index].featureId < data.first) index++; if (index >= pointRelation.size()) { PointFeatureRelation featureRelation; featureRelation.featureId = data.first; featureRelation.frameId.emplace_back(frame.id); featureRelation.measurement.emplace_back(data.second); featureRelation.init = false; featureRelation.gyroData.emplace_back(frame.gyroData); pointRelation.emplace_back(featureRelation); continue; } pointRelation[index].frameId.emplace_back(frame.id); pointRelation[index].measurement.emplace_back(data.second); pointRelation[index].gyroData.emplace_back(frame.gyroData); } for (auto data : frame.unmatchedPoint) { radarRelation.featureId.emplace_back(data.first); while ((index < pointRelation.size()) && pointRelation[index].featureId < data.first) index++; if (index >= pointRelation.size()) { PointFeatureRelation featureRelation; featureRelation.featureId = data.first; featureRelation.frameId.emplace_back(frame.id); featureRelation.measurement.emplace_back(data.second); featureRelation.init = false; featureRelation.gyroData.emplace_back(frame.gyroData); pointRelation.emplace_back(featureRelation); continue; } pointRelation[index].frameId.emplace_back(frame.id); pointRelation[index].measurement.emplace_back(data.second); pointRelation[index].gyroData.emplace_back(frame.gyroData); } // push frame radarRelation.radarFrameId = frame.id; frameRelation.emplace_back(radarRelation); } void RadarFeatureManager::popFront() { int index = 0; // delete point relation for (int i = 0; i < frameRelation.front().featureId.size(); i++) { while ( (index < pointRelation.size()) && (pointRelation[index].featureId < frameRelation.front().featureId[i])) index++; if (index >= pointRelation.size()) break; // delete relation if (pointRelation[index].featureId == frameRelation.front().featureId[i]) { pointRelation[index].frameId.pop_front(); pointRelation[index].measurement.pop_front(); pointRelation[index].gyroData.pop_front(); } // delete point if (pointRelation[index].frameId.empty()) pointRelation.erase(pointRelation.begin() + index); } // delete frame frameRelation.pop_front(); } void RadarFeatureManager::popBack() { int index = 0; // delete point relation for (int i = 0; i < frameRelation.back().featureId.size(); i++) { while ((index < pointRelation.size()) && (pointRelation[index].featureId < frameRelation.back().featureId[i])) index++; if (index >= pointRelation.size()) break; // delete relation if (pointRelation[index].featureId == frameRelation.back().featureId[i]) { pointRelation[index].frameId.pop_back(); pointRelation[index].measurement.pop_back(); pointRelation[index].gyroData.pop_back(); } // delete point if (pointRelation[index].frameId.empty()) pointRelation.erase(pointRelation.begin() + index); } // delete frame frameRelation.pop_back(); } void RadarFeatureManager::clear() { frameRelation.clear(); pointRelation.clear(); } size_t RadarFeatureManager::frameSize() const { return frameRelation.size(); } size_t RadarFeatureManager::activeFeatureSize() const { size_t result = 0; for (auto point : pointRelation) if (point.frameId.size() > 1) result++; return result; } size_t RadarFeatureManager::featureSize() const { return pointRelation.size(); } } // namespace Factor ================================================ FILE: rio/src/factor/RadarFeatureManager.hpp ================================================ #pragma once #include #include #include #include "RadarFrame.hpp" namespace Factor { // it presents a graph class RadarFeatureManager { public: struct RadarFrameRelation; struct PointFeatureRelation; struct RadarFrameRelation { uint32_t radarFrameId; std::deque featureId; }; struct PointFeatureRelation { uint32_t featureId; std::deque frameId; std::deque gyroData; Eigen::Vector3d pointState; bool init; std::deque measurement; }; std::deque frameRelation; std::deque pointRelation; void pushBack(Frame::RadarFrame &frame); void popFront(); void popBack(); void mergeBack(Eigen::Quaterniond previousRot, Eigen::Vector3d previousVec, Eigen::Quaterniond currentRot, Eigen::Vector3d currentVec); void clear(); size_t frameSize() const; size_t activeFeatureSize() const; size_t featureSize() const; }; } // namespace Factor ================================================ FILE: rio/src/frame_type/IMUFrame.hpp ================================================ #pragma once #include "eigen3/Eigen/Eigen" #include "ros/time.h" namespace Frame { class IMUFrame { public: IMUFrame() = default; ~IMUFrame() = default; IMUFrame(IMUFrame &another) = default; IMUFrame(IMUFrame &&another) = default; IMUFrame &operator=(const IMUFrame &another) = default; IMUFrame &operator=(IMUFrame &&another) = default; Eigen::Vector3d accData; Eigen::Vector3d gyroData; uint32_t id = 0; ros::Time receivedTime{0, 0}; // double receivedTime = 0; }; } // namespace Frame ================================================ FILE: rio/src/frame_type/RadarFrame.hpp ================================================ #pragma once #include #include #include #include #include "eigen3/Eigen/Eigen" #include "ros/time.h" namespace Frame { struct RadarData { double azimuth; double elevation; double range; double intensity; double doppler; float x; float y; float z; double rcs; int featureID = -1; static inline void anglesToXYZ(RadarData &data) { data.x = data.range * cos(data.azimuth) * cos(data.elevation); data.y = data.range * sin(data.azimuth) * cos(data.elevation); data.z = data.range * sin(data.elevation); }; static inline void xyzToAngles(RadarData &data) { data.range = sqrt(data.x * data.x + data.y * data.y + data.z * data.z); data.elevation = asin(data.z / data.range); data.azimuth = asin(data.y / data.range / cos(data.elevation)); } static inline void intensityToRCS(RadarData &data) { data.rcs = data.intensity * 16 * 3.1415926 * 3.1415926 * data.range * data.range * data.range * data.range; }; static inline void rcsToIntensity(RadarData &data) { data.intensity = data.rcs / 16 / 3.1415926 / 3.1415926 / data.range / data.range / data.range / data.range; } }; class RadarFrame { public: RadarFrame() = default; ~RadarFrame() = default; RadarFrame(RadarFrame &another) = default; RadarFrame(RadarFrame &&another) = default; RadarFrame &operator=(const RadarFrame &another) = default; RadarFrame &operator=(RadarFrame &&another) = default; uint32_t id = 0; std::vector data; std::map matchedPoint; std::map unmatchedPoint; std::vector staticPoint; Eigen::Vector3d gyroData{}; ros::Time receivedTime{0, 0}; }; class RCSFrame { public: RCSFrame() = default; ~RCSFrame() = default; RCSFrame(RCSFrame &another) = default; RCSFrame(RCSFrame &&another) = default; RCSFrame &operator=(const RCSFrame &another) = default; RCSFrame &operator=(RCSFrame &&another) = default; uint32_t id = 0; ros::Time receivedTime{0, 0}; std::vector frameID{}; }; } // namespace Frame ================================================ FILE: rio/src/frontend/RadarPreprocessor.cpp ================================================ #include "RadarPreprocessor.hpp" #include #include #include #include "RadarFrame.hpp" #include "Utility.hpp" namespace Frontend { void RadarPreprocessor::process(std::vector &frame) const { if (enFovFilter) fovFilter(frame); if (enVelFilter) velocityFilter(frame); if (enDistFilter) distanceFilter(frame); if (enSpatFilter) spatialFilter(frame); }; void RadarPreprocessor::addFOVParams(FOVParams param) { fovParams.emplace_back(param); enFovFilter = true; }; void RadarPreprocessor::setVelParams(VelParams param) { velParams = param; enVelFilter = true; }; void RadarPreprocessor::setDistanceParams(double distance) { maxDistance = distance; enDistFilter = true; } void RadarPreprocessor::setSpatialParams(SpatialFilterParams param) { spatialParams = param; enSpatFilter = true; } void RadarPreprocessor::fovFilter(std::vector &frame) const { if (frame.empty()) return; std::vector status(frame.size(), 0); for (auto param : fovParams) { for (int index = 0; index < frame.size(); index++) { auto data = frame.at(index); if ((data.azimuth < param.maxAzimuth) && (data.azimuth > param.minAzimuth) && (data.elevation < param.maxElevation) && (data.elevation > param.minElevation) && (data.range < param.maxDistance) && (data.range > param.minDistance)) status.at(index) = 1; } } reduceVector(frame, status); } void RadarPreprocessor::velocityFilter( std::vector &frame) const { if (frame.empty()) return; std::vector status; for (auto data : frame) if ((data.doppler < velParams.maxVelocity) && (data.doppler > velParams.minVelocity)) status.emplace_back(1); else status.emplace_back(0); reduceVector(frame, status); } void RadarPreprocessor::distanceFilter( std::vector &frame) const { if (frame.size() <= 1) return; // TODO use KD-tree std::vector status(frame.size(), 0); for (int i = 0; i < frame.size() - 1; i++) for (int j = i + 1; j < frame.size(); j++) { double deltaX = frame.at(i).x - frame.at(j).x; double deltaY = frame.at(i).y - frame.at(j).y; double deltaZ = frame.at(i).z - frame.at(j).z; double dis = sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ); if (dis < maxDistance) { status.at(i) = 1; status.at(j) = 1; } } reduceVector(frame, status); } void RadarPreprocessor::spatialFilter( std::vector &frame) const { // TODO use KD-tree if (frame.size() <= 1) return; std::vector> distanceArray; for (int i = 0; i < frame.size(); i++) { distanceArray.emplace_back(std::vector()); for (int j = 0; j < frame.size(); j++) { double deltaX = frame.at(i).x - frame.at(j).x; double deltaY = frame.at(i).y - frame.at(j).y; double deltaZ = frame.at(i).z - frame.at(j).z; double dis = sqrt(deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ); if (dis < spatialParams.radius) distanceArray.back().emplace_back(dis); } } std::vector status; for (auto &disVec : distanceArray) { if (disVec.size() > spatialParams.number) status.emplace_back(1); else status.emplace_back(0); } reduceVector(frame, status); reduceVector(distanceArray, status); if (frame.empty()) return; status.clear(); for (auto &disVec : distanceArray) { double mean = 0; double std = 0; for (auto dis : disVec) mean += dis; mean /= (disVec.size() - 1); // NOLINT : unsigned long to double for (auto dis : disVec) std += (dis - mean) * (dis - mean); std = std::sqrt(std / (disVec.size() - 1)); // NOLINT : unsigned long to double if (std < spatialParams.sigma) status.emplace_back(1); else status.emplace_back(0); } reduceVector(frame, status); } } // namespace Frontend ================================================ FILE: rio/src/frontend/RadarPreprocessor.hpp ================================================ #pragma once #include #include #include "RadarFrame.hpp" namespace Frontend { class RadarPreprocessor { public: struct FOVParams { float minElevation; float maxElevation; float minAzimuth; float maxAzimuth; float minDistance; float maxDistance; }; struct VelParams { float minVelocity; float maxVelocity; }; struct SpatialFilterParams { int number; float radius; float sigma; }; RadarPreprocessor() = default; ~RadarPreprocessor() = default; RadarPreprocessor(RadarPreprocessor &another) = delete; RadarPreprocessor(RadarPreprocessor &&another) = delete; RadarPreprocessor &operator=(const RadarPreprocessor &another) = delete; RadarPreprocessor &operator=(RadarPreprocessor &&another) = delete; void process(std::vector &frame) const; void addFOVParams(FOVParams param); void setVelParams(VelParams param); void setDistanceParams(double distance); void setSpatialParams(SpatialFilterParams param); private: void fovFilter(std::vector &frame) const; void velocityFilter(std::vector &frame) const; void distanceFilter(std::vector &frame) const; void spatialFilter(std::vector &frame) const; std::vector fovParams{}; VelParams velParams{}; double maxDistance = 0; SpatialFilterParams spatialParams{}; bool enFovFilter = false; bool enVelFilter = false; bool enDistFilter = false; bool enSpatFilter = false; }; } // namespace Frontend ================================================ FILE: rio/src/frontend/RadarTracker.cpp ================================================ #include "RadarTracker.hpp" #include #include #include #include "RadarFrame.hpp" #include "Utility.hpp" #include "opencv2/core/hal/interface.h" #include "ros/time.h" static inline double pdf(double x, double mu, double sigma) { return exp(-0.5 * pow((x - mu) / sigma, 2)) / (sigma * sqrt(2 * M_PI)); } namespace Frontend { void RadarTracker::setMatchingThreshold(double maxRCSError, double maxDistanceError) { rcsThreshold = maxRCSError; distanceThreshold = maxDistanceError; } void RadarTracker::setPredictedVelocityThreshold(double maxVelocityError) { velThreshold = maxVelocityError; } void RadarTracker::setMatchingParameters(double sigmaR, double sigmaTheta, double sigmaPhi, double numSigma, bool useRCSFilter) { useRCSFilter = useRCSFilter; SigmaR = sigmaR; SigmaTheta = sigmaTheta; SigmaPhi = sigmaPhi; if (numSigma < 10e-6) { pdfThreshold = 0; } else { pdfThreshold = pdf(numSigma * sigmaR, 0, SigmaR) * pdf(numSigma * sigmaTheta, 0, SigmaTheta) * pdf(numSigma * sigmaPhi, 0, SigmaPhi); } } Frame::RadarFrame RadarTracker::trackPoints( std::vector points, ros::Time time) { previousFrame = currentFrame; if (!init) { init = true; initialization(points, time); return currentFrame; } // detected tracked point and new corresponding currentFrame.data = points; frameId++; currentFrame.id = frameId; currentFrame.receivedTime = time; currentFrame.matchedPoint.clear(); currentFrame.unmatchedPoint.clear(); currentFrame.staticPoint.clear(); trackAndDetectPoints(points); // previousFrame = currentFrame; return currentFrame; } void RadarTracker::setPrediction(Eigen::Quaterniond previousRot, Eigen::Vector3d previousVec, Eigen::Quaterniond currentRot, Eigen::Vector3d currentVec, Eigen::Vector3d velInRadar) { hasPrediction = true; prevRot = previousRot; prevVec = previousVec; currRot = currentRot; currVec = currentVec; currVelInRadar = velInRadar; } void RadarTracker::removeTrackingPoints( const std::set &idToRemove) {} Frame::RadarFrame RadarTracker::mergeFrame(Eigen::Quaterniond previousRot, Eigen::Vector3d previousVec, Eigen::Quaterniond currentRot, Eigen::Vector3d currentVec) { // transform unmatched point back for (auto pts : currentFrame.unmatchedPoint) { auto transformed = Eigen::Vector3d{pts.second.x, pts.second.y, pts.second.z}; transformed = previousRot.inverse() * ((currentRot * transformed + currentVec) - previousVec); pts.second.x = transformed.x(); pts.second.y = transformed.y(); pts.second.z = transformed.z(); Frame::RadarData::xyzToAngles(pts.second); previousTrackingPoints.emplace_back( TrackingPointsInfo{pts.first, 1, pts.second}); previousFrame.unmatchedPoint[pts.first] = pts.second; } trackingPoints = previousTrackingPoints; currentFrame = previousFrame; return currentFrame; } void RadarTracker::initialization(std::vector &points, ros::Time time) { currentFrame.data = points; frameId++; currentFrame.id = frameId; currentFrame.matchedPoint.clear(); currentFrame.unmatchedPoint.clear(); currentFrame.staticPoint.clear(); currentFrame.receivedTime = time; for (auto iter : points) { featureId++; trackingPoints.emplace_back(TrackingPointsInfo{featureId, 1, iter}); currentFrame.unmatchedPoint[featureId] = iter; } } std::vector RadarTracker::searchCorrsponding( std::vector &previousPoints, std::vector ¤tPoints, std::vector &detectedPoints, std::vector &undetectedPoints) { // tracking points std::vector result; detectedPoints.clear(); undetectedPoints.clear(); std::vector undetectedStatus(currentPoints.size(), 1); std::vector> matchScore(currentPoints.size(), {-1, 0}); for (int j = 0; j < previousPoints.size(); j++) { auto previousData = previousPoints[j]; double minDistance = 1000; double maxPDF = 0; int minIndex = 0; Frame::RadarData minData{}; // pdfThreshold = 0; if (pdfThreshold < 10e-6) { // Distance Based for (int i = 0; i < currentPoints.size(); i++) { auto currentData = currentPoints[i]; // Radar Cross Section difference if (!useRCSFilter || abs(previousData.rcs - currentData.rcs) <= rcsThreshold) { auto pointA = Eigen::Vector3d{previousData.x, previousData.y, previousData.z}; auto pointB = Eigen::Vector3d{currentData.x, currentData.y, currentData.z}; auto distance = (pointA - pointB).norm(); if (distance < minDistance) { minDistance = distance; minData = currentData; minIndex = i; } } } // Distance Threshold if (minDistance < distanceThreshold) { result.emplace_back(1); undetectedStatus.at(minIndex) = 0; detectedPoints.emplace_back(minData); } else result.emplace_back(0); } else { // Convariance Based for (int i = 0; i < currentPoints.size(); i++) { auto currentData = currentPoints[i]; Frame::RadarData::xyzToAngles(currentData); if (!useRCSFilter || abs(previousData.rcs - currentData.rcs) <= rcsThreshold) { double PDF = calculatePDF(currentData, previousData); if (PDF > maxPDF) { maxPDF = PDF; minData = currentData; minIndex = i; } } } // PDF Threshold if (maxPDF > pdfThreshold) { result.emplace_back(1); undetectedStatus.at(minIndex) = 0; detectedPoints.emplace_back(minData); } else result.emplace_back(0); } } for (int i = 0; i < matchScore.size(); i++) { if (matchScore[i].first != -1) { detectedPoints.emplace_back(currentPoints[i]); Eigen::Vector3d PointA{previousPoints[matchScore[i].first].x, previousPoints[matchScore[i].first].y, previousPoints[matchScore[i].first].z}; Eigen::Vector3d PointB{currentPoints[i].x, currentPoints[i].y, currentPoints[i].z}; double distance = (PointA - PointB).norm(); // std::cout << "Distance: " << distance << std::endl; } } for (int i = 0; i < undetectedStatus.size(); i++) if (undetectedStatus.at(i) == 1) undetectedPoints.emplace_back(currentPoints[i]); return result; } std::vector RadarTracker::searchStaticPoints( std::vector ¤tPoints, Eigen::Vector3d velocity) { std::vector result; for (auto data : currentPoints) { // sqrtInfoGain not used double sqrtInfoGain; Eigen::Matrix Sigma; Eigen::Matrix JThetaPhi; double Theta = data.azimuth; double Phi = data.elevation; JThetaPhi << -sin(Theta) * cos(Phi), -cos(Theta) * sin(Phi), cos(Theta) * cos(Phi), -sin(Theta) * sin(Phi), 0, cos(Phi); Sigma << SigmaTheta * SigmaTheta, 0, 0, SigmaPhi * SigmaPhi; sqrtInfoGain = sqrt(1.0 / (velocity.transpose() * JThetaPhi * Sigma * JThetaPhi.transpose() * velocity)); Eigen::Vector3d dir{data.x, data.y, data.z}; dir.normalize(); double velDrift = abs(data.doppler - dir.dot(velocity)); if (velDrift < velThreshold) { result.emplace_back(1); } else { result.emplace_back(0); } } return result; } void RadarTracker::trackAndDetectPoints(std::vector &points) { std::vector detectedPoints; std::vector undetectedPoints; std::vector previousPoints; std::vector status; if (!hasPrediction) { // tracking with previous for (auto pts : trackingPoints) previousPoints.emplace_back(pts.data); status = searchCorrsponding(previousPoints, points, detectedPoints, undetectedPoints); } else { // track with prediction // since it is more accurate than position measurement. // velocity prediction, we process it separately status = searchStaticPoints(points, currVelInRadar); currentFrame.staticPoint = points; reduceVector(currentFrame.staticPoint, status); for (auto pts : trackingPoints) { // calculate the prediction points Eigen::Vector3d trackingPts{pts.data.x, pts.data.y, pts.data.z}; trackingPts = currRot.inverse() * ((prevRot * trackingPts + prevVec) - currVec); pts.data.x = trackingPts.x(); pts.data.y = trackingPts.y(); pts.data.z = trackingPts.z(); Frame::RadarData::xyzToAngles(pts.data); previousPoints.emplace_back(pts.data); } status = searchCorrsponding(previousPoints, currentFrame.staticPoint, detectedPoints, undetectedPoints); #ifdef DEBUG std::cout << "currentFrame.staticPoint.size(): " << currentFrame.staticPoint.size() << std::endl; std::cout << "detectedPoints.size(): " << detectedPoints.size() << std::endl; std::cout << "undetectedPoints.size(): " << undetectedPoints.size() << std::endl; #endif // prediction check failed, fallback int successCnt = 0; for (auto i = 0; i < status.size(); i++) if (status[i] != 0) successCnt++; previousPoints.clear(); if (successCnt <= 3) { for (auto pts : trackingPoints) { // calculate the prediction points Eigen::Vector3d trackingPts{pts.data.x, pts.data.y, pts.data.z}; trackingPts = currRot.inverse() * ((prevRot * trackingPts + prevVec) - currVec); pts.data.x = trackingPts.x(); pts.data.y = trackingPts.y(); pts.data.z = trackingPts.z(); Frame::RadarData::xyzToAngles(pts.data); previousPoints.emplace_back(pts.data); } status = searchCorrsponding(previousPoints, points, detectedPoints, undetectedPoints); } } previousTrackingPoints = trackingPoints; reduceVector(trackingPoints, status); // save result for (auto i = 0; i < detectedPoints.size(); i++) { trackingPoints[i].count++; trackingPoints[i].data = detectedPoints[i]; currentFrame.matchedPoint[trackingPoints[i].id] = detectedPoints[i]; } // prepare for next tracking for (auto pts : undetectedPoints) { featureId++; trackingPoints.emplace_back(TrackingPointsInfo{featureId, 1, pts}); currentFrame.unmatchedPoint[featureId] = pts; } // std::cout << detectedPoints.size() << "\t" << undetectedPoints.size() << // std::endl; hasPrediction = false; } double RadarTracker::calculatePDF(Frame::RadarData &previousPoint, Frame::RadarData ¤tPoint) { double pdfR = pdf(previousPoint.range, currentPoint.range, SigmaR); double pdfTheta = pdf(previousPoint.azimuth, currentPoint.azimuth, SigmaTheta); double pdfPhi = pdf(previousPoint.elevation, currentPoint.elevation, SigmaPhi); return pdfR * pdfTheta * pdfPhi; } } // namespace Frontend ================================================ FILE: rio/src/frontend/RadarTracker.hpp ================================================ #pragma once #include #include #include #include #include "RadarFrame.hpp" #include "RadarPreprocessor.hpp" namespace Frontend { class RadarTracker { struct TrackingPointsInfo { uint32_t id; uint32_t count; Frame::RadarData data; }; public: RadarTracker() = default; ~RadarTracker() = default; RadarTracker(RadarTracker &another) = delete; RadarTracker(RadarTracker &&another) = delete; RadarTracker &operator=(const RadarTracker &another) = delete; RadarTracker &operator=(RadarTracker &&another) = delete; void setMatchingThreshold(double maxRCSError, double maxDistanceError); void setMatchingParameters(double sigmaR, double sigmaTheta, double sigmaPhi, double numSigma, bool useRCSFilter); void setPredictedVelocityThreshold(double maxVelocityError); Frame::RadarFrame trackPoints(std::vector points, ros::Time time); void setPrediction(Eigen::Quaterniond previousRot, Eigen::Vector3d previousVec, Eigen::Quaterniond currentRot, Eigen::Vector3d currentVec, Eigen::Vector3d velInRadar); void removeTrackingPoints(const std::set &idToRemove); Frame::RadarFrame mergeFrame(Eigen::Quaterniond previousRot, Eigen::Vector3d previousVec, Eigen::Quaterniond currentRot, Eigen::Vector3d currentVec); private: void initialization(std::vector &points, ros::Time time); std::vector searchCorrsponding( std::vector &previousPoints, std::vector ¤tPoints, std::vector &detectedPoints, std::vector &undetectedPoints); std::vector searchStaticPoints( std::vector ¤tPoints, Eigen::Vector3d velocity); void trackAndDetectPoints(std::vector &points); double calculatePDF(Frame::RadarData &previousPoint, Frame::RadarData ¤tPoint); Frame::RadarFrame previousFrame; Frame::RadarFrame currentFrame; uint32_t featureId = 0; uint32_t frameId = 0; std::vector trackingPoints; std::vector previousTrackingPoints; bool hasPrediction = false; bool useRCSFilter = false; double rcsThreshold = 0; double distanceThreshold = 0; double velThreshold = 0; double SigmaR = 0; double SigmaTheta = 0; double SigmaPhi = 0; double pdfThreshold = 0; bool init = false; Eigen::Quaterniond prevRot{}; Eigen::Vector3d prevVec{}; Eigen::Quaterniond currRot{}; Eigen::Vector3d currVec{}; Eigen::Vector3d currVelInRadar{}; }; } // namespace Frontend ================================================ FILE: rio/src/frontend/readme.md ================================================ ## Visual Tracker ### Workflow ``` if (init): featureDetection(); undistortion(); else: trackPrediction(); if not success : trackPreviousFeatures(); saveTrackingResult(); featureDetection(); ``` ### TODO - [ ] degeneration detection - [X] save and get configuration file - [X] move unnecessary function and parameters to private domain - [X] re-organize const expression - [X] test - [ ] is flow-back checking necessary? - [X] finish comments - [X] documentation ### Notices ================================================ FILE: rio/src/manifold/EigenQuaternionManifold.cpp ================================================ #include "EigenQuaternionManifold.hpp" #include "MathUtility.hpp" namespace Manifold { int EigenQuaternionManifold::AmbientSize() const { return 4; } int EigenQuaternionManifold::TangentSize() const { return 3; } bool EigenQuaternionManifold::Plus(const double *x, const double *delta, double *x_plus_delta) const { Eigen::Map quaternionSrc(x); Eigen::Map deltaVec(delta); Eigen::Map quaternionDes(x_plus_delta); quaternionDes = (quaternionSrc * MathUtility::deltaQ(deltaVec)).normalized(); return true; } bool EigenQuaternionManifold::PlusJacobian(const double *x, double *jacobian) const { Eigen::Map> jacobianWrapper( jacobian); jacobianWrapper.topRows<3>().setIdentity(); jacobianWrapper.bottomRows<1>().setZero(); return true; } bool EigenQuaternionManifold::Minus(const double *y, const double *x, double *y_minus_x) const { Eigen::Map quaternionSrc(y); Eigen::Map quaternionDes(x); Eigen::Map deltaVec(y_minus_x); deltaVec = MathUtility::vectorQ(quaternionSrc * quaternionDes.inverse()); return true; } bool EigenQuaternionManifold::MinusJacobian(const double *x, double *jacobian) const { Eigen::Map> jacobianWrapper( jacobian); jacobianWrapper.topRows<3>().setIdentity(); jacobianWrapper.rightCols<1>().setZero(); jacobianWrapper = -jacobianWrapper; return true; } } // namespace Manifold ================================================ FILE: rio/src/manifold/EigenQuaternionManifold.hpp ================================================ #pragma once #include #include "Eigen/Eigen" namespace Manifold { class EigenQuaternionManifold : public ceres::Manifold { public: EigenQuaternionManifold() = default; ~EigenQuaternionManifold() override = default; int AmbientSize() const final; int TangentSize() const final; bool Plus(const double *x, const double *delta, double *x_plus_delta) const final; bool PlusJacobian(const double *x, double *jacobian) const final; bool Minus(const double *y, const double *x, double *y_minus_x) const final; bool MinusJacobian(const double *x, double *jacobian) const final; }; } // namespace Manifold ================================================ FILE: rio/src/residual/IMUResidual.cpp ================================================ #include "IMUResidual.hpp" #include #include #include "MathUtility.hpp" // #include // #include namespace Residuals { IMUResidual::IMUResidual(Preintegration imuPreintegration, const Eigen::Vector3d &gravity) : imuIntegration(imuPreintegration), gInW(gravity){}; bool IMUResidual::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { // position of 1st imu frame (3) Eigen::Vector3d tIToW(parameters[0][0], parameters[0][1], parameters[0][2]); // rotation of 1st imu frame (4) Eigen::Quaterniond qIToW(parameters[1][3], parameters[1][0], parameters[1][1], parameters[1][2]); // velocity of 1st imu frame (3) Eigen::Vector3d velocityI(parameters[2][0], parameters[2][1], parameters[2][2]); // accelerometer bias of 1st imu frame (3) Eigen::Vector3d biasAccI(parameters[3][0], parameters[3][1], parameters[3][2]); // gyroscope bias of 1st imu frame (3) Eigen::Vector3d biasGyroI(parameters[4][0], parameters[4][1], parameters[4][2]); // position of 2nd imu frame (3) Eigen::Vector3d tJToW(parameters[5][0], parameters[5][1], parameters[5][2]); // rotation of 2nd imu frame (4) Eigen::Quaterniond qJToW(parameters[6][3], parameters[6][0], parameters[6][1], parameters[6][2]); // velocity of 2nd imu frame (3) Eigen::Vector3d velocityJ(parameters[7][0], parameters[7][1], parameters[7][2]); // accelerometer bias of 2nd imu frame (3) Eigen::Vector3d biasAccJ(parameters[8][0], parameters[8][1], parameters[8][2]); // gyroscope bias of 2nd imu frame (3) Eigen::Vector3d biasGyroJ(parameters[9][0], parameters[9][1], parameters[9][2]); // calculate residuals Eigen::Map> residual(residuals); // correct result by newest biases Eigen::Vector3d deltaBiasAcc = biasAccI - imuIntegration.getLinearizedBiasAcc(); Eigen::Vector3d deltaBiasGyro = biasGyroI - imuIntegration.getLinearizedBiasGyro(); Eigen::Matrix3d jacobPosToBiasAcc = imuIntegration.getJacobian().block<3, 3>(0, 9); Eigen::Matrix3d jacobPosToBiasGyro = imuIntegration.getJacobian().block<3, 3>(0, 12); Eigen::Matrix3d jacobRotToBiasGyro = imuIntegration.getJacobian().block<3, 3>(3, 12); Eigen::Matrix3d jacobVelToBiasAcc = imuIntegration.getJacobian().block<3, 3>(6, 9); Eigen::Matrix3d jacobVelToBiasGyro = imuIntegration.getJacobian().block<3, 3>(6, 12); double totalTime = imuIntegration.getTotalTime(); Eigen::Quaterniond correctedDeltaQ = imuIntegration.getDeltaQ() * MathUtility::deltaQ(jacobRotToBiasGyro * deltaBiasGyro); Eigen::Vector3d correctedDeltaV = imuIntegration.getDeltaV() + jacobVelToBiasAcc * deltaBiasAcc + jacobVelToBiasGyro * deltaBiasGyro; Eigen::Vector3d correctedDeltaP = imuIntegration.getDeltaP() + jacobPosToBiasAcc * deltaBiasAcc + jacobPosToBiasGyro * deltaBiasGyro; // position residuals residual.block<3, 1>(0, 0) = qIToW.inverse() * (0.5 * gInW * totalTime * totalTime + tJToW - tIToW - velocityI * totalTime) - correctedDeltaP; // rotation residuals, slightly different with paper, // results are same, but jacobians are not residual.block<3, 1>(3, 0) = 2 * (correctedDeltaQ.inverse() * (qIToW.inverse() * qJToW)).vec(); // velocity residuals residual.block<3, 1>(6, 0) = qIToW.inverse() * (gInW * totalTime + velocityJ - velocityI) - correctedDeltaV; // accelerometer bias residual residual.block<3, 1>(9, 0) = biasAccJ - biasAccI; // gyroscope bias residual residual.block<3, 1>(12, 0) = biasGyroJ - biasGyroI; Eigen::Matrix sqrtInfoMatrix = Eigen::LLT>( imuIntegration.getCovariance().inverse()) .matrixL() .transpose(); residual = sqrtInfoMatrix * residual; // TODO // std::cout << "//////////" << std::endl << sqrtInfoMatrix << std::endl; if (jacobians == nullptr) return true; // compute jacobians // jacobian w.r.t i-th imu frame postion if (jacobians[0] != nullptr) { Eigen::Map> jacobianPositionI( jacobians[0]); jacobianPositionI.setZero(); // position error jacobianPositionI.block<3, 3>(0, 0) = -qIToW.inverse().toRotationMatrix(); jacobianPositionI = sqrtInfoMatrix * jacobianPositionI; } // jacobian w.r.t i-th imu frame rotation if (jacobians[1] != nullptr) { Eigen::Map> jacobianRotationI( jacobians[1]); jacobianRotationI.setZero(); // position error jacobianRotationI.block<3, 3>(0, 0) = MathUtility::skewSymmetric( qIToW.inverse() * (0.5 * gInW * totalTime * totalTime + tJToW - tIToW - velocityI * totalTime)); // rotation error jacobianRotationI.block<3, 3>(3, 0) = -(MathUtility::leftMultiplyQ(qJToW.inverse() * qIToW) * MathUtility::rightMultiplyQ(correctedDeltaQ)) .bottomRightCorner<3, 3>(); // velocity error jacobianRotationI.block<3, 3>(6, 0) = MathUtility::skewSymmetric( qIToW.inverse() * (gInW * totalTime + velocityJ - velocityI)); jacobianRotationI = sqrtInfoMatrix * jacobianRotationI; } // jacobian w.r.t i-th imu speed if (jacobians[2] != nullptr) { Eigen::Map> jacobianVelocityI( jacobians[2]); jacobianVelocityI.setZero(); // position error jacobianVelocityI.block<3, 3>(0, 0) = -qIToW.inverse().toRotationMatrix() * totalTime; // velocity error jacobianVelocityI.block<3, 3>(6, 0) = -qIToW.inverse().toRotationMatrix(); jacobianVelocityI = sqrtInfoMatrix * jacobianVelocityI; } // jacobian w.r.t i-th accelerometer bias if (jacobians[3] != nullptr) { Eigen::Map> jacobianAccBiasI( jacobians[3]); jacobianAccBiasI.setZero(); // position error jacobianAccBiasI.block<3, 3>(0, 0) = -jacobPosToBiasAcc; // velocity error jacobianAccBiasI.block<3, 3>(6, 0) = -jacobVelToBiasAcc; // accelerometer bias error jacobianAccBiasI.block<3, 3>(9, 0) = -Eigen::Matrix3d::Identity(); jacobianAccBiasI = sqrtInfoMatrix * jacobianAccBiasI; } // jacobian w.r.t i-th gyroscope bias if (jacobians[4] != nullptr) { Eigen::Map> jacobianGyroBiasI( jacobians[4]); jacobianGyroBiasI.setZero(); // position error jacobianGyroBiasI.block<3, 3>(0, 0) = -jacobPosToBiasGyro; // rotation error jacobianGyroBiasI.block<3, 3>(3, 0) = -MathUtility::leftMultiplyQ(qJToW.inverse() * qIToW * imuIntegration.getDeltaQ()) .bottomRightCorner<3, 3>() * jacobRotToBiasGyro; // velocity error jacobianGyroBiasI.block<3, 3>(6, 0) = -jacobVelToBiasGyro; // gyroscope bias error jacobianGyroBiasI.block<3, 3>(12, 0) = -Eigen::Matrix3d::Identity(); jacobianGyroBiasI = sqrtInfoMatrix * jacobianGyroBiasI; } // jacobian w.r.t j-th imu frame postion if (jacobians[5] != nullptr) { Eigen::Map> jacobianPositionJ( jacobians[5]); jacobianPositionJ.setZero(); // position error jacobianPositionJ.block<3, 3>(0, 0) = qIToW.inverse().toRotationMatrix(); jacobianPositionJ = sqrtInfoMatrix * jacobianPositionJ; } // jacobian w.r.t j-th imu frame rotation if (jacobians[6] != nullptr) { Eigen::Map> jacobianRotationJ( jacobians[6]); jacobianRotationJ.setZero(); // rotation error jacobianRotationJ.block<3, 3>(3, 0) = MathUtility::leftMultiplyQ(correctedDeltaQ.inverse() * qIToW.inverse() * qJToW) .bottomRightCorner<3, 3>(); jacobianRotationJ = sqrtInfoMatrix * jacobianRotationJ; } // jacobian w.r.t j-th imu speed if (jacobians[7] != nullptr) { Eigen::Map> jacobianVelocityJ( jacobians[7]); jacobianVelocityJ.setZero(); // velocity error jacobianVelocityJ.block<3, 3>(6, 0) = qIToW.inverse().toRotationMatrix(); jacobianVelocityJ = sqrtInfoMatrix * jacobianVelocityJ; } // jacobian w.r.t j-th accelerometer bias if (jacobians[8] != nullptr) { Eigen::Map> jacobianAccBiasJ( jacobians[8]); jacobianAccBiasJ.setZero(); // accelerometer bias error jacobianAccBiasJ.block<3, 3>(9, 0) = Eigen::Matrix3d::Identity(); jacobianAccBiasJ = sqrtInfoMatrix * jacobianAccBiasJ; } // jacobian w.r.t j-th gyroscope bias if (jacobians[9] != nullptr) { Eigen::Map> jacobianGyroBiasJ( jacobians[9]); jacobianGyroBiasJ.setZero(); // accelerometer bias error jacobianGyroBiasJ.block<3, 3>(12, 0) = Eigen::Matrix3d::Identity(); jacobianGyroBiasJ = sqrtInfoMatrix * jacobianGyroBiasJ; } return true; } } // namespace Residuals ================================================ FILE: rio/src/residual/IMUResidual.hpp ================================================ #pragma once #include // #include #include "Preintegration.hpp" namespace Residuals { // size of parameters in each parameter block. // // number of residuals (15), // // position of previous imu frame (3), // rotation of previous imu frame (4), // velocity of previous imu frame (3), // accelerometer bias of previous imu frame (3), // gyroscope bias of previous imu frame (3), // // position of current imu frame (3), // rotation of current imu frame (4), // velocity of current imu frame (3), // accelerometer bias of current imu frame (3), // gyroscope bias of current imu frame (3) class IMUResidual : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3> { public: IMUResidual(Preintegration imuPreintegration, const Eigen::Vector3d &gravity); bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const final; private: Preintegration imuIntegration; Eigen::Vector3d gInW; }; } // namespace Residuals ================================================ FILE: rio/src/residual/Preintegration.cpp ================================================ #include "Preintegration.hpp" #include #include namespace Residuals { Preintegration::Preintegration(Eigen::Vector3d accData, Eigen::Vector3d gyroData, Eigen::Vector3d biasAcc, Eigen::Vector3d biasGyro, double accNoise, double gyroNoise, double accBiasNoise, double gyroBiasNoise) : linearizedBiasAcc(std::move(biasAcc)), linearizedBiasGyro(std::move(biasGyro)) { deltaP.setZero(); deltaV.setZero(); deltaQ.setIdentity(); jacobian.setIdentity(); covariance.setZero(); imuBuffer.emplace_back(IMUData{accData, gyroData, 0}); noise.setZero(); noise.block<3, 3>(0, 0) = (accNoise * accNoise) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(3, 3) = gyroNoise * gyroNoise * Eigen::Matrix3d::Identity(); noise.block<3, 3>(6, 6) = (accNoise * accNoise) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(9, 9) = (gyroNoise * gyroNoise) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(12, 12) = (accBiasNoise * accBiasNoise) * Eigen::Matrix3d::Identity(); noise.block<3, 3>(15, 15) = (gyroBiasNoise * gyroBiasNoise) * Eigen::Matrix3d::Identity(); } void Preintegration::clearData() { deltaP.setZero(); deltaV.setZero(); deltaQ.setIdentity(); imuBuffer.clear(); totalTime = 0; } void Preintegration::propagate(double deltaTime, Eigen::Vector3d accData, Eigen::Vector3d gyroData) { imuBuffer.emplace_back(IMUData{accData, gyroData, deltaTime}); integrate(imuBuffer.back().deltaTime, (imuBuffer.end() - 2)->accData, (imuBuffer.end() - 2)->gyroData, imuBuffer.back().accData, imuBuffer.back().gyroData, linearizedBiasAcc, linearizedBiasGyro, deltaP, deltaQ, deltaV); totalTime += deltaTime; } void Preintegration::repropagate(Eigen::Vector3d biasAcc, Eigen::Vector3d biasGyro) { deltaP.setZero(); deltaV.setZero(); deltaQ.setIdentity(); linearizedBiasAcc = biasAcc; linearizedBiasGyro = biasGyro; jacobian.setIdentity(); covariance.setZero(); for (auto iter = imuBuffer.begin() + 1; iter != imuBuffer.end(); iter++) integrate(iter->deltaTime, (iter - 1)->accData, (iter - 1)->gyroData, iter->accData, iter->gyroData, linearizedBiasAcc, linearizedBiasGyro, deltaP, deltaQ, deltaV); } Eigen::Vector3d Preintegration::getDeltaP() const { return deltaP; } Eigen::Quaterniond Preintegration::getDeltaQ() const { return deltaQ; } Eigen::Vector3d Preintegration::getDeltaV() const { return deltaV; } Eigen::Matrix Preintegration::getJacobian() const { return jacobian; }; Eigen::Matrix Preintegration::getCovariance() const { return covariance; }; double Preintegration::getTotalTime() const { return totalTime; } Eigen::Vector3d Preintegration::getLinearizedBiasAcc() const { return linearizedBiasAcc; } Eigen::Vector3d Preintegration::getLinearizedBiasGyro() const { return linearizedBiasGyro; } void Preintegration::integrate( double deltaTime, Eigen::Vector3d startAcc, Eigen::Vector3d startGyro, Eigen::Vector3d endAcc, Eigen::Vector3d endGyro, Eigen::Vector3d biasAcc, Eigen::Vector3d biasGyro, Eigen::Vector3d &deltaPos, Eigen::Quaterniond &deltaRot, Eigen::Vector3d &deltaVel) { // mid-point integration Eigen::Vector3d resultDeltaPos; Eigen::Quaterniond resultDeltaRot; Eigen::Vector3d resultDeltaVel; // rotation result Eigen::Vector3d unbiasedGyro = 0.5 * (startGyro + endGyro) - biasGyro; resultDeltaRot = deltaRot * Eigen::Quaterniond(1, unbiasedGyro(0) * deltaTime / 2, unbiasedGyro(1) * deltaTime / 2, unbiasedGyro(2) * deltaTime / 2); // Why not normalize here? // resultDeltaRot.normalize(); // position and velocity result Eigen::Vector3d unbiasedStartAcc = startAcc - biasAcc; Eigen::Vector3d unbiasedEndAcc = endAcc - biasAcc; Eigen::Vector3d unbiasedAcc = 0.5 * (deltaRot * unbiasedStartAcc + resultDeltaRot * unbiasedEndAcc); resultDeltaPos = deltaPos + deltaVel * deltaTime + 0.5 * unbiasedAcc * deltaTime * deltaTime; resultDeltaVel = deltaVel + unbiasedAcc * deltaTime; // update jacobian // process model Eigen::MatrixXd processModel = Eigen::MatrixXd::Zero(15, 15); // position processModel.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); processModel.block<3, 3>(0, 3) = -0.25 * deltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedStartAcc) * deltaTime * deltaTime + -0.25 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * (Eigen::Matrix3d::Identity() - MathUtility::skewSymmetric(unbiasedGyro) * deltaTime) * deltaTime * deltaTime; processModel.block<3, 3>(0, 6) = Eigen::Matrix3d::Identity() * deltaTime; processModel.block<3, 3>(0, 9) = -0.25 * (deltaRot.toRotationMatrix() + resultDeltaRot.toRotationMatrix()) * deltaTime * deltaTime; processModel.block<3, 3>(0, 12) = 0.25 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * deltaTime * deltaTime * deltaTime; // rotation processModel.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() - MathUtility::skewSymmetric(unbiasedGyro) * deltaTime; processModel.block<3, 3>(3, 12) = -deltaTime * Eigen::Matrix3d::Identity(); // velocity processModel.block<3, 3>(6, 3) = -0.5 * deltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedStartAcc) * deltaTime + -0.5 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * (Eigen::Matrix3d::Identity() - MathUtility::skewSymmetric(unbiasedGyro) * deltaTime) * deltaTime; processModel.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity(); processModel.block<3, 3>(6, 9) = -0.5 * (deltaRot.toRotationMatrix() + resultDeltaRot.toRotationMatrix()) * deltaTime; processModel.block<3, 3>(6, 12) = 0.5 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * deltaTime * deltaTime; // accelerometer bias processModel.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity(); // gyroscope bias processModel.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity(); // noise model Eigen::MatrixXd noiseModel = Eigen::MatrixXd::Zero(15, 18); // position noiseModel.block<3, 3>(0, 0) = 0.25 * deltaRot.toRotationMatrix() * deltaTime * deltaTime; noiseModel.block<3, 3>(0, 3) = -0.125 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * deltaTime * deltaTime * deltaTime; noiseModel.block<3, 3>(0, 6) = 0.25 * resultDeltaRot.toRotationMatrix() * deltaTime * deltaTime; noiseModel.block<3, 3>(0, 9) = -0.125 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * deltaTime * deltaTime * deltaTime; // rotation noiseModel.block<3, 3>(3, 3) = 0.5 * Eigen::Matrix3d::Identity() * deltaTime; noiseModel.block<3, 3>(3, 9) = 0.5 * Eigen::Matrix3d::Identity() * deltaTime; // velocity noiseModel.block<3, 3>(6, 0) = 0.5 * deltaRot.toRotationMatrix() * deltaTime; noiseModel.block<3, 3>(6, 3) = -0.25 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * deltaTime * deltaTime; noiseModel.block<3, 3>(6, 6) = 0.5 * resultDeltaRot.toRotationMatrix() * deltaTime; noiseModel.block<3, 3>(6, 9) = -0.25 * resultDeltaRot.toRotationMatrix() * MathUtility::skewSymmetric(unbiasedEndAcc) * deltaTime * deltaTime; // accelerometer bias noiseModel.block<3, 3>(9, 12) = Eigen::Matrix3d::Identity() * deltaTime; // gyroscope bias noiseModel.block<3, 3>(12, 15) = Eigen::Matrix3d::Identity() * deltaTime; // update result jacobian = processModel * jacobian; covariance = processModel * covariance * processModel.transpose() + noiseModel * noise * noiseModel.transpose(); // due to approximation the result quaternion may not be legal resultDeltaRot.normalize(); deltaPos = resultDeltaPos; deltaRot = resultDeltaRot; deltaVel = resultDeltaVel; } } // namespace Residuals ================================================ FILE: rio/src/residual/Preintegration.hpp ================================================ #pragma once #include #include namespace Residuals { class Preintegration { struct IMUData { Eigen::Vector3d accData; Eigen::Vector3d gyroData; double deltaTime; }; public: Preintegration(Eigen::Vector3d accData, Eigen::Vector3d gyroData, Eigen::Vector3d biasAcc, Eigen::Vector3d biasGyro, double accNoise, double gyroNoise, double accBiasNoise, double gyroBiasNoise); ~Preintegration() = default; Preintegration() = delete; Preintegration(Preintegration &another) = default; Preintegration(Preintegration &&another) = default; Preintegration &operator=(const Preintegration &another) = default; Preintegration &operator=(Preintegration &&another) = default; void clearData(); void propagate(double deltaTime, Eigen::Vector3d accData, Eigen::Vector3d gyroData); void repropagate(Eigen::Vector3d biasAcc, Eigen::Vector3d biasGyro); Eigen::Vector3d getDeltaP() const; Eigen::Quaterniond getDeltaQ() const; Eigen::Vector3d getDeltaV() const; Eigen::Matrix getJacobian() const; Eigen::Matrix getCovariance() const; Eigen::Vector3d getLinearizedBiasAcc() const; Eigen::Vector3d getLinearizedBiasGyro() const; double getTotalTime() const; private: void integrate(double deltaTime, Eigen::Vector3d startAcc, Eigen::Vector3d startGyro, Eigen::Vector3d endAcc, Eigen::Vector3d endGyro, Eigen::Vector3d biasAcc, Eigen::Vector3d biasGyro, Eigen::Vector3d &deltaPos, Eigen::Quaterniond &deltaRot, Eigen::Vector3d &deltaVel); Eigen::Vector3d deltaP; Eigen::Quaterniond deltaQ; Eigen::Vector3d deltaV; std::vector imuBuffer; Eigen::Vector3d linearizedBiasAcc; Eigen::Vector3d linearizedBiasGyro; Eigen::Matrix noise; Eigen::Matrix jacobian; Eigen::Matrix covariance; double totalTime = 0; }; } // namespace Residuals ================================================ FILE: rio/src/residual/RadarImuVelResidual.cpp ================================================ #include "RadarImuVelResidual.hpp" #include #include #include "MathUtility.hpp" namespace Residuals { RadarImuVelocityResidual::RadarImuVelocityResidual( Eigen::Vector3d angularVelocity, double doppler, double sqrtInfoGain) : angularVel(angularVelocity), dopplerVel(doppler), sqrtInfoGain(sqrtInfoGain) {} RadarImuVelocityResidual::RadarImuVelocityResidual( Eigen::Vector3d angularVelocity, double doppler, double azimuth, double elevation, double SigmaTheta, double SigmaPhi, double maxInfoGain) : angularVel(angularVelocity), dopplerVel(doppler), Theta(azimuth), Phi(elevation), SigmaTheta(SigmaTheta), SigmaPhi(SigmaPhi), maxInfoGain(maxInfoGain) { JThetaPhi << -sin(Theta) * cos(Phi), -cos(Theta) * sin(Phi), cos(Theta) * cos(Phi), -sin(Theta) * sin(Phi), 0, cos(Phi); } double RadarImuVelocityResidual::calculateSqrtInfoGain( Eigen::Vector3d velInRadar) const { double sqrtInfoGain; Eigen::Matrix Sigma; Sigma << SigmaTheta * SigmaTheta, 0, 0, SigmaPhi * SigmaPhi; sqrtInfoGain = 1.0 / (velInRadar.transpose() * JThetaPhi * Sigma * JThetaPhi.transpose() * velInRadar); sqrtInfoGain = sqrt(sqrtInfoGain); sqrtInfoGain = (sqrtInfoGain > maxInfoGain || std::isnan(sqrtInfoGain)) ? maxInfoGain : sqrtInfoGain; // std::cout << "sqrtInfoGain: " << sqrtInfoGain << std::endl; return sqrtInfoGain; } bool RadarImuVelocityResidual::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { // rotation of current frame (4) Eigen::Quaterniond qImuToW(parameters[0][3], parameters[0][0], parameters[0][1], parameters[0][2]); // velocity of current frame (3) Eigen::Vector3d velInWorld(parameters[1][0], parameters[1][1], parameters[1][2]); // gyroscope bias of current frame (3) Eigen::Vector3d gyroBias(parameters[2][0], parameters[2][1], parameters[2][2]); // translation of extrinsic Eigen::Vector3d tRadarToImu(parameters[3][0], parameters[3][1], parameters[3][2]); // rotation of extrinsic Eigen::Quaterniond qRadarToImu(parameters[4][3], parameters[4][0], parameters[4][1], parameters[4][2]); // point state in radar Eigen::Vector3d pointState(parameters[5][0], parameters[5][1], parameters[5][2]); Eigen::Vector3d unbiasedAngularVel = angularVel - gyroBias; Eigen::Vector3d tangentVel = MathUtility::skewSymmetric(unbiasedAngularVel) * tRadarToImu; Eigen::Vector3d radialVel = qImuToW.inverse() * velInWorld; Eigen::Vector3d velInRadar = qRadarToImu.inverse() * (radialVel + tangentVel); Eigen::Vector3d direction = pointState.normalized(); double DynamicSqrtInfoGain; if (sqrtInfoGain == 0) { DynamicSqrtInfoGain = calculateSqrtInfoGain(velInRadar); } else { DynamicSqrtInfoGain = sqrtInfoGain; } // ROS_INFO("DynamicSqrtInfoGain: %f", DynamicSqrtInfoGain); *residuals = DynamicSqrtInfoGain * (direction.dot(velInRadar) - dopplerVel); if (jacobians == nullptr) return true; Eigen::Matrix3d rIToW = qImuToW.toRotationMatrix(); Eigen::Matrix3d rRtoI = qRadarToImu.toRotationMatrix(); // jacobian w.r.t to current rotation if (jacobians[0] != nullptr) { Eigen::Map> jacobianRotation( jacobians[0]); jacobianRotation.setZero(); jacobianRotation.block<1, 3>(0, 0) = direction.transpose() * rRtoI.transpose() * MathUtility::skewSymmetric(radialVel); jacobianRotation = DynamicSqrtInfoGain * jacobianRotation; } // jacobian w.r.t to current velocity if (jacobians[1] != nullptr) { Eigen::Map> jacobianVelocity( jacobians[1]); jacobianVelocity.setZero(); jacobianVelocity = direction.transpose() * rRtoI.transpose() * rIToW.transpose(); jacobianVelocity = DynamicSqrtInfoGain * jacobianVelocity; } // jacobian w.r.t to current gyroscope bias if (jacobians[2] != nullptr) { Eigen::Map> jacobianGyroBias( jacobians[2]); jacobianGyroBias.setZero(); jacobianGyroBias = direction.transpose() * rRtoI.transpose() * MathUtility::skewSymmetric(tRadarToImu); jacobianGyroBias = DynamicSqrtInfoGain * jacobianGyroBias; } // jacobian w.r.t to extrinsic translation if (jacobians[3] != nullptr) { Eigen::Map> jacobianPositionEx( jacobians[3]); jacobianPositionEx.setZero(); jacobianPositionEx = direction.transpose() * rRtoI.transpose() * MathUtility::skewSymmetric(unbiasedAngularVel); jacobianPositionEx = DynamicSqrtInfoGain * jacobianPositionEx; } // jacobian w.r.t to extrinsic rotation if (jacobians[4] != nullptr) { Eigen::Map> jacobianRotationEx( jacobians[4]); jacobianRotationEx.setZero(); jacobianRotationEx.block<1, 3>(0, 0) = direction.transpose() * MathUtility::skewSymmetric(velInRadar); jacobianRotationEx = DynamicSqrtInfoGain * jacobianRotationEx; } // jacobian w.r.t to Point State if (jacobians[5] != nullptr) { Eigen::Map> jacobianPointState( jacobians[5]); double normValue = pointState.norm(); jacobianPointState = DynamicSqrtInfoGain * (velInRadar.transpose() / normValue - pointState.transpose() * velInRadar * pointState.transpose() / (normValue * normValue * normValue)); // jacobianPositionEx = -direction.transpose() * rRtoI.transpose() * // MathUtility::skewSymmetric(unbiasedAngularVel); // jacobianPositionEx = sqrtInfoGain * jacobianPositionEx; } return true; } } // namespace Residuals ================================================ FILE: rio/src/residual/RadarImuVelResidual.hpp ================================================ #pragma once #include #include "Eigen/Dense" #include "Eigen/Eigen" #include "ceres/ceres.h" namespace Residuals { // size of parameters in each parameter block. // // number of residuals (1), // // rotation (4) // velocity (3) // gyroscope bias (3), // extrinsic transaltion (3) // extrinsic rotation (4) // point state in radar (3) class RadarImuVelocityResidual : public ceres::SizedCostFunction<1, 4, 3, 3, 3, 4, 3> { public: RadarImuVelocityResidual(Eigen::Vector3d angularVelocity, double doppler, double sqrtInfoGain); RadarImuVelocityResidual(Eigen::Vector3d angularVelocity, double doppler, double azimuth, double elevation, double SigmaTheta, double SigmaPhi, double maxInfoGain); bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const final; private: Eigen::Vector3d angularVel; double dopplerVel; double sqrtInfoGain = 0; double maxInfoGain = 0; double Theta; double Phi; double SigmaTheta; double SigmaPhi; Eigen::Matrix JThetaPhi; double calculateSqrtInfoGain(Eigen::Vector3d velInRadar) const; }; } // namespace Residuals ================================================ FILE: rio/src/residual/RadarPointResidual.cpp ================================================ #include "RadarPointResidual.hpp" #include "MathUtility.hpp" namespace Residuals { RadarPointResidual::RadarPointResidual(Eigen::Vector3d measurement, double sqrtInfoGainValue) : measuredPoint(measurement) { this->sqrtInfoGain << sqrtInfoGainValue, 0, 0, 0, sqrtInfoGainValue, 0, 0, 0, sqrtInfoGainValue; } RadarPointResidual::RadarPointResidual(Eigen::Vector3d measurement, double range, double azimuth, double elevation, double SigmaRange, double SigmaTheta, double SigmaPhi, double maxInfoGain, int observationCount, int pointNum) : measuredPoint(measurement), Range(range), Theta(azimuth), Phi(elevation), SigmaRange(SigmaRange), SigmaTheta(SigmaTheta), SigmaPhi(SigmaPhi), maxInfoGain(maxInfoGain), observationCount(observationCount), pointNum(pointNum) { // std::cout << "range: " << range << "\t" << "azimuth: " << azimuth << "\t" // << "elevation: " << elevation << std::endl; JRangeThetaPhi << cos(Theta) * cos(Phi), -Range * sin(Theta) * cos(Phi), -Range * cos(Theta) * sin(Phi), sin(Theta) * cos(Phi), Range * cos(Theta) * cos(Phi), -Range * sin(Theta) * sin(Phi), sin(Phi), 0, -Range * cos(Phi); // std::cout << "JRangeThetaPhi: " << JRangeThetaPhi << std::endl; calculateSqrtInfoGain(Range, Theta, Phi); // std::cout << "sqrtInfoGain: " << sqrtInfoGain << std::endl; } void RadarPointResidual::calculateSqrtInfoGain(double range, double azimuth, double elevation) { Eigen::Matrix Sigma; Sigma << SigmaRange * SigmaRange, 0, 0, 0, SigmaTheta * SigmaTheta, 0, 0, 0, SigmaPhi * SigmaPhi; Eigen::Matrix Cov; Cov = JRangeThetaPhi * Sigma * JRangeThetaPhi.transpose(); Eigen::Matrix InfoGain = Cov.inverse(); Eigen::SelfAdjointEigenSolver> es(InfoGain); sqrtInfoGain = es.operatorSqrt(); sqrtInfoGain = sqrtInfoGain * observationCount / pointNum; double scale = abs(sqrtInfoGain.maxCoeff() / maxInfoGain); scale = scale > abs(sqrtInfoGain.minCoeff() / maxInfoGain) ? scale : abs(sqrtInfoGain.minCoeff() / maxInfoGain); sqrtInfoGain = sqrtInfoGain / scale; if (es.info() != Eigen::Success || sqrtInfoGain.hasNaN()) { sqrtInfoGain << maxInfoGain, 0, 0, 0, maxInfoGain, 0, 0, 0, maxInfoGain; } // std::cout << "sqrtInfoGain: \n" << sqrtInfoGain << std::endl; } bool RadarPointResidual::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const { // translation of i-th frame (3) Eigen::Vector3d tIToW(parameters[0][0], parameters[0][1], parameters[0][2]); // rotation of i-th frame (4) Eigen::Quaterniond qIToW(parameters[1][3], parameters[1][0], parameters[1][1], parameters[1][2]); // translation of extrinsic (3) Eigen::Vector3d tRadarToImu(parameters[2][0], parameters[2][1], parameters[2][2]); // rotation of extrinsic (4) Eigen::Quaterniond qRadarToImu(parameters[3][3], parameters[3][0], parameters[3][1], parameters[3][2]); // point in world (3) Eigen::Vector3d pointState(parameters[4][0], parameters[4][1], parameters[4][2]); Eigen::Map resultResidual(residuals); Eigen::Vector3d pointInIImu = qRadarToImu * measuredPoint + tRadarToImu; Eigen::Vector3d pointInWorld = qIToW * pointInIImu + tIToW; resultResidual = sqrtInfoGain * (pointState - pointInWorld); if (jacobians == nullptr) return true; // jacobians w.r.t translation of frame i if (jacobians[0] != nullptr) { Eigen::Map> jacobianPositionI( jacobians[0]); jacobianPositionI = -sqrtInfoGain * Eigen::Matrix3d::Identity(); } // jacobians w.r.t rotation of frame i if (jacobians[1] != nullptr) { Eigen::Map> jacobianRotationI( jacobians[1]); jacobianRotationI.setZero(); jacobianRotationI.block<3, 3>(0, 0) = sqrtInfoGain * qIToW.toRotationMatrix() * MathUtility::skewSymmetric(pointInIImu); } // jacobians w.r.t translation of extrinsic if (jacobians[2] != nullptr) { Eigen::Map> jacobianPositionEx( jacobians[2]); jacobianPositionEx = -sqrtInfoGain * qIToW.toRotationMatrix(); } // jacobians w.r.t rotation of extrinsic if (jacobians[3] != nullptr) { Eigen::Map> jacobianRotationEx( jacobians[3]); jacobianRotationEx.setZero(); jacobianRotationEx.block<3, 3>(0, 0) = sqrtInfoGain * qIToW.toRotationMatrix() * qRadarToImu.toRotationMatrix() * MathUtility::skewSymmetric(measuredPoint); } // jacobians w.r.t translation of point if (jacobians[4] != nullptr) { Eigen::Map> jacobianPositionEx( jacobians[4]); jacobianPositionEx = sqrtInfoGain * Eigen::Matrix3d::Identity(); } return true; } } // namespace Residuals ================================================ FILE: rio/src/residual/RadarPointResidual.hpp ================================================ #pragma once #include #include #include "../utility/MathUtility.hpp" namespace Residuals { // size of parameters in each parameter block. // // number of residuals (3), // // translation of i-th frame (3) // rotation of i-th frame (4) // translation of extrinsic (radar to imu) (3) // rotation of extrinsic (radar to imu) (4) // point in world (3) class RadarPointResidual : public ceres::SizedCostFunction<3, 3, 4, 3, 4, 3> { public: RadarPointResidual(Eigen::Vector3d measurement, double sqrtInfoGainValue); RadarPointResidual(Eigen::Vector3d measurement, double range, double azimuth, double elevation, double SigmaRange, double SigmaTheta, double SigmaPhi, double maxInfoGain, int observationCount = 1, int pointNum = 1); bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const final; private: Eigen::Vector3d measuredPoint; Eigen::Matrix sqrtInfoGain; double SigmaRange; double SigmaTheta; double SigmaPhi; double maxInfoGain; double Theta; double Phi; double Range; int observationCount; int pointNum; Eigen::Matrix JRangeThetaPhi; void calculateSqrtInfoGain(double range, double azimuth, double elevation); }; } // namespace Residuals ================================================ FILE: rio/src/residual/readme.md ================================================ ## Residual [Projection Residual](./doc/ProjectionResidual.md) [IMU Residual](./doc/IMUResidual.md) ================================================ FILE: rio/src/state/IMUState.hpp ================================================ #pragma once namespace State { constexpr int SIZE_IMU_STATE = 9; struct IMUState { // Vx,Vy,Vz double velocityParams[3]; // Bax,Bay,Baz double biasAccParams[3]; // Bgx,Bgy,Bgz double biasGyroParams[3]; }; } // namespace State ================================================ FILE: rio/src/state/R3State.hpp ================================================ #pragma once namespace State { constexpr int SIZE_R3_STATE = 3; struct R3State { // Px,Py,Pz double positionParams[3]; }; } // namespace State ================================================ FILE: rio/src/state/RadarPointFeatureState.hpp ================================================ #pragma once namespace State { constexpr int SIZE_RADAR_POINT_FEATURE_STATE = 2; struct RadarPointFeatureState { double azimuth[1]; double elevation[1]; }; } // namespace State ================================================ FILE: rio/src/state/SE3State.hpp ================================================ #pragma once namespace State { constexpr int SIZE_SE3_STATE = 7; struct SE3State { // Px,Py,Pz double positionParams[3]; // Qx,Qy,Qz,Qw double rotationParams[4]; }; } // namespace State ================================================ FILE: rio/src/state/StateManager.cpp ================================================ ================================================ FILE: rio/src/state/StateManager.hpp ================================================ #pragma once #include #include #include #include "IMUState.hpp" #include "R3State.hpp" #include "RadarPointFeatureState.hpp" #include "SE3State.hpp" namespace State { constexpr int MAX_WINDOWS_SIZE = 6; constexpr int STATE_BUFFER_SIZE = 1; constexpr int ARRAY_SIZE = MAX_WINDOWS_SIZE + STATE_BUFFER_SIZE; constexpr int MAX_POINT_FEATURE_SIZE = 500 * MAX_WINDOWS_SIZE; class StateManager { public: std::array basicState{}; int basicStateNum = 0; std::array imuState{}; int imuStateNum = 0; std::array pointState{}; int pointNum = 0; SE3State exRadarToImu{}; // template // void slideOutFront(std::array array, int &number) // { // for (int i = 0; i < number - 1; i++) // array[i] = array[i + 1]; // number--; // } // template // void slideOutBack(std::array array, int &number) // { // number--; // } }; } // namespace State ================================================ FILE: rio/src/utility/MathUtility.hpp ================================================ #pragma once #include #include "Eigen/Eigen" namespace MathUtility { static Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d &rotationVector) { Eigen::Matrix3d result; // clang-format off result << 0.0, -rotationVector.z(), rotationVector.y(), rotationVector.z(), 0.0, -rotationVector.x(), -rotationVector.y(),rotationVector.x(), 0.0; // clang-format on return result; } static Eigen::Quaterniond deltaQ(const Eigen::Vector3d &theta) { Eigen::Quaterniond result; Eigen::Vector3d halfTheta = theta / 2.0; result.w() = 1.0; result.x() = halfTheta.x(); result.y() = halfTheta.y(); result.z() = halfTheta.z(); return result; } static Eigen::Vector3d vectorQ(const Eigen::Quaterniond &quat) { Eigen::Vector3d result; double norm = std::hypot(quat.x(), quat.y(), quat.z()); if (std::fpclassify(norm) == FP_ZERO) return result.setZero(); double theta = std::atan2(norm, quat.w()); result.x() = theta * quat.x() / norm; result.y() = theta * quat.y() / norm; result.z() = theta * quat.z() / norm; return result; } static Eigen::Matrix4d leftMultiplyQ(const Eigen::Quaterniond &quat) { Eigen::Matrix4d result; result(0, 0) = quat.w(); result.block<1, 3>(0, 1) = -quat.vec().transpose(); result.block<3, 1>(1, 0) = quat.vec(); result.block<3, 3>(1, 1) = quat.w() * Eigen::Matrix3d::Identity() + skewSymmetric(quat.vec()); return result; } static Eigen::Matrix4d rightMultiplyQ(const Eigen::Quaterniond &quat) { Eigen::Matrix4d result; result(0, 0) = quat.w(); result.block<1, 3>(0, 1) = -quat.vec().transpose(); result.block<3, 1>(1, 0) = quat.vec(); result.block<3, 3>(1, 1) = quat.w() * Eigen::Matrix3d::Identity() - skewSymmetric(quat.vec()); return result; } } // namespace MathUtility ================================================ FILE: rio/src/utility/TicToc.hpp ================================================ /** * @file tic_toc.hpp * @author NLC (qhuangag@connect.ust.hk) * @brief Adopted from VINS * @version 0.1 * @date 2022-10-18 * * @copyright Copyright (c) 2022 * */ #pragma once #include #include #include constexpr int US_TO_MS = 1000; /** * @brief Time measurment tool * */ class TicToc { public: TicToc() { tic(); } void tic() { start = std::chrono::system_clock::now(); } double toc() { end = std::chrono::system_clock::now(); std::chrono::duration elapsedSeconds = end - start; return elapsedSeconds.count() * US_TO_MS; } private: std::chrono::time_point start, end; }; ================================================ FILE: rio/src/utility/Timestamp.hpp ================================================ #pragma once #include struct Timestamp { static constexpr int MAX_NSEC_VALUE = 1000000000; uint32_t sec; uint32_t nsec; Timestamp(uint32_t secValue, uint32_t nsecValue) : sec(secValue), nsec(nsecValue) { if (nsec > MAX_NSEC_VALUE) { sec += nsec % MAX_NSEC_VALUE; nsec = nsec / MAX_NSEC_VALUE; } } Timestamp() = default; ~Timestamp() = default; Timestamp(Timestamp &another) = default; Timestamp(Timestamp &&another) = default; Timestamp &operator=(const Timestamp &another) = default; Timestamp &operator=(Timestamp &&another) = default; bool operator!=(const Timestamp &another) const { return sec != another.sec || nsec != another.nsec; } bool operator==(const Timestamp &another) const { return sec == another.sec && nsec == another.nsec; } bool operator<(const Timestamp &another) const { return sec == another.sec ? nsec < another.nsec : sec < another.sec; } bool operator>(const Timestamp &another) const { return sec == another.sec ? nsec > another.nsec : sec > another.sec; } bool operator<=(const Timestamp &another) const { return sec == another.sec ? nsec <= another.nsec : sec <= another.sec; } bool operator>=(const Timestamp &another) const { return sec == another.sec ? nsec >= another.nsec : sec >= another.sec; } double toSec() const { return sec + nsec * 1e-9; }; double toMSec() const { return sec * 1e3 + nsec * 1e-6; }; double toUSec() const { return sec * 1e6 + nsec * 1e-3; }; double toNSec() const { return sec * 1e9 + nsec; }; }; ================================================ FILE: rio/src/utility/Utility.hpp ================================================ #pragma once #include #include #include #include "opencv2/core/hal/interface.h" #include "opencv2/core/types.hpp" template inline void reduceVector(std::vector &vector, std::vector status) { int size = 0; for (int i = 0; i < int(vector.size()); i++) if (status[i]) vector[size++] = vector[i]; vector.resize(size); } inline double calDistance(cv::Point2f &point1, cv::Point2f &point2) { double deltaX = point1.x - point2.x; double deltaY = point1.y - point2.y; return sqrt(deltaX * deltaX + deltaY * deltaY); } inline double calDistance(cv::Point3f &point1, cv::Point3f &point2) { return std::sqrt((point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y) + (point1.z - point2.z) * (point1.z - point2.z)); } inline double calDistance(Eigen::Vector3d &point1, Eigen::Vector3d &point2) { return std::sqrt((point1.x() - point2.x()) * (point1.x() - point2.x()) + (point1.y() - point2.y()) * (point1.y() - point2.y()) + (point1.z() - point2.z()) * (point1.z() - point2.z())); }