Showing preview only (231K chars total). Download the full file or copy to clipboard to get everything.
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
## 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.
## Trajectories on self-collected and ColoRadar dataset
**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 <cstdint>
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 <cstdint>
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 <cstdint>
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 <cstdint>
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 <netinet/in.h>
#include <cstdint>
inline uint8_t byteArrayToUint8(const uint8_t *array) { return *array; }
inline int8_t byteArrayToInt8(const uint8_t *array)
{
return *reinterpret_cast<const int8_t *>(array);
}
inline uint16_t byteArrayToUint16(const uint8_t *array)
{
return ntohs(*reinterpret_cast<const uint16_t *>(array));
}
inline int16_t byteArrayToInt16(const uint8_t *array)
{
uint16_t temp = ntohs(*reinterpret_cast<const uint16_t *>(array));
return *reinterpret_cast<int16_t *>(&temp);
}
inline uint32_t byteArrayToUint32(const uint8_t *array)
{
return ntohl(*reinterpret_cast<const uint32_t *>(array));
}
inline int32_t byteArrayToInt32(const uint8_t *array)
{
uint32_t temp = ntohl(*reinterpret_cast<const uint32_t *>(array));
return *reinterpret_cast<int32_t *>(&temp);
}
inline uint64_t byteArrayToUint64(const uint8_t *array)
{
return static_cast<uint64_t>(*array) << 56 |
static_cast<uint64_t>(*(array + 1)) << 48 |
static_cast<uint64_t>(*(array + 2)) << 40 |
static_cast<uint64_t>(*(array + 3)) << 32 |
static_cast<uint64_t>(*(array + 4)) << 24 |
static_cast<uint64_t>(*(array + 5)) << 16 |
static_cast<uint64_t>(*(array + 6)) << 8 |
static_cast<uint64_t>(*(array + 7));
}
inline int64_t byteArrayToInt64(const uint8_t *array)
{
uint64_t temp = static_cast<uint64_t>(*array) << 56 |
static_cast<uint64_t>(*(array + 1)) << 48 |
static_cast<uint64_t>(*(array + 2)) << 40 |
static_cast<uint64_t>(*(array + 3)) << 32 |
static_cast<uint64_t>(*(array + 4)) << 24 |
static_cast<uint64_t>(*(array + 5)) << 16 |
static_cast<uint64_t>(*(array + 6)) << 8 |
static_cast<uint64_t>(*(array + 7));
return *reinterpret_cast<int64_t *>(&temp);
}
inline float byteArrayToFloat(const uint8_t *array)
{
uint32_t temp = ntohl(*reinterpret_cast<const uint32_t *>(array));
return *reinterpret_cast<float *>(&temp);
}
inline double byteArrayToDouble(const uint8_t *array)
{
uint64_t temp = static_cast<uint64_t>(*array) << 56 |
static_cast<uint64_t>(*(array + 1)) << 48 |
static_cast<uint64_t>(*(array + 2)) << 40 |
static_cast<uint64_t>(*(array + 3)) << 32 |
static_cast<uint64_t>(*(array + 4)) << 24 |
static_cast<uint64_t>(*(array + 5)) << 16 |
static_cast<uint64_t>(*(array + 6)) << 8 |
static_cast<uint64_t>(*(array + 7));
return *reinterpret_cast<double *>(&temp);
}
// inline void uint8ToByteArray(uint8_t value, uint8_t *array) { *array = value;
// }
// inline void int8ToByteArray(int8_t value, uint8_t *array)
// {
// *reinterpret_cast<int8_t *>(array) = value;
// }
// inline void uint16ToByteArray(uint16_t value, uint8_t *array)
// {
// *reinterpret_cast<uint16_t *>(array) = value;
// }
// inline void int16ToByteArray(int16_t value, uint8_t *array)
// {
// *reinterpret_cast<int16_t *>(array) = value;
// }
// inline void uint32ToByteArray(uint32_t value, uint8_t *array)
// {
// *reinterpret_cast<uint32_t *>(array) = value;
// }
// inline void int32ToByteArray(int32_t value, uint8_t *array)
// {
// *reinterpret_cast<int32_t *>(array) = value;
// }
// inline void floatToByteArray(float value, uint8_t *array)
// {
// *reinterpret_cast<float *>(array) = value;
// }
================================================
FILE: ars548_driver/data_type/VehicleInfo.hpp
================================================
#pragma once
#include <cstdint>
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 <pcl/PCLPointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <cmath>
#include <cstdint>
#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<sensor_msgs::PointCloud2>("/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<uint32_t *>(&detection.azimuthAngle);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray =
*reinterpret_cast<uint32_t *>(&detection.azimuthSTD);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray = *reinterpret_cast<uint32_t *>(
&detection.elevationAngle);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray =
*reinterpret_cast<uint32_t *>(&detection.elevationSTD);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray = *reinterpret_cast<uint32_t *>(&detection.range);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray =
*reinterpret_cast<uint32_t *>(&detection.rangeSTD);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray =
*reinterpret_cast<uint32_t *>(&detection.rangeRate);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 24) & 0xFF));
byteArray =
*reinterpret_cast<uint32_t *>(&detection.rangeRateSTD);
pubFrame.data.emplace_back(
static_cast<uint8_t>(byteArray & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 8) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((byteArray >> 16) & 0xFF));
pubFrame.data.emplace_back(
static_cast<uint8_t>((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
================================================
<?xml version="1.0"?>
<package format="2">
<name>ars548_driver</name>
<version>0.0.0</version>
<description>The ars548_driver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="root@todo.todo">root</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/ars548_driver</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
================================================
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 <pcap/pcap.h>
#include <cerrno>
#include <cstddef>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <iostream>
#include <string>
#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 <cstddef>
#include <cstdint>
#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<ptrdiff_t>(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<ptrdiff_t>(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 <cstdint>
#include <iostream>
#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 <linux/if_ether.h>
#include <linux/ip.h>
#include <linux/udp.h>
#include <netinet/in.h>
#include <pcap/pcap.h>
#include <cstdint>
#include <cstring>
#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<const ethhdr *>(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<const iphdr *>(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: <Fixed 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: <Fixed 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
================================================
<launch>
<!-- Radar Node -->
<arg name="nodeName" default="rio"/>
<!-- Test param -->
<arg name="round" default="0"/>
<arg name="playBagpath" default="$(find rio)/../dataset/exp/square_good.bag"/>
<arg name="playSpeed" default="0.5"/>
<!-- Param file -->
<arg name="configFile" default="$(find rio)/config/umrio_ars548.yaml"/>
<!-- Main node -->
<node pkg="rio" type="$(arg nodeName)" name="$(arg nodeName)" output="screen">
<rosparam command="load" file="$(arg configFile)" />
</node>
<!-- Auto test tools-->
<node pkg="rosbag" type="play" name="rosbag_play" required="true" args="$(arg playBagpath) -r $(arg playSpeed)" />
<node pkg="topic_tools" type="relay" name="relay" args="/rosbag_play/finished /shutdown" />
</launch>
================================================
FILE: rio/node/rio.cpp
================================================
#include "rosWarper.hpp"
#ifdef DEBUG
#define BACKWARD_HAS_BFD 1
#define BACKWARD_HAS_DW 1
#include <backward.hpp>
backward::SignalHandling sh;
#endif
int main(int argc, char **argv) {
ros::init(argc, argv, "rio");
ros::NodeHandle nodeHandle("~");
std::shared_ptr<RIO> rio = std::make_shared<RIO>(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<RadarType>(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<Frame::RadarData> RIO::decodeRadarMsg_ARS548(
const sensor_msgs::PointCloud2 &msg) {
std::vector<Frame::RadarData> result;
int pointBytes = static_cast<int>(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<int>(fields[i].offset);
else if (fields[i].name == "azimuthSTD")
offsetAzimuthSTD = static_cast<int>(fields[i].offset);
else if (fields[i].name == "elevation")
offsetElevation = static_cast<int>(fields[i].offset);
else if (fields[i].name == "elevationSTD")
offsetElevationSTD = static_cast<int>(fields[i].offset);
else if (fields[i].name == "range")
offsetRange = static_cast<int>(fields[i].offset);
else if (fields[i].name == "rangeSTD")
offsetRangeSTD = static_cast<int>(fields[i].offset);
else if (fields[i].name == "velocity")
offsetVelocity = static_cast<int>(fields[i].offset);
else if (fields[i].name == "velocitySTD")
offsetVelocitySTD = static_cast<int>(fields[i].offset);
else if (fields[i].name == "rcs")
offsetRCS = static_cast<int>(fields[i].offset);
}
pcl::PointCloud<pcl::PointXYZI> pointCloud;
for (auto i = 0; i < msg.row_step; i++) {
float azimuth = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetAzimuth));
float azimuthSTD = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetAzimuthSTD));
float elevation = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetElevation));
float elevationSTD = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetElevationSTD));
float range = *reinterpret_cast<const float *>(
msg.data.data() + static_cast<ptrdiff_t>(pointBytes * i + offsetRange));
float rangeSTD = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetRangeSTD));
float velocity = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetVelocity));
float velocitySTD = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetVelocitySTD));
int8_t rcs = *reinterpret_cast<const int8_t *>(
msg.data.data() + static_cast<ptrdiff_t>(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<pcl::PointXYZI> 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<Frame::RadarData> RIO::decodeRadarMsg_ColoRadar(
const sensor_msgs::PointCloud2 &msg) {
std::vector<Frame::RadarData> result;
int pointBytes = static_cast<int>(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<int>(fields[i].offset);
else if (fields[i].name == "y")
offsetY = static_cast<int>(fields[i].offset);
else if (fields[i].name == "z")
offsetZ = static_cast<int>(fields[i].offset);
else if (fields[i].name == "intensity")
offsetIntensity = static_cast<int>(fields[i].offset);
else if (fields[i].name == "range")
offsetRange = static_cast<int>(fields[i].offset);
else if (fields[i].name == "doppler")
offsetDoppler = static_cast<int>(fields[i].offset);
}
pcl::PointCloud<pcl::PointXYZI> pointCloud;
for (auto i = 0; i < msg.width; i++) {
float xValue = *reinterpret_cast<const float *>(
msg.data.data() + static_cast<ptrdiff_t>(pointBytes * i + offsetX));
float yValue = *reinterpret_cast<const float *>(
msg.data.data() + static_cast<ptrdiff_t>(pointBytes * i + offsetY));
float zValue = *reinterpret_cast<const float *>(
msg.data.data() + static_cast<ptrdiff_t>(pointBytes * i + offsetZ));
float range = *reinterpret_cast<const float *>(
msg.data.data() + static_cast<ptrdiff_t>(pointBytes * i + offsetRange));
float velocity = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(pointBytes * i + offsetDoppler));
float intensity = *reinterpret_cast<const float *>(
msg.data.data() +
static_cast<ptrdiff_t>(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<Frame::RadarData> &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<Frame::IMUFrame> 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<Frame::RadarData> &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<int> 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<pcl::PointXYZ>::Ptr featurePoints(
new pcl::PointCloud<pcl::PointXYZ>);
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<Frame::RadarData> 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<geometry_msgs::PoseStamped>(posePubTopic,
PUB_QUEUE_SIZE);
pathPub = nodeHandle.advertise<nav_msgs::Path>(pathPubTopic, PUB_QUEUE_SIZE);
gtPub = nodeHandle.advertise<geometry_msgs::PoseStamped>(gtPubTopic,
PUB_QUEUE_SIZE);
framePub = nodeHandle.advertise<sensor_msgs::PointCloud2>(framePubTopic,
PUB_QUEUE_SIZE);
trackingPub = nodeHandle.advertise<sensor_msgs::PointCloud2>(trackingPubTopic,
PUB_QUEUE_SIZE);
subMapPub = nodeHandle.advertise<sensor_msgs::PointCloud2>(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 <memory>
#include <string>
// 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 <pcl/PCLPointCloud2.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>
// Cerse Library
#include <ceres/loss_function.h>
#include <ceres/problem.h>
#include <ceres/solver.h>
#include <ceres/types.h>
// 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<pcl::PointXYZI>::Ptr trackingMap =
pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr worldMap =
pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>);
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<Frame::RadarData> decodeRadarMsg_ARS548(
const sensor_msgs::PointCloud2 &msg);
std::vector<Frame::RadarData> decodeRadarMsg_ColoRadar(
const sensor_msgs::PointCloud2 &msg);
// Estimator
Residuals::Preintegration imuPreintegration(ros::Time start, ros::Time end);
void factorGraphInit(std::vector<Frame::RadarData> &frameRadarData,
const ros::Time &timeStamp);
void constructFactor(std::vector<Frame::RadarData> &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
================================================
<?xml version="1.0"?>
<package format="2">
<name>rio</name>
<version>0.0.0</version>
<description>The rio package</description>
<maintainer email="root@todo.todo">root</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<!-- The build_depend -->
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<!-- The build_export_depend -->
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<!-- The exec_depend -->
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<export>
</export>
</package>
================================================
FILE: rio/src/data_manager/IMUManager.cpp
================================================
#include "IMUManager.hpp"
#include <tuple>
#include <vector>
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<Frame::IMUFrame> IMUManager::getDataWithinInterval(
ros::Time startTime, ros::Time endTime) {
std::vector<Frame::IMUFrame> 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 <deque>
#include <memory>
#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<Frame::IMUFrame> getDataWithinInterval(ros::Time startTime,
ros::Time endTime);
std::deque<Frame::IMUFrame> 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<Frame::RadarFrame> RadarManager::getDataWithinInterval(
ros::Time startTime, ros::Time endTime) {
std::vector<Frame::RadarFrame> 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 <deque>
#include <memory>
#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<Frame::RadarFrame> getDataWithinInterval(ros::Time startTime,
ros::Time endTime);
std::deque<Frame::RadarFrame> 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 <cstddef>
#include <cstdint>
#include <deque>
#include <utility>
#include <vector>
#include "Preintegration.hpp"
namespace Factor {
class ImuPreintegrationManger {
public:
struct Relation {
int startStateIndex;
int endStateIndex;
};
std::deque<Residuals::Preintegration> imuPreintegration;
std::deque<Relation> 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 <cstddef>
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 <cstddef>
#include <cstdint>
#include <deque>
#include "RadarFrame.hpp"
namespace Factor {
// it presents a graph
class RadarFeatureManager {
public:
struct RadarFrameRelation;
struct PointFeatureRelation;
struct RadarFrameRelation {
uint32_t radarFrameId;
std::deque<uint32_t> featureId;
};
struct PointFeatureRelation {
uint32_t featureId;
std::deque<uint32_t> frameId;
std::deque<Eigen::Vector3d> gyroData;
Eigen::Vector3d pointState;
bool init;
std::deque<Frame::RadarData> measurement;
};
std::deque<RadarFrameRelation> frameRelation;
std::deque<PointFeatureRelation> 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 <cmath>
#include <cstdint>
#include <map>
#include <vector>
#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<RadarData> data;
std::map<uint32_t, RadarData> matchedPoint;
std::map<uint32_t, RadarData> unmatchedPoint;
std::vector<RadarData> 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<uint32_t> frameID{};
};
} // namespace Frame
================================================
FILE: rio/src/frontend/RadarPreprocessor.cpp
================================================
#include "RadarPreprocessor.hpp"
#include <opencv2/core/hal/interface.h>
#include <cmath>
#include <vector>
#include "RadarFrame.hpp"
#include "Utility.hpp"
namespace Frontend {
void RadarPreprocessor::process(std::vector<Frame::RadarData> &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::RadarData> &frame) const {
if (frame.empty()) return;
std::vector<uchar> 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::RadarData> &frame) const {
if (frame.empty()) return;
std::vector<uchar> 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::RadarData> &frame) const {
if (frame.size() <= 1) return;
// TODO use KD-tree
std::vector<uchar> 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::RadarData> &frame) const {
// TODO use KD-tree
if (frame.size() <= 1) return;
std::vector<std::vector<double>> distanceArray;
for (int i = 0; i < frame.size(); i++) {
distanceArray.emplace_back(std::vector<double>());
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<uchar> 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 <memory>
#include <vector>
#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::RadarData> &frame) const;
void addFOVParams(FOVParams param);
void setVelParams(VelParams param);
void setDistanceParams(double distance);
void setSpatialParams(SpatialFilterParams param);
private:
void fovFilter(std::vector<Frame::RadarData> &frame) const;
void velocityFilter(std::vector<Frame::RadarData> &frame) const;
void distanceFilter(std::vector<Frame::RadarData> &frame) const;
void spatialFilter(std::vector<Frame::RadarData> &frame) const;
std::vector<FOVParams> 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 <cmath>
#include <fstream>
#include <vector>
#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<Frame::RadarData> 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<unsigned int> &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<Frame::RadarData> &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<uchar> RadarTracker::searchCorrsponding(
std::vector<Frame::RadarData> &previousPoints,
std::vector<Frame::RadarData> ¤tPoints,
std::vector<Frame::RadarData> &detectedPoints,
std::vector<Frame::RadarData> &undetectedPoints) {
// tracking points
std::vector<uchar> result;
detectedPoints.clear();
undetectedPoints.clear();
std::vector<uchar> undetectedStatus(currentPoints.size(), 1);
std::vector<std::pair<int, double>> 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<uchar> RadarTracker::searchStaticPoints(
std::vector<Frame::RadarData> ¤tPoints, Eigen::Vector3d velocity) {
std::vector<uchar> result;
for (auto data : currentPoints) {
// sqrtInfoGain not used
double sqrtInfoGain;
Eigen::Matrix<double, 2, 2> Sigma;
Eigen::Matrix<double, 3, 2> 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<Frame::RadarData> &points) {
std::vector<Frame::RadarData> detectedPoints;
std::vector<Frame::RadarData> undetectedPoints;
std::vector<Frame::RadarData> previousPoints;
std::vector<uchar> 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 <opencv2/core/hal/interface.h>
#include <cstdint>
#include <set>
#include <vector>
#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<Frame::RadarData> 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<unsigned int> &idToRemove);
Frame::RadarFrame mergeFrame(Eigen::Quaterniond previousRot,
Eigen::Vector3d previousVec,
Eigen::Quaterniond currentRot,
Eigen::Vector3d currentVec);
private:
void initialization(std::vector<Frame::RadarData> &points, ros::Time time);
std::vector<uchar> searchCorrsponding(
std::vector<Frame::RadarData> &previousPoints,
std::vector<Frame::RadarData> ¤tPoints,
std::vector<Frame::RadarData> &detectedPoints,
std::vector<Frame::RadarData> &undetectedPoints);
std::vector<uchar> searchStaticPoints(
std::vector<Frame::RadarData> ¤tPoints, Eigen::Vector3d velocity);
void trackAndDetectPoints(std::vector<Frame::RadarData> &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<TrackingPointsInfo> trackingPoints;
std::vector<TrackingPointsInfo> 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<const Eigen::Quaterniond> quaternionSrc(x);
Eigen::Map<const Eigen::Vector3d> deltaVec(delta);
Eigen::Map<Eigen::Quaterniond> quaternionDes(x_plus_delta);
quaternionDes = (quaternionSrc * MathUtility::deltaQ(deltaVec)).normalized();
return true;
}
bool EigenQuaternionManifold::PlusJacobian(const double *x,
double *jacobian) const {
Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> 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<const Eigen::Quaterniond> quaternionSrc(y);
Eigen::Map<const Eigen::Quaterniond> quaternionDes(x);
Eigen::Map<Eigen::Vector3d> deltaVec(y_minus_x);
deltaVec = MathUtility::vectorQ(quaternionSrc * quaternionDes.inverse());
return true;
}
bool EigenQuaternionManifold::MinusJacobian(const double *x,
double *jacobian) const {
Eigen::Map<Eigen::Matrix<double, 3, 4, Eigen::RowMajor>> 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 <ceres/manifold.h>
#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 <iostream>
#include <utility>
#include "MathUtility.hpp"
// #include <Eigen/src/Geometry/Quaternion.h>
// #include <Eigen/src/Core/Matrix.h>
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<Eigen::Matrix<double, 15, 1>> 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<double, 15, 15> sqrtInfoMatrix =
Eigen::LLT<Eigen::Matrix<double, 15, 15>>(
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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 4, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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<Eigen::Matrix<double, 15, 3, Eigen::RowMajor>> 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 <ceres/sized_cost_function.h>
// #include <Eigen/Eigen>
#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 <MathUtility.hpp>
#include <utility>
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<double, 15, 15> Preintegration::getJacobian() const {
return jacobian;
};
Eigen::Matrix<double, 15, 15> 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 * delta
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
SYMBOL INDEX (218 symbols across 49 files)
FILE: ars548_driver/data_type/RadarConfiguration.hpp
type Driver (line 3) | namespace Driver
type RadarConfiguration (line 5) | struct RadarConfiguration
FILE: ars548_driver/data_type/RadarDetection.hpp
type Driver (line 3) | namespace Driver
type ARS548RadarDetection (line 6) | struct ARS548RadarDetection
type ARS548RadarDetectionList (line 44) | struct ARS548RadarDetectionList
FILE: ars548_driver/data_type/RadarObject.hpp
type Driver (line 4) | namespace Driver
type ARS548RadarObject (line 6) | struct ARS548RadarObject
type ARS548RadarObjectList (line 138) | struct ARS548RadarObjectList
FILE: ars548_driver/data_type/RadarStatus.hpp
type Driver (line 3) | namespace Driver
type ARS548RadarStatus (line 5) | struct ARS548RadarStatus
FILE: ars548_driver/data_type/StatusDef.hpp
type Driver (line 3) | namespace Driver
type ARS548RadarDetectionFlags (line 6) | enum ARS548RadarDetectionFlags
type ARS548RadarDetectionClasses (line 18) | enum ARS548RadarDetectionClasses
type ARS548SyncStatus (line 28) | enum ARS548SyncStatus
type ARS548AlignmentStatus (line 35) | enum ARS548AlignmentStatus
type ARS548ObjectStatus (line 42) | enum ARS548ObjectStatus
type ARS548MovementStatus (line 50) | enum ARS548MovementStatus
type ARS548PositionReference (line 57) | enum ARS548PositionReference
type ARS548ShapeStatus (line 70) | enum ARS548ShapeStatus
type ARS548PlugOrientation (line 78) | enum ARS548PlugOrientation
type ARS548FrequencySlot (line 84) | enum ARS548FrequencySlot
type ARS548CountryCode (line 94) | enum ARS548CountryCode
type ARS548PowerSaveStandstillMode (line 100) | enum ARS548PowerSaveStandstillMode
type ARS548VDYStatus (line 106) | enum ARS548VDYStatus
type ARS548SensorStatus (line 112) | enum ARS548SensorStatus
type ARS548ParameterConfiguration (line 119) | enum ARS548ParameterConfiguration
type ARS548VehicleDirection (line 125) | enum ARS548VehicleDirection
FILE: ars548_driver/data_type/TypeConverter.hpp
function byteArrayToUint8 (line 7) | inline uint8_t byteArrayToUint8(const uint8_t *array) { return *array; }
function byteArrayToInt8 (line 9) | inline int8_t byteArrayToInt8(const uint8_t *array)
function byteArrayToUint16 (line 14) | inline uint16_t byteArrayToUint16(const uint8_t *array)
function byteArrayToInt16 (line 19) | inline int16_t byteArrayToInt16(const uint8_t *array)
function byteArrayToUint32 (line 25) | inline uint32_t byteArrayToUint32(const uint8_t *array)
function byteArrayToInt32 (line 30) | inline int32_t byteArrayToInt32(const uint8_t *array)
function byteArrayToUint64 (line 36) | inline uint64_t byteArrayToUint64(const uint8_t *array)
function byteArrayToInt64 (line 48) | inline int64_t byteArrayToInt64(const uint8_t *array)
function byteArrayToFloat (line 61) | inline float byteArrayToFloat(const uint8_t *array)
function byteArrayToDouble (line 67) | inline double byteArrayToDouble(const uint8_t *array)
FILE: ars548_driver/data_type/VehicleInfo.hpp
type Driver (line 4) | namespace Driver
type VehicleAccLateral (line 6) | struct VehicleAccLateral
type VehicleAccLongitudinal (line 22) | struct VehicleAccLongitudinal
type VehicleCharacteristicSpeed (line 38) | struct VehicleCharacteristicSpeed
type VehicleDrivingDirection (line 48) | struct VehicleDrivingDirection
type VehicleSteeringAngle (line 56) | struct VehicleSteeringAngle
type VehicleVelocity (line 72) | struct VehicleVelocity
type VehicleYawRate (line 86) | struct VehicleYawRate
FILE: ars548_driver/node/ARS548_node.cpp
function main (line 21) | int main(int argc, char **argv)
FILE: ars548_driver/src/ARS548.cpp
type Driver (line 3) | namespace Driver
function ARS548RadarDetectionList (line 92) | ARS548RadarDetectionList &ARS548::getDetectionList() { return detectio...
function ARS548RadarObjectList (line 94) | ARS548RadarObjectList &ARS548::getObjectList() { return objectList; }
function ARS548RadarStatus (line 96) | ARS548RadarStatus &ARS548::getRadarStatus() { return radarStatus; }
FILE: ars548_driver/src/ARS548.hpp
type Driver (line 15) | namespace Driver
class ARS548 (line 17) | class ARS548
method ARS548 (line 20) | ARS548() = default;
method ARS548 (line 28) | ARS548(ARS548 &another) = delete;
method ARS548 (line 29) | ARS548(ARS548 &&another) = delete;
method ARS548 (line 30) | ARS548 &operator=(const ARS548 &another) = delete;
method ARS548 (line 31) | ARS548 &operator=(ARS548 &&another) = delete;
class UdpIO (line 53) | class UdpIO
method UdpIO (line 56) | UdpIO() = default;
method UdpIO (line 58) | UdpIO(UdpIO &another) = delete;
method UdpIO (line 59) | UdpIO(UdpIO &&another) = delete;
method UdpIO (line 60) | UdpIO &operator=(const UdpIO &another) = delete;
method UdpIO (line 61) | UdpIO &operator=(UdpIO &&another) = delete;
FILE: ars548_driver/src/ARS548Coder.cpp
type Driver (line 6) | namespace Driver
function ARS548RadarObjectList (line 8) | ARS548RadarObjectList ARS548Coder::decodeObjectListMessage(
function ARS548RadarObject (line 47) | ARS548RadarObject ARS548Coder::decodeObjectMessage(const uint8_t *data...
function ARS548RadarDetectionList (line 117) | ARS548RadarDetectionList ARS548Coder::decodeDetectionListMessage(
function ARS548RadarDetection (line 173) | ARS548RadarDetection ARS548Coder::decodeDetectionMessage(
function ARS548RadarStatus (line 201) | ARS548RadarStatus ARS548Coder::decodeBasicStatusMessage(
FILE: ars548_driver/src/ARS548Coder.hpp
type Driver (line 10) | namespace Driver
class ARS548Coder (line 12) | class ARS548Coder
method ARS548Coder (line 15) | ARS548Coder() = default;
method ARS548Coder (line 17) | ARS548Coder(ARS548Coder &another) = delete;
method ARS548Coder (line 18) | ARS548Coder(ARS548Coder &&another) = delete;
method ARS548Coder (line 19) | ARS548Coder &operator=(const ARS548Coder &another) = delete;
method ARS548Coder (line 20) | ARS548Coder &operator=(ARS548Coder &&another) = delete;
FILE: ars548_driver/src/UdpIO.cpp
type Driver (line 13) | namespace Driver
FILE: docker/run.py
function ArgsParser (line 9) | def ArgsParser(argv):
function RunEstimator (line 20) | def RunEstimator(dataset, node, conf, round, playSpeed):
function runSingleRound (line 25) | def runSingleRound(dataset, node, conf, round, playSpeed):
function main (line 28) | def main(argv):
FILE: rio/node/rio.cpp
function main (line 10) | int main(int argc, char **argv) {
FILE: rio/node/rosWarper.hpp
type RadarType (line 58) | enum RadarType { ARS548, ColoRadar }
class RIO (line 60) | class RIO {
type runtimeState (line 125) | struct runtimeState {
type exParam (line 135) | struct exParam {
method RIO (line 182) | RIO(RIO &another) = delete;
method RIO (line 183) | RIO(RIO &&another) = delete;
FILE: rio/src/data_manager/IMUManager.cpp
type Data (line 6) | namespace Data {
FILE: rio/src/data_manager/IMUManager.hpp
type Data (line 9) | namespace Data {
class IMUManager (line 11) | class IMUManager {
method IMUManager (line 13) | IMUManager() = default;
method IMUManager (line 16) | IMUManager(IMUManager &another) = delete;
method IMUManager (line 17) | IMUManager(IMUManager &&another) = delete;
method IMUManager (line 18) | IMUManager &operator=(const IMUManager &another) = delete;
method IMUManager (line 19) | IMUManager &operator=(IMUManager &&another) = delete;
FILE: rio/src/data_manager/RadarManager.cpp
type Data (line 3) | namespace Data {
FILE: rio/src/data_manager/RadarManager.hpp
type Data (line 9) | namespace Data {
class RadarManager (line 11) | class RadarManager {
method RadarManager (line 13) | RadarManager() = default;
method RadarManager (line 16) | RadarManager(RadarManager &another) = delete;
method RadarManager (line 17) | RadarManager(RadarManager &&another) = delete;
method RadarManager (line 18) | RadarManager &operator=(const RadarManager &another) = delete;
method RadarManager (line 19) | RadarManager &operator=(RadarManager &&another) = delete;
FILE: rio/src/factor/ImuPreintegrationManager.cpp
type Factor (line 3) | namespace Factor {
FILE: rio/src/factor/ImuPreintegrationManager.hpp
type Factor (line 10) | namespace Factor {
class ImuPreintegrationManger (line 12) | class ImuPreintegrationManger {
type Relation (line 14) | struct Relation {
FILE: rio/src/factor/RadarFeatureManager.cpp
type Factor (line 5) | namespace Factor {
FILE: rio/src/factor/RadarFeatureManager.hpp
type Factor (line 8) | namespace Factor {
class RadarFeatureManager (line 11) | class RadarFeatureManager {
type RadarFrameRelation (line 13) | struct RadarFrameRelation
type PointFeatureRelation (line 14) | struct PointFeatureRelation
type RadarFrameRelation (line 16) | struct RadarFrameRelation {
type PointFeatureRelation (line 21) | struct PointFeatureRelation {
FILE: rio/src/frame_type/IMUFrame.hpp
type Frame (line 6) | namespace Frame {
class IMUFrame (line 7) | class IMUFrame {
method IMUFrame (line 9) | IMUFrame() = default;
method IMUFrame (line 11) | IMUFrame(IMUFrame &another) = default;
method IMUFrame (line 12) | IMUFrame(IMUFrame &&another) = default;
method IMUFrame (line 13) | IMUFrame &operator=(const IMUFrame &another) = default;
method IMUFrame (line 14) | IMUFrame &operator=(IMUFrame &&another) = default;
FILE: rio/src/frame_type/RadarFrame.hpp
type Frame (line 11) | namespace Frame {
type RadarData (line 12) | struct RadarData {
method anglesToXYZ (line 24) | static inline void anglesToXYZ(RadarData &data) {
method xyzToAngles (line 30) | static inline void xyzToAngles(RadarData &data) {
method intensityToRCS (line 36) | static inline void intensityToRCS(RadarData &data) {
method rcsToIntensity (line 41) | static inline void rcsToIntensity(RadarData &data) {
class RadarFrame (line 47) | class RadarFrame {
method RadarFrame (line 49) | RadarFrame() = default;
method RadarFrame (line 51) | RadarFrame(RadarFrame &another) = default;
method RadarFrame (line 52) | RadarFrame(RadarFrame &&another) = default;
method RadarFrame (line 53) | RadarFrame &operator=(const RadarFrame &another) = default;
method RadarFrame (line 54) | RadarFrame &operator=(RadarFrame &&another) = default;
class RCSFrame (line 65) | class RCSFrame {
method RCSFrame (line 67) | RCSFrame() = default;
method RCSFrame (line 69) | RCSFrame(RCSFrame &another) = default;
method RCSFrame (line 70) | RCSFrame(RCSFrame &&another) = default;
method RCSFrame (line 71) | RCSFrame &operator=(const RCSFrame &another) = default;
method RCSFrame (line 72) | RCSFrame &operator=(RCSFrame &&another) = default;
FILE: rio/src/frontend/RadarPreprocessor.cpp
type Frontend (line 11) | namespace Frontend {
FILE: rio/src/frontend/RadarPreprocessor.hpp
type Frontend (line 8) | namespace Frontend {
class RadarPreprocessor (line 9) | class RadarPreprocessor {
type FOVParams (line 11) | struct FOVParams {
type VelParams (line 20) | struct VelParams {
type SpatialFilterParams (line 25) | struct SpatialFilterParams {
method RadarPreprocessor (line 31) | RadarPreprocessor() = default;
method RadarPreprocessor (line 34) | RadarPreprocessor(RadarPreprocessor &another) = delete;
method RadarPreprocessor (line 35) | RadarPreprocessor(RadarPreprocessor &&another) = delete;
method RadarPreprocessor (line 36) | RadarPreprocessor &operator=(const RadarPreprocessor &another) = del...
method RadarPreprocessor (line 37) | RadarPreprocessor &operator=(RadarPreprocessor &&another) = delete;
FILE: rio/src/frontend/RadarTracker.cpp
function pdf (line 12) | static inline double pdf(double x, double mu, double sigma) {
type Frontend (line 16) | namespace Frontend {
FILE: rio/src/frontend/RadarTracker.hpp
type Frontend (line 11) | namespace Frontend {
class RadarTracker (line 12) | class RadarTracker {
type TrackingPointsInfo (line 13) | struct TrackingPointsInfo {
method RadarTracker (line 20) | RadarTracker() = default;
method RadarTracker (line 23) | RadarTracker(RadarTracker &another) = delete;
method RadarTracker (line 24) | RadarTracker(RadarTracker &&another) = delete;
method RadarTracker (line 25) | RadarTracker &operator=(const RadarTracker &another) = delete;
method RadarTracker (line 26) | RadarTracker &operator=(RadarTracker &&another) = delete;
FILE: rio/src/manifold/EigenQuaternionManifold.cpp
type Manifold (line 5) | namespace Manifold {
FILE: rio/src/manifold/EigenQuaternionManifold.hpp
type Manifold (line 7) | namespace Manifold {
class EigenQuaternionManifold (line 8) | class EigenQuaternionManifold : public ceres::Manifold {
method EigenQuaternionManifold (line 10) | EigenQuaternionManifold() = default;
FILE: rio/src/residual/IMUResidual.cpp
type Residuals (line 10) | namespace Residuals {
FILE: rio/src/residual/IMUResidual.hpp
type Residuals (line 8) | namespace Residuals {
class IMUResidual (line 25) | class IMUResidual
FILE: rio/src/residual/Preintegration.cpp
type Residuals (line 6) | namespace Residuals {
FILE: rio/src/residual/Preintegration.hpp
type Residuals (line 6) | namespace Residuals {
class Preintegration (line 7) | class Preintegration {
type IMUData (line 8) | struct IMUData {
method Preintegration (line 20) | Preintegration() = delete;
method Preintegration (line 21) | Preintegration(Preintegration &another) = default;
method Preintegration (line 22) | Preintegration(Preintegration &&another) = default;
method Preintegration (line 23) | Preintegration &operator=(const Preintegration &another) = default;
method Preintegration (line 24) | Preintegration &operator=(Preintegration &&another) = default;
FILE: rio/src/residual/RadarImuVelResidual.cpp
type Residuals (line 9) | namespace Residuals {
FILE: rio/src/residual/RadarImuVelResidual.hpp
type Residuals (line 8) | namespace Residuals {
class RadarImuVelocityResidual (line 20) | class RadarImuVelocityResidual
FILE: rio/src/residual/RadarPointResidual.cpp
type Residuals (line 5) | namespace Residuals {
FILE: rio/src/residual/RadarPointResidual.hpp
type Residuals (line 8) | namespace Residuals {
class RadarPointResidual (line 19) | class RadarPointResidual : public ceres::SizedCostFunction<3, 3, 4, 3,...
FILE: rio/src/state/IMUState.hpp
type State (line 3) | namespace State {
type IMUState (line 6) | struct IMUState {
FILE: rio/src/state/R3State.hpp
type State (line 3) | namespace State {
type R3State (line 6) | struct R3State {
FILE: rio/src/state/RadarPointFeatureState.hpp
type State (line 3) | namespace State {
type RadarPointFeatureState (line 7) | struct RadarPointFeatureState {
FILE: rio/src/state/SE3State.hpp
type State (line 3) | namespace State {
type SE3State (line 6) | struct SE3State {
FILE: rio/src/state/StateManager.hpp
type State (line 12) | namespace State {
class StateManager (line 18) | class StateManager {
FILE: rio/src/utility/MathUtility.hpp
type MathUtility (line 7) | namespace MathUtility {
function skewSymmetric (line 8) | static Eigen::Matrix3d skewSymmetric(const Eigen::Vector3d &rotationVe...
function deltaQ (line 18) | static Eigen::Quaterniond deltaQ(const Eigen::Vector3d &theta) {
function vectorQ (line 28) | static Eigen::Vector3d vectorQ(const Eigen::Quaterniond &quat) {
function leftMultiplyQ (line 39) | static Eigen::Matrix4d leftMultiplyQ(const Eigen::Quaterniond &quat) {
function rightMultiplyQ (line 49) | static Eigen::Matrix4d rightMultiplyQ(const Eigen::Quaterniond &quat) {
FILE: rio/src/utility/TicToc.hpp
class TicToc (line 23) | class TicToc {
method TicToc (line 25) | TicToc() { tic(); }
method tic (line 27) | void tic() { start = std::chrono::system_clock::now(); }
method toc (line 29) | double toc() {
FILE: rio/src/utility/Timestamp.hpp
type Timestamp (line 5) | struct Timestamp {
method Timestamp (line 11) | Timestamp(uint32_t secValue, uint32_t nsecValue)
method Timestamp (line 19) | Timestamp() = default;
method Timestamp (line 21) | Timestamp(Timestamp &another) = default;
method Timestamp (line 22) | Timestamp(Timestamp &&another) = default;
method Timestamp (line 23) | Timestamp &operator=(const Timestamp &another) = default;
method Timestamp (line 24) | Timestamp &operator=(Timestamp &&another) = default;
method toSec (line 50) | double toSec() const { return sec + nsec * 1e-9; }
method toMSec (line 52) | double toMSec() const { return sec * 1e3 + nsec * 1e-6; }
method toUSec (line 54) | double toUSec() const { return sec * 1e6 + nsec * 1e-3; }
method toNSec (line 56) | double toNSec() const { return sec * 1e9 + nsec; }
FILE: rio/src/utility/Utility.hpp
function reduceVector (line 10) | inline void reduceVector(std::vector<T> &vector, std::vector<uchar> stat...
function calDistance (line 17) | inline double calDistance(cv::Point2f &point1, cv::Point2f &point2) {
function calDistance (line 23) | inline double calDistance(cv::Point3f &point1, cv::Point3f &point2) {
function calDistance (line 29) | inline double calDistance(Eigen::Vector3d &point1, Eigen::Vector3d &poin...
Condensed preview — 85 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (228K chars).
[
{
"path": ".gitignore",
"chars": 15,
"preview": "/CMakeLists.txt"
},
{
"path": ".pre-commit-config.yaml",
"chars": 322,
"preview": "repos:\n - repo: https://github.com/cpp-linter/cpp-linter-hooks\n rev: v0.4.0 # Use the ref you want to point at\n "
},
{
"path": "LICENSE",
"chars": 1084,
"preview": "MIT License\n\nCopyright (c) 2024 HKUST Aerial Robotics Group\n\nPermission is hereby granted, free of charge, to any person"
},
{
"path": "README.md",
"chars": 4589,
"preview": "# RIO\r\n\r\nOptimization Based and Point Uncertainty Aware Radar-inertial Odometry for 4D Radar System\r\n\r\n## Prerequisites\r"
},
{
"path": "ars548_driver/CMakeLists.txt",
"chars": 7227,
"preview": "cmake_minimum_required(VERSION 3.0.2)\nproject(ars548_driver)\n\n## Compile as C++11, supported in ROS Kinetic and newer\n# "
},
{
"path": "ars548_driver/data_type/RadarConfiguration.hpp",
"chars": 1457,
"preview": "#pragma once\n#include <cstdint>\nnamespace Driver\n{\nstruct RadarConfiguration\n{\n // -100 to 100 m\n float longitudin"
},
{
"path": "ars548_driver/data_type/RadarDetection.hpp",
"chars": 2911,
"preview": "#pragma once\n#include <cstdint>\nnamespace Driver\n{\n\nstruct ARS548RadarDetection\n{\n // -3.14 to 3.14 in rad\n float "
},
{
"path": "ars548_driver/data_type/RadarObject.hpp",
"chars": 4169,
"preview": "#pragma once\n\n#include <cstdint>\nnamespace Driver\n{\nstruct ARS548RadarObject\n{\n // 0 to 65535, TBD\n uint16_t statu"
},
{
"path": "ars548_driver/data_type/RadarStatus.hpp",
"chars": 1766,
"preview": "#pragma once\n#include <cstdint>\nnamespace Driver\n{\nstruct ARS548RadarStatus\n{\n // -100 to 100 m\n float longitudina"
},
{
"path": "ars548_driver/data_type/StatusDef.hpp",
"chars": 2219,
"preview": "#pragma once\n\nnamespace Driver\n{\n\nenum ARS548RadarDetectionFlags\n{\n VALID_ALL = 0x00,\n INVALID_DISTANCE = 0x01,\n "
},
{
"path": "ars548_driver/data_type/TypeConverter.hpp",
"chars": 3555,
"preview": "#pragma once\n\n#include <netinet/in.h>\n\n#include <cstdint>\n\ninline uint8_t byteArrayToUint8(const uint8_t *array) { retur"
},
{
"path": "ars548_driver/data_type/VehicleInfo.hpp",
"chars": 2245,
"preview": "#pragma once\n\n#include <cstdint>\nnamespace Driver\n{\nstruct VehicleAccLateral\n{\n // unused\n float accelerationLater"
},
{
"path": "ars548_driver/msg/AccelerationLateralCog.msg",
"chars": 247,
"preview": "std_msgs/Header header\n\nfloat32 accelerationLateralErrAmp\nuint8 accelerationLateralErrAmpInvalidFlag\nuint8 qualifierAcce"
},
{
"path": "ars548_driver/msg/AccelerationLongitudinalCog.msg",
"chars": 277,
"preview": "std_msgs/Header header\n\nfloat32 accelerationLongitudinalErrAmp\nuint8 accelerationLongitudinalErrAmpInvalidFlag\nuint8 qua"
},
{
"path": "ars548_driver/msg/CharacteristicSpeed.msg",
"chars": 116,
"preview": "std_msgs/Header header\n\nuint8 characteristicSpeedErrAmp\nuint8 qualifierCharacteristicSpeed\nuint8 characteristicSpeed"
},
{
"path": "ars548_driver/msg/DetectionList.msg",
"chars": 814,
"preview": "uint16 serviceID\nuint16 MethodID\nuint32 dataLength\nuint16 clientID\nuint16 sessionID\nuint8 protocolVersion\nuint8 interfac"
},
{
"path": "ars548_driver/msg/Detections.msg",
"chars": 361,
"preview": "std_msgs/Header header\n \nfloat32 azimuthAngle\nfloat32 azimuthSTD\nfloat32 elevationAngle\nfloat32 elevationSTD\nfloat32 ra"
},
{
"path": "ars548_driver/msg/DrivingDirection.msg",
"chars": 90,
"preview": "std_msgs/Header header\n\nuint8 drivingDirectionUnconfirmed\nuint8 drivingDirectionConfirmed\n"
},
{
"path": "ars548_driver/msg/ObjectList.msg",
"chars": 392,
"preview": "uint16 serviceID\nuint16 methodID\nuint32 dataLength\nuint16 clientID\nuint16 sessionID\nuint8 protocolVersion\nuint8 interfac"
},
{
"path": "ars548_driver/msg/Objects.msg",
"chars": 1777,
"preview": "std_msgs/Header header\n \nuint16 statusSensor\nuint32 id\nuint16 age\nuint8 statusMeasurement\nuint8 statusMovement\nuint16"
},
{
"path": "ars548_driver/msg/RadarBasicStatus.msg",
"chars": 599,
"preview": "std_msgs/Header header\n\nfloat32 longitudinal\nfloat32 lateral\nfloat32 vertical\nfloat32 yaw\nfloat32 pitch\nuint8 plugOrient"
},
{
"path": "ars548_driver/msg/SteeringAngleFrontAxle.msg",
"chars": 263,
"preview": "std_msgs/Header header\n\nuint8 qualifierSteeringAngleFrontAxle\nfloat32 steeringAngleFrontAxleErrAmp\nuint8 steeringAngleFr"
},
{
"path": "ars548_driver/msg/VelocityVehicle.msg",
"chars": 186,
"preview": "std_msgs/Header header\n\nuint8 statusVelocityNearStandstill\nuint8 qualifierVelocityVehicle\nuint8 velocityVehicleEventData"
},
{
"path": "ars548_driver/msg/YawRate.msg",
"chars": 172,
"preview": "std_msgs/Header header\n\nfloat32 yawRateErrAmp\nuint8 yawRateErrAmpInvalidFlag\nuint8 qualifierYawRate\nfloat32 yawRate\nuint"
},
{
"path": "ars548_driver/node/ARS548_node.cpp",
"chars": 9508,
"preview": "#include <pcl/PCLPointCloud2.h>\n#include <pcl/point_cloud.h>\n#include <pcl_conversions/pcl_conversions.h>\n\n#include <cma"
},
{
"path": "ars548_driver/package.xml",
"chars": 2623,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>ars548_driver</name>\n <version>0.0.0</version>\n <description>The ar"
},
{
"path": "ars548_driver/src/ARS548.cpp",
"chars": 2688,
"preview": "#include \"ARS548.hpp\"\n\nnamespace Driver\n{\nARS548::ARS548(std::string device,\n std::string multicastIP,\n "
},
{
"path": "ars548_driver/src/ARS548.hpp",
"chars": 2664,
"preview": "#pragma once\n#include <pcap/pcap.h>\n\n#include <cerrno>\n#include <cstddef>\n#include <cstdint>\n#include <cstdio>\n#include "
},
{
"path": "ars548_driver/src/ARS548Coder.cpp",
"chars": 11504,
"preview": "#include <cstddef>\n#include <cstdint>\n\n#include \"ARS548Coder.hpp\"\n\nnamespace Driver\n{\nARS548RadarObjectList ARS548Coder:"
},
{
"path": "ars548_driver/src/ARS548Coder.hpp",
"chars": 996,
"preview": "#pragma once\n#include <cstdint>\n#include <iostream>\n#include \"../data_type/RadarDetection.hpp\"\n#include \"../data_type/Ra"
},
{
"path": "ars548_driver/src/UdpIO.cpp",
"chars": 5526,
"preview": "#include <linux/if_ether.h>\n#include <linux/ip.h>\n#include <linux/udp.h>\n#include <netinet/in.h>\n#include <pcap/pcap.h>\n"
},
{
"path": "dataset/.gitignore",
"chars": 10,
"preview": "*/**/*.pcd"
},
{
"path": "docker/Dockerfile",
"chars": 4649,
"preview": "FROM ubuntu:20.04\n\n# Environment variables\nENV DEBIAN_FRONTEND=noninteractive\nENV TZ=Asia/Beijing\nENV WS=/root/ws\nENV SH"
},
{
"path": "docker/docker.sh",
"chars": 807,
"preview": "#!/bin/bash\nSCRIPT_DIR=$(cd $(dirname $0); pwd)\n\nfunction build() {\n docker build \\\n -t rio \\\n -f $SCRIPT_DIR/D"
},
{
"path": "docker/run.py",
"chars": 1826,
"preview": "#!/bin/env python3\nimport argparse\nimport shutil\nimport sys\nimport os\n\nBASE_DIR = os.path.dirname(os.path.dirname(os.pat"
},
{
"path": "rio/CMakeLists.txt",
"chars": 1014,
"preview": "cmake_minimum_required(VERSION 3.0.2)\nproject(rio)\n\nset(CMAKE_BUILD_TYPE \"Release\")\nset(CMAKE_CXX_STANDARD 17)\nset(CMAKE"
},
{
"path": "rio/config/RIO.rviz",
"chars": 8736,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "rio/config/ars548.yaml",
"chars": 742,
"preview": "# Algorithm configuration\nuseWeightedResiduals: true\n\nuseDopplerResidual: true\nusePoint2PointResidual: true\nradarType: 0"
},
{
"path": "rio/config/coloradar.yaml",
"chars": 807,
"preview": "# Algorithm configuration\nuseWeightedResiduals: true\n\nuseDopplerResidual: true\nusePoint2PointResidual: true\nradarType: 1"
},
{
"path": "rio/launch/TestRadar.launch",
"chars": 795,
"preview": "<launch>\n <!-- Radar Node -->\n <arg name=\"nodeName\" default=\"rio\"/>\n\n <!-- Test param -->\n <arg name=\"round\""
},
{
"path": "rio/node/rio.cpp",
"chars": 347,
"preview": "#include \"rosWarper.hpp\"\n\n#ifdef DEBUG\n#define BACKWARD_HAS_BFD 1\n#define BACKWARD_HAS_DW 1\n#include <backward.hpp>\nback"
},
{
"path": "rio/node/rosWarper.cpp",
"chars": 39065,
"preview": "#include \"rosWarper.hpp\"\n\nRIO::RIO(ros::NodeHandle &nh) {\n initROS(nh);\n initRIO();\n}\n\nRIO::~RIO() {}\n\nvoid RIO::initR"
},
{
"path": "rio/node/rosWarper.hpp",
"chars": 4700,
"preview": "#pragma once\n\n// Std\n#include <memory>\n#include <string>\n\n// ROS\n#include \"geometry_msgs/PoseStamped.h\"\n#include \"nav_ms"
},
{
"path": "rio/package.xml",
"chars": 1046,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>rio</name>\n <version>0.0.0</version>\n <description>The rio package<"
},
{
"path": "rio/src/data_manager/IMUManager.cpp",
"chars": 944,
"preview": "#include \"IMUManager.hpp\"\n\n#include <tuple>\n#include <vector>\n\nnamespace Data {\nvoid IMUManager::push(Frame::IMUFrame fr"
},
{
"path": "rio/src/data_manager/IMUManager.hpp",
"chars": 800,
"preview": "#pragma once\n\n#include <deque>\n#include <memory>\n\n#include \"../frame_type/IMUFrame.hpp\"\n#include \"ros/time.h\"\n\nnamespace"
},
{
"path": "rio/src/data_manager/RadarManager.cpp",
"chars": 928,
"preview": "#include \"RadarManager.hpp\"\n\nnamespace Data {\nvoid RadarManager::push(Frame::RadarFrame frame) { data.emplace_back(frame"
},
{
"path": "rio/src/data_manager/RadarManager.hpp",
"chars": 832,
"preview": "#pragma once\n\n#include <deque>\n#include <memory>\n\n#include \"../frame_type/RadarFrame.hpp\"\n#include \"ros/time.h\"\n\nnamespa"
},
{
"path": "rio/src/data_manager/readme.md",
"chars": 503,
"preview": "## DataManager\n### Workflow\nAll managers are wrappers, we keep this module just for record all resource. \n\n## IMUManager"
},
{
"path": "rio/src/factor/ImuPreintegrationManager.cpp",
"chars": 700,
"preview": "#include \"ImuPreintegrationManager.hpp\"\n\nnamespace Factor {\nvoid ImuPreintegrationManger::pushBack(Residuals::Preintegra"
},
{
"path": "rio/src/factor/ImuPreintegrationManager.hpp",
"chars": 543,
"preview": "#pragma once\n#include <cstddef>\n#include <cstdint>\n#include <deque>\n#include <utility>\n#include <vector>\n\n#include \"Prei"
},
{
"path": "rio/src/factor/RadarFeatureManager.cpp",
"chars": 3929,
"preview": "#include \"RadarFeatureManager.hpp\"\n\n#include <cstddef>\n\nnamespace Factor {\nvoid RadarFeatureManager::pushBack(Frame::Rad"
},
{
"path": "rio/src/factor/RadarFeatureManager.hpp",
"chars": 1037,
"preview": "#pragma once\n#include <cstddef>\n#include <cstdint>\n#include <deque>\n\n#include \"RadarFrame.hpp\"\n\nnamespace Factor {\n\n// i"
},
{
"path": "rio/src/frame_type/IMUFrame.hpp",
"chars": 516,
"preview": "#pragma once\n\n#include \"eigen3/Eigen/Eigen\"\n#include \"ros/time.h\"\n\nnamespace Frame {\nclass IMUFrame {\n public:\n IMUFram"
},
{
"path": "rio/src/frame_type/RadarFrame.hpp",
"chars": 2128,
"preview": "#pragma once\n\n#include <cmath>\n#include <cstdint>\n#include <map>\n#include <vector>\n\n#include \"eigen3/Eigen/Eigen\"\n#inclu"
},
{
"path": "rio/src/frontend/RadarPreprocessor.cpp",
"chars": 4032,
"preview": "#include \"RadarPreprocessor.hpp\"\n\n#include <opencv2/core/hal/interface.h>\n\n#include <cmath>\n#include <vector>\n\n#include "
},
{
"path": "rio/src/frontend/RadarPreprocessor.hpp",
"chars": 1567,
"preview": "#pragma once\n\n#include <memory>\n#include <vector>\n\n#include \"RadarFrame.hpp\"\n\nnamespace Frontend {\nclass RadarPreprocess"
},
{
"path": "rio/src/frontend/RadarTracker.cpp",
"chars": 11738,
"preview": "#include \"RadarTracker.hpp\"\n\n#include <cmath>\n#include <fstream>\n#include <vector>\n\n#include \"RadarFrame.hpp\"\n#include \""
},
{
"path": "rio/src/frontend/RadarTracker.hpp",
"chars": 2859,
"preview": "#pragma once\n#include <opencv2/core/hal/interface.h>\n\n#include <cstdint>\n#include <set>\n#include <vector>\n\n#include \"Rad"
},
{
"path": "rio/src/frontend/readme.md",
"chars": 509,
"preview": "## Visual Tracker\n\n### Workflow\n```\nif (init):\n featureDetection();\n undistortion();\nelse:\n trackPrediction();\n"
},
{
"path": "rio/src/manifold/EigenQuaternionManifold.cpp",
"chars": 1743,
"preview": "#include \"EigenQuaternionManifold.hpp\"\n\n#include \"MathUtility.hpp\"\n\nnamespace Manifold {\nint EigenQuaternionManifold::Am"
},
{
"path": "rio/src/manifold/EigenQuaternionManifold.hpp",
"chars": 654,
"preview": "#pragma once\n\n#include <ceres/manifold.h>\n\n#include \"Eigen/Eigen\"\n\nnamespace Manifold {\nclass EigenQuaternionManifold : "
},
{
"path": "rio/src/residual/IMUResidual.cpp",
"chars": 9632,
"preview": "#include \"IMUResidual.hpp\"\n\n#include <iostream>\n#include <utility>\n\n#include \"MathUtility.hpp\"\n// #include <Eigen/src/Ge"
},
{
"path": "rio/src/residual/IMUResidual.hpp",
"chars": 1031,
"preview": "#pragma once\n#include <ceres/sized_cost_function.h>\n\n// #include <Eigen/Eigen>\n\n#include \"Preintegration.hpp\"\n\nnamespace"
},
{
"path": "rio/src/residual/Preintegration.cpp",
"chars": 8768,
"preview": "#include \"Preintegration.hpp\"\n\n#include <MathUtility.hpp>\n#include <utility>\n\nnamespace Residuals {\nPreintegration::Prei"
},
{
"path": "rio/src/residual/Preintegration.hpp",
"chars": 2037,
"preview": "#pragma once\n\n#include <Eigen/Eigen>\n#include <vector>\n\nnamespace Residuals {\nclass Preintegration {\n struct IMUData {\n"
},
{
"path": "rio/src/residual/RadarImuVelResidual.cpp",
"chars": 6240,
"preview": "\n#include \"RadarImuVelResidual.hpp\"\n\n#include <math.h>\n#include <ros/ros.h>\n\n#include \"MathUtility.hpp\"\n\nnamespace Resid"
},
{
"path": "rio/src/residual/RadarImuVelResidual.hpp",
"chars": 1254,
"preview": "#pragma once\n#include <ceres/sized_cost_function.h>\n\n#include \"Eigen/Dense\"\n#include \"Eigen/Eigen\"\n#include \"ceres/ceres"
},
{
"path": "rio/src/residual/RadarPointResidual.cpp",
"chars": 5436,
"preview": "#include \"RadarPointResidual.hpp\"\n\n#include \"MathUtility.hpp\"\n\nnamespace Residuals {\n\nRadarPointResidual::RadarPointResi"
},
{
"path": "rio/src/residual/RadarPointResidual.hpp",
"chars": 1372,
"preview": "#pragma once\n\n#include <ceres/ceres.h>\n#include <ceres/sized_cost_function.h>\n\n#include \"../utility/MathUtility.hpp\"\n\nna"
},
{
"path": "rio/src/residual/readme.md",
"chars": 100,
"preview": "## Residual\n\n[Projection Residual](./doc/ProjectionResidual.md)\n[IMU Residual](./doc/IMUResidual.md)"
},
{
"path": "rio/src/state/IMUState.hpp",
"chars": 241,
"preview": "#pragma once\n\nnamespace State {\nconstexpr int SIZE_IMU_STATE = 9;\n\nstruct IMUState {\n // Vx,Vy,Vz\n double velocityPara"
},
{
"path": "rio/src/state/R3State.hpp",
"chars": 150,
"preview": "#pragma once\n\nnamespace State {\nconstexpr int SIZE_R3_STATE = 3;\n\nstruct R3State {\n // Px,Py,Pz\n double positionParams"
},
{
"path": "rio/src/state/RadarPointFeatureState.hpp",
"chars": 185,
"preview": "#pragma once\n\nnamespace State {\n\nconstexpr int SIZE_RADAR_POINT_FEATURE_STATE = 2;\n\nstruct RadarPointFeatureState {\n do"
},
{
"path": "rio/src/state/SE3State.hpp",
"chars": 197,
"preview": "#pragma once\n\nnamespace State {\nconstexpr int SIZE_SE3_STATE = 7;\n\nstruct SE3State {\n // Px,Py,Pz\n double positionPara"
},
{
"path": "rio/src/state/StateManager.cpp",
"chars": 0,
"preview": ""
},
{
"path": "rio/src/state/StateManager.hpp",
"chars": 1069,
"preview": "#pragma once\n\n#include <array>\n#include <cstddef>\n#include <deque>\n\n#include \"IMUState.hpp\"\n#include \"R3State.hpp\"\n#incl"
},
{
"path": "rio/src/utility/MathUtility.hpp",
"chars": 1842,
"preview": "#pragma once\n\n#include <cmath>\n\n#include \"Eigen/Eigen\"\n\nnamespace MathUtility {\nstatic Eigen::Matrix3d skewSymmetric(con"
},
{
"path": "rio/src/utility/TicToc.hpp",
"chars": 677,
"preview": "/**\n * @file tic_toc.hpp\n * @author NLC (qhuangag@connect.ust.hk)\n * @brief Adopted from VINS\n * @version 0.1\n * @date 2"
},
{
"path": "rio/src/utility/Timestamp.hpp",
"chars": 1583,
"preview": "#pragma once\n\n#include <cstdint>\n\nstruct Timestamp {\n static constexpr int MAX_NSEC_VALUE = 1000000000;\n\n uint32_t sec"
},
{
"path": "rio/src/utility/Utility.hpp",
"chars": 1162,
"preview": "#pragma once\n#include <Eigen/Eigen>\n#include <cmath>\n#include <vector>\n\n#include \"opencv2/core/hal/interface.h\"\n#include"
}
]
// ... and 4 more files (download for full content)
About this extraction
This page contains the full source code of the HKUST-Aerial-Robotics/RIO GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 85 files (45.6 MB), approximately 59.9k tokens, and a symbol index with 218 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.