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