feat(localization): add nerf_based_localizer

launch/tier4_localization_launch/launch/pose_twist_estimator/nerf_based_localizer.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <group>
+    <push-ros-namespace namespace="nerf_based_localizer"/>
+    <group>
+      <include file="$(find-pkg-share nerf_based_localizer)/launch/nerf_based_localizer.launch.xml">
+        <arg name="input_image" value="/sensing/camera/traffic_light/image_raw"/>
+        <arg name="input_pose" value="/localization/pose_twist_fusion_filter/biased_pose_with_covariance"/>
+        <arg name="output_image" value="/localization/pose_estimator/nerf_image"/>
+        <arg name="output_pose_with_covariance" value="/localization/pose_estimator/pose_with_covariance"/>
+        <arg name="param_file" value="$(find-pkg-share nerf_based_localizer)/config/nerf_based_localizer.param.yaml"/>
+      </include>
+    </group>
+  </group>
+</launch>

launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml

@@ -145,4 +145,26 @@
       <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"/>
     </group>
   </group>
+
+  <!-- NeRF Based Localizer (as pose estimator) -->
+  <group if="$(eval &quot;'$(var pose_source)'=='nerf'&quot;)">
+    <group>
+      <push-ros-namespace namespace="pose_estimator"/>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/nerf_based_localizer.launch.xml"/>
+    </group>
+
+    <group>
+      <push-ros-namespace namespace="util"/>
+      <include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
+        <arg name="ndt_enabled" value="false"/>
+        <arg name="gnss_enabled" value="false"/>
+        <arg name="ekf_enabled" value="true"/>
+        <arg name="yabloc_enabled" value="false"/>
+        <arg name="stop_check_enabled" value="$(var stop_check_enabled)"/>
+        <arg name="config_file" value="$(var pose_initializer_param_path)"/>
+        <arg name="sub_gnss_pose_cov" value="/sensing/gnss/pose_with_covariance"/>
+      </include>
+      <include file="$(find-pkg-share tier4_localization_launch)/launch/util/util.launch.py"/>
+    </group>
+  </group>
 </launch>

launch/tier4_localization_launch/package.xml

@@ -23,6 +23,7 @@
   <exec_depend>gyro_odometer</exec_depend>
   <exec_depend>landmark_tf_caster</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
+  <exec_depend>nerf_based_localizer</exec_depend>
   <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pose_initializer</exec_depend>
   <exec_depend>topic_tools</exec_depend>

localization/nerf_based_localizer/CMakeLists.txt

@@ -0,0 +1,50 @@
+cmake_minimum_required(VERSION 3.14)
+project(nerf_based_localizer)
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+find_package(CUDA REQUIRED)
+find_package(CUDAToolkit REQUIRED)
+
+if(NOT ${CUDA_FOUND})
+  message(WARNING "cuda is not found, so the nerf_based_localizer package won't be built.")
+  return()
+endif()
+
+# libtorch
+set(CMAKE_PREFIX_PATH ${DCMAKE_PREFIX_PATH}$ ${CMAKE_SOURCE_DIR}/external/libtorch/)
+find_package(Torch PATHS ${CMAKE_SOURCE_DIR}/external/libtorch NO_DEFAULT_PATH)
+
+# skip if libtorch is not found
+if(NOT Torch_FOUND)
+  return()
+endif()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS} -w")
+link_directories(${CMAKE_SOURCE_DIR}/external/libtorch/lib/)
+include_directories(${TORCH_INCLUDE_DIRS})
+
+# OpenCV
+find_package(OpenCV REQUIRED)
+link_directories(${OpenCV_LIBRARIES})
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+file(GLOB_RECURSE SRC_ALL
+  src/*.cpp
+  src/*.cu)
+
+ament_auto_add_executable(nerf_based_localizer ${SRC_ALL})
+ament_export_dependencies(CUDA)
+target_link_libraries(nerf_based_localizer
+  ${TORCH_LIBRARIES}
+  ${TORCH_CUDA_LIBRARIES}
+  ${OpenCV_LIBS}
+  stdc++fs
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

localization/nerf_based_localizer/README.md

@@ -0,0 +1,121 @@
+# NeRF Based Localizer
+
+NeRFBasedLocalizer is a vision-based localization package.
+
+![example_of_result](./doc_image/example_of_result.png)
+
+## Node diagram
+
+![node diagram](./doc_image/node_diagram.drawio.svg)
+
+## Inputs / Outputs
+
+### Input
+
+| Name            | Type                                            | Description                      |
+| :-------------- | :---------------------------------------------- | :------------------------------- |
+| `~/input/pose`  | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. |
+| `~/input/image` | `sensor_msgs::msg::Image`                       | Camera Image                     |
+
+### Output
+
+| Name                            | Type                                            | Description                    |
+| :------------------------------ | :---------------------------------------------- | :----------------------------- |
+| `~/output/pose`                 | `geometry_msgs::msg::PoseStamped`               | estimated pose                 |
+| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance |
+| `~/output/score`                | `std_msgs::msg::Float32`                        | estimated score of nerf        |
+| `~/output/image`                | `sensor_msgs::msg::Image`                       | estimated image of nerf        |
+
+## How to build
+
+Download libtorch and extract it to the `nerf_based_localizer/external` directory.
+
+For example,
+
+```bash
+cd nerf_based_localizer/external
+wget https://download.pytorch.org/libtorch/cu117/libtorch-cxx11-abi-shared-with-deps-1.13.1%2Bcu117.zip
+unzip ./libtorch-cxx11-abi-shared-with-deps-1.13.1+cu117.zip
+```
+
+If libtorch is prepared, `nerf_based_localizer` can be built as a normal package.
+Otherwise, building `nerf_based_localizer` will be skipped.
+
+## How to launch
+
+Set the train result directory to the parameter `train_result_dir` in the `nerf_based_localizer/config/nerf_based_localizer.param.yaml`.
+
+When launching Autoware, set `nerf` for `pose_source`.
+
+```bash
+ros2 launch autoware_launch ... \
+    pose_source:=nerf \
+    ...
+```
+
+For example, to run `logging_simulator`, the command is as follows.
+
+```bash
+ros2 launch autoware_launch logging_simulator.launch.xml \
+    map_path:=/path/to/map \
+    pose_source:=nerf \
+    vehicle_model:=sample_vehicle \
+    sensor_model:=awsim_sensor_kit \
+    perception:=false \
+    planning:=false \
+    control:=false
+```
+
+[This trained weights](https://drive.google.com/file/d/1w4hLw7aJ_o6OM8XCCXyNPZTGIy4ah9aZ/view?usp=sharing) and [this rosbag data](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) can be used as sample data.
+
+## How to train
+
+### Prepare training data
+
+Use `prepare_data.py`.
+
+```bash
+python3 prepare_data.py /path/to/rosbag /path/to/prepared_data/
+```
+
+The rosbag must contain the following topics.
+
+| Topic name                                                           | Message type                                    | Description                     |
+| :------------------------------------------------------------------- | :---------------------------------------------- | :------------------------------ |
+| `/tf_static`                                                         | `tf2_msgs::msg::TFMessage`                      | tf_static                       |
+| `/localization/pose_twist_fusion_filter/biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction |
+| `/sensing/camera/traffic_light/image_raw`                            | `sensor_msgs::msg::Image`                       | Camera Image                    |
+| `/sensing/camera/traffic_light/camera_info`                          | `sensor_msgs::msg::CameraInfo`                  | Camera Info                     |
+
+For example, the following rosbag obtained from AWSIM can be used.
+
+<https://drive.google.com/file/d/1JM8SgrqxZevyS9thnNMkMx8u8JF2oKhu/view?usp=drive_link>
+
+### Execute training
+
+Use training_tool.
+
+```bash
+cd nerf_based_localizer/training_tool/script
+./build_and_exec_training.sh /path/to/result_dir/ /path/to/prepared_data/
+```
+
+Set the train result directory to the parameter `train_result_dir` in the `nerf_based_localizer/config/nerf_based_localizer.param.yaml`.
+
+## Principle
+
+[NeRF](https://www.matthewtancik.com/nerf), standing for Neural Radiance Fields, presents a novel approach to synthesize novel views of a scene by leveraging the power of neural networks. It was introduced with an aim to handle the challenges of view synthesis, which includes creating novel, previously unseen views of a 3D scene given a sparse set of input photographs.
+
+Training Phase: The model is trained with a set of 2D images of a 3D scene and their corresponding camera parameters (position and orientation). The neural network learns to predict the color and transparency of rays cast through the scene, effectively learning a representation of the 3D scene. The objective is to minimize the difference between the rendered images and the input images.
+
+Inference Phase: After training, NeRF synthesizes novel views of the scene by sampling and summing the colors of volumetric points along the rays cast from a new camera viewpoint. The synthesized images exhibit high-quality view synthesis even under significant viewpoint changes.
+
+### Application in Localization
+
+Implementing NeRF for localization involves utilizing the learned 3D scene representation to estimate the position and orientation of a camera (or observer) in the scene. By comparing the synthesized views and the actual camera view, the algorithm iteratively refines the estimated camera parameters to minimize the difference between the rendered and actual views.
+
+This approach unlocks the potential to achieve accurate and robust self-localization in various environments, allowing devices and robots to comprehend their position and orientation within a previously learned 3D space.
+
+## Acknowledgement
+
+The code for this package is based on [F2-NeRF](https://github.com/Totoro97/f2-nerf) with significant code changes.

localization/nerf_based_localizer/config/nerf_based_localizer.param.yaml

@@ -0,0 +1,22 @@
+/**:
+  ros__parameters:
+    train_result_dir: "/path/to/train_result_dir"
+    optimization_mode: 1  # 0: random_search, 1: differential
+
+    # if optimization_mode is 0, the following parameters are used
+    particle_num: 50
+    render_pixel_num: 128
+    noise_position_x: 0.10  # [m]
+    noise_position_y: 0.10  # [m]
+    noise_position_z: 0.05  # [m]
+    noise_rotation_x: 0.05  # [deg]
+    noise_rotation_y: 0.05  # [deg]
+    noise_rotation_z: 0.10  # [deg]
+
+    # if optimization_mode is 1, the following parameters are used
+    iteration_num: 1
+    learning_rate: 0.0001
+
+    # The following parameters are used in both optimization_mode 0 and 1
+    output_covariance: 0.000225
+    resize_factor: 20

localization/nerf_based_localizer/external/.gitignore

@@ -0,0 +1,2 @@
+*
+!.gitignore

localization/nerf_based_localizer/launch/nerf_based_localizer.launch.xml

@@ -0,0 +1,29 @@
+<launch>
+  <arg name="param_file" default="$(find-pkg-share nerf_based_localizer)/config/nerf_based_localizer.param.yaml"/>
+  <arg name="node_name" default="nerf_based_localizer"/>
+
+  <!-- Topics -->
+  <arg name="input_pose" default="~/input/pose"/>
+  <arg name="input_image" default="~/input/image"/>
+  <arg name="service_optimize_pose" default="~/service/optimize_pose"/>
+  <arg name="service_trigger_node" default="~/service/trigger_node"/>
+
+  <arg name="output_pose" default="~/output/pose"/>
+  <arg name="output_pose_with_covariance" default="~/output/pose_with_covariance"/>
+  <arg name="output_score" default="~/output/score"/>
+  <arg name="output_image" default="~/output/image"/>
+
+  <node pkg="nerf_based_localizer" exec="nerf_based_localizer" name="$(var node_name)" output="log" args="--ros-args --log-level INFO">
+    <remap from="~/input/pose" to="$(var input_pose)"/>
+    <remap from="~/input/image" to="$(var input_image)"/>
+    <remap from="~/service/optimize_pose" to="$(var service_optimize_pose)"/>
+    <remap from="~/service/trigger_node" to="$(var service_trigger_node)"/>
+
+    <remap from="~/output/pose" to="$(var output_pose)"/>
+    <remap from="~/output/pose_with_covariance" to="$(var output_pose_with_covariance)"/>
+    <remap from="~/output/score" to="$(var output_score)"/>
+    <remap from="~/output/image" to="$(var output_image)"/>
+
+    <param from="$(var param_file)"/>
+  </node>
+</launch>

localization/nerf_based_localizer/package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nerf_based_localizer</name>
+  <version>0.1.0</version>
+  <description>The nerf_based_localizer package</description>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Shintaro Sakoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_map_msgs</depend>
+  <depend>diagnostic_msgs</depend>
+  <depend>fmt</depend>
+  <depend>geometry_msgs</depend>
+  <depend>libpcl-all-dev</depend>
+  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_sensor_msgs</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_debug_msgs</depend>
+  <depend>tier4_localization_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <test_depend>ament_cmake_cppcheck</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>