diff --git a/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt b/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt
index abb003dd..6bd3c480 100644
--- a/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt
+++ b/calibrators/marker_radar_lidar_calibrator/CMakeLists.txt
@@ -3,32 +3,25 @@ cmake_minimum_required(VERSION 3.5)
 project(marker_radar_lidar_calibrator)
 
 find_package(autoware_cmake REQUIRED)
-
 autoware_package()
 
-ament_python_install_package(${PROJECT_NAME})
+find_package(Ceres REQUIRED)
 
-ament_export_include_directories(
-  include
-  ${OpenCV_INCLUDE_DIRS}
-)
+ament_python_install_package(${PROJECT_NAME})
 
 ament_auto_add_executable(marker_radar_lidar_calibrator
   src/marker_radar_lidar_calibrator.cpp
+  src/transformation_estimator.cpp
   src/track.cpp
   src/main.cpp
 )
 
 target_link_libraries(marker_radar_lidar_calibrator
-  ${OpenCV_LIBS}
+  ${CERES_LIBRARIES}
 )
 
 install(PROGRAMS
   scripts/calibrator_ui_node.py
-  DESTINATION lib/${PROJECT_NAME}
-)
-
-install(PROGRAMS
   scripts/metrics_plotter_node.py
   DESTINATION lib/${PROJECT_NAME}
 )
diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp
index d7c9fd82..c9820956 100644
--- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp
+++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp
@@ -24,6 +24,7 @@
 #include <std_srvs/srv/empty.hpp>
 
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <radar_msgs/msg/radar_scan.hpp>
 #include <radar_msgs/msg/radar_tracks.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <std_msgs/msg/float32_multi_array.hpp>
@@ -40,7 +41,6 @@
 
 #include <algorithm>
 #include <cstdint>
-#include <iostream>
 #include <limits>
 #include <memory>
 #include <mutex>
@@ -53,11 +53,20 @@
 namespace marker_radar_lidar_calibrator
 {
 
+struct TransformationResult
+{
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs;
+  pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs;
+  Eigen::Isometry3d calibrated_radar_to_lidar_transformation;
+};
+
 class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
 {
 public:
-  using PointType = pcl::PointXYZ;
   using index_t = std::uint32_t;
+  enum class TransformationType { svd_2d, yaw_only_rotation_2d, svd_3d, zero_roll_3d };
+
+  enum class MsgType { radar_tracks, radar_scan, radar_cloud };
 
   explicit ExtrinsicReflectorBasedCalibrator(const rclcpp::NodeOptions & options);
 
@@ -85,28 +94,39 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     const std::shared_ptr<std_srvs::srv::Empty::Response> response);
 
   void lidarCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
-  void radarCallback(const radar_msgs::msg::RadarTracks::SharedPtr msg);
 
-  std::vector<Eigen::Vector3d> extractReflectors(
+  void radarTracksCallback(const radar_msgs::msg::RadarTracks::SharedPtr msg);
+
+  void radarScanCallback(const radar_msgs::msg::RadarScan::SharedPtr msg);
+
+  void radarCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);
+
+  template <typename RadarMsgType>
+  pcl::PointCloud<common_types::PointType>::Ptr extractRadarPointcloud(
+    const std::shared_ptr<RadarMsgType> & msg);
+
+  std::vector<Eigen::Vector3d> extractLidarReflectors(
     const sensor_msgs::msg::PointCloud2::SharedPtr msg);
-  std::vector<Eigen::Vector3d> extractReflectors(const radar_msgs::msg::RadarTracks::SharedPtr msg);
+  std::vector<Eigen::Vector3d> extractRadarReflectors(
+    pcl::PointCloud<common_types::PointType>::Ptr radar_pointcloud_ptr);
 
   void extractBackgroundModel(
-    const pcl::PointCloud<PointType>::Ptr & sensor_pointcloud,
+    const pcl::PointCloud<common_types::PointType>::Ptr & sensor_pointcloud,
     const std_msgs::msg::Header & current_header, std_msgs::msg::Header & last_updated_header,
     std_msgs::msg::Header & first_header, BackgroundModel & background_model);
 
   void extractForegroundPoints(
-    const pcl::PointCloud<PointType>::Ptr & sensor_pointcloud,
+    const pcl::PointCloud<common_types::PointType>::Ptr & sensor_pointcloud,
     const BackgroundModel & background_model, bool use_ransac,
-    pcl::PointCloud<PointType>::Ptr & foreground_points, Eigen::Vector4f & ground_model);
+    pcl::PointCloud<common_types::PointType>::Ptr & foreground_points,
+    Eigen::Vector4f & ground_model);
 
-  std::vector<pcl::PointCloud<PointType>::Ptr> extractClusters(
-    const pcl::PointCloud<PointType>::Ptr & foreground_pointcloud,
+  std::vector<pcl::PointCloud<common_types::PointType>::Ptr> extractClusters(
+    const pcl::PointCloud<common_types::PointType>::Ptr & foreground_pointcloud,
     const double cluster_max_tolerance, const int cluster_min_points, const int cluster_max_points);
 
   std::vector<Eigen::Vector3d> findReflectorsFromClusters(
-    const std::vector<pcl::PointCloud<PointType>::Ptr> & clusters,
+    const std::vector<pcl::PointCloud<common_types::PointType>::Ptr> & clusters,
     const Eigen::Vector4f & ground_model);
 
   bool checkInitialTransforms();
@@ -119,19 +139,27 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     const std::vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> & matches,
     builtin_interfaces::msg::Time & time);
 
-  std::tuple<pcl::PointCloud<PointType>::Ptr, pcl::PointCloud<PointType>::Ptr, double, double>
-  getPointsSetAndDelta();
+  std::tuple<
+    pcl::PointCloud<common_types::PointType>::Ptr, pcl::PointCloud<common_types::PointType>::Ptr>
+  getPointsSet();
+  std::tuple<double, double> get2DRotationDelta(
+    std::vector<Track> converged_tracks, bool is_crossval);
+
   std::pair<double, double> computeCalibrationError(
     const Eigen::Isometry3d & radar_to_lidar_isometry);
-  void estimateTransformation(
-    pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
-    pcl::PointCloud<PointType>::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum);
+  TransformationResult estimateTransformation();
+  void evaluateTransformation(Eigen::Isometry3d calibrated_radar_to_lidar_transformation);
+  void crossValEvaluation(TransformationResult transformation_result);
   void findCombinations(
-    int n, int k, std::vector<int> & curr, int first_num,
-    std::vector<std::vector<int>> & combinations);
-  void crossValEvaluation(
-    pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
-    pcl::PointCloud<PointType>::Ptr radar_points_rcs);
+    std::size_t n, std::size_t k, std::vector<std::size_t> & curr, std::size_t first_num,
+    std::vector<std::vector<std::size_t>> & combinations);
+  void selectCombinations(
+    std::size_t tracks_size, std::size_t num_of_samples,
+    std::vector<std::vector<std::size_t>> & combinations);
+  void evaluateCombinations(
+    std::vector<std::vector<std::size_t>> & combinations, std::size_t num_of_samples,
+    TransformationResult transformation_result);
+
   void publishMetrics();
   void calibrateSensors();
   void visualizationMarkers(
@@ -142,15 +170,18 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   void deleteTrackMarkers();
   void drawCalibrationStatusText();
   geometry_msgs::msg::Point eigenToPointMsg(const Eigen::Vector3d & p_eigen);
-  double getYawError(const Eigen::Vector3d & v1, const Eigen::Vector3d & v2);
+  double getDistanceError(const Eigen::Vector3d v1, const Eigen::Vector3d v2);
+  double getYawError(const Eigen::Vector3d v1, const Eigen::Vector3d v2);
 
   rcl_interfaces::msg::SetParametersResult paramCallback(
     const std::vector<rclcpp::Parameter> & parameters);
 
   struct Parameters
   {
-    std::string radar_parallel_frame;  // frame that is assumed to be parallel to the radar (needed
-                                       // for radars that do not provide elevation)
+    std::string radar_optimization_frame;  // If the radar does not provide elevation,
+                                           // this frame needs to be parallel to the radar
+                                           // and should only use the 2D transformation.
+
     bool use_lidar_initial_crop_box_filter;
     double lidar_initial_crop_box_min_x;
     double lidar_initial_crop_box_min_y;
@@ -185,7 +216,7 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
     double max_matching_distance;
     double max_initial_calibration_translation_error;
     double max_initial_calibration_rotation_error;
-    int max_number_of_combination_samples;
+    std::size_t max_number_of_combination_samples;
   } parameters_;
 
   // ROS Interface
@@ -212,7 +243,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr metrics_pub_;
 
   rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr lidar_sub_;
-  rclcpp::Subscription<radar_msgs::msg::RadarTracks>::SharedPtr radar_sub_;
+  rclcpp::Subscription<radar_msgs::msg::RadarTracks>::SharedPtr radar_tracks_sub_;
+  rclcpp::Subscription<radar_msgs::msg::RadarScan>::SharedPtr radar_scan_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr radar_cloud_sub_;
 
   rclcpp::Service<tier4_calibration_msgs::srv::ExtrinsicCalibrator>::SharedPtr
     calibration_request_server_;
@@ -233,8 +266,12 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   Eigen::Isometry3d initial_radar_to_lidar_eigen_;
   Eigen::Isometry3d calibrated_radar_to_lidar_eigen_;
 
-  geometry_msgs::msg::Transform radar_parallel_to_lidar_msg_;
-  Eigen::Isometry3d radar_parallel_to_lidar_eigen_;
+  // radar optimization is the frame that radar optimize the transformation to.
+  geometry_msgs::msg::Transform radar_optimization_to_lidar_msg_;
+  Eigen::Isometry3d radar_optimization_to_lidar_eigen_;
+
+  geometry_msgs::msg::Transform initial_radar_optimization_to_radar_msg_;
+  Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_;
 
   bool got_initial_transform_{false};
   bool broadcast_tf_{false};
@@ -254,7 +291,9 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   BackgroundModel lidar_background_model_;
   BackgroundModel radar_background_model_;
 
-  radar_msgs::msg::RadarTracks::SharedPtr latest_radar_msgs_;
+  radar_msgs::msg::RadarTracks::SharedPtr latest_radar_tracks_msgs_;
+  radar_msgs::msg::RadarScan::SharedPtr latest_radar_scan_msgs_;
+  sensor_msgs::msg::PointCloud2::SharedPtr latest_radar_cloud_msgs_;
 
   // Tracking
   bool tracking_active_{false};
@@ -266,6 +305,8 @@ class ExtrinsicReflectorBasedCalibrator : public rclcpp::Node
   // Metrics
   std::vector<float> output_metrics_;
 
+  MsgType msg_type_;
+  TransformationType transformation_type_;
   static constexpr int MARKER_SIZE_PER_TRACK = 8;
 };
 
diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp
new file mode 100644
index 00000000..93beda2b
--- /dev/null
+++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/sensor_residual.hpp
@@ -0,0 +1,64 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__SENSOR_RESIDUAL_HPP_
+#define MARKER_RADAR_LIDAR_CALIBRATOR__SENSOR_RESIDUAL_HPP_
+
+#include <Eigen/Dense>
+
+namespace marker_radar_lidar_calibrator
+{
+
+struct SensorResidual
+{
+  SensorResidual(const Eigen::Vector4d & radar_point, const Eigen::Vector4d & lidar_point)
+  : radar_point_(radar_point), lidar_point_(lidar_point)
+  {
+  }
+
+  template <class T>
+  bool operator()(T const * const params, T * s_residuals) const
+  {
+    // parameters: x, y, z, pitch, yaw.
+    Eigen::Matrix<T, 4, 4> transformation_matrix = Eigen::Matrix<T, 4, 4>::Identity(4, 4);
+    Eigen::Matrix<T, 3, 3> rotation_matrix;
+
+    transformation_matrix(0, 3) = T(params[0]);
+    transformation_matrix(1, 3) = T(params[1]);
+    transformation_matrix(2, 3) = T(params[2]);
+
+    // This rotation matrix is rotate from radar to optimization frames (usually base_link).
+    // To avoid make sure that the Y axis does not approaches 90 degrees to avoid gimbal lock.
+    rotation_matrix = (Eigen::AngleAxis<T>(T(params[4]), Eigen::Vector3<T>::UnitZ()) *
+                       Eigen::AngleAxis<T>(T(params[3]), Eigen::Vector3<T>::UnitY()) *
+                       Eigen::AngleAxis<T>(T(0), Eigen::Vector3<T>::UnitX()))
+                        .matrix();
+
+    transformation_matrix.block(0, 0, 3, 3) = rotation_matrix;
+
+    Eigen::Map<Eigen::Matrix<T, 3, 1>> residuals(s_residuals);
+    Eigen::Matrix<T, 4, 1> residuals4d =
+      lidar_point_.cast<T>() - transformation_matrix * radar_point_.cast<T>();
+    residuals = residuals4d.block(0, 0, 3, 1);
+
+    return true;
+  }
+
+  Eigen::Vector4d radar_point_;
+  Eigen::Vector4d lidar_point_;
+};
+
+}  // namespace marker_radar_lidar_calibrator
+
+#endif  // MARKER_RADAR_LIDAR_CALIBRATOR__SENSOR_RESIDUAL_HPP_
diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp
new file mode 100644
index 00000000..c47bdf45
--- /dev/null
+++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/transformation_estimator.hpp
@@ -0,0 +1,66 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MARKER_RADAR_LIDAR_CALIBRATOR__TRANSFORMATION_ESTIMATOR_HPP_
+#define MARKER_RADAR_LIDAR_CALIBRATOR__TRANSFORMATION_ESTIMATOR_HPP_
+
+#include <Eigen/Dense>
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp>
+#include <marker_radar_lidar_calibrator/types.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+
+#include <ceres/ceres.h>
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <tf2/convert.h>
+
+namespace marker_radar_lidar_calibrator
+{
+
+class TransformationEstimator
+{
+public:
+  TransformationEstimator(
+    Eigen::Isometry3d initial_radar_to_lidar_eigen,
+    Eigen::Isometry3d initial_radar_to_radar_optimization_eigen,
+    Eigen::Isometry3d radar_optimization_to_lidar_eigen);
+  void setPoints(
+    pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs,
+    pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs);
+  void set2DRotationDelta(double delta_cos, double delta_sin);
+  void estimateYawOnlyTransformation();
+  void estimateSVDTransformation(
+    ExtrinsicReflectorBasedCalibrator::TransformationType transformation_type);
+  void estimateZeroRollTransformation();
+  Eigen::Isometry3d getTransformation();
+
+  double delta_cos_;
+  double delta_sin_;
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs_;
+  pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs_;
+  Eigen::Isometry3d calibrated_radar_to_lidar_transformation_;
+
+  Eigen::Isometry3d initial_radar_to_lidar_eigen_;
+  Eigen::Isometry3d initial_radar_optimization_to_radar_eigen_;
+  Eigen::Isometry3d radar_optimization_to_lidar_eigen_;
+};
+
+}  // namespace marker_radar_lidar_calibrator
+
+#endif  // MARKER_RADAR_LIDAR_CALIBRATOR__TRANSFORMATION_ESTIMATOR_HPP_
diff --git a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp
index 8caa0e93..6f5880b1 100644
--- a/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp
+++ b/calibrators/marker_radar_lidar_calibrator/include/marker_radar_lidar_calibrator/types.hpp
@@ -28,11 +28,15 @@
 namespace marker_radar_lidar_calibrator
 {
 
+namespace common_types
+{
+using PointType = pcl::PointXYZ;
+}
+
 struct BackgroundModel
 {
 public:
-  using PointType = pcl::PointXYZ;
-  using TreeType = pcl::KdTreeFLANN<PointType>;  // cSpell:ignore FLANN
+  using TreeType = pcl::KdTreeFLANN<common_types::PointType>;  // cSpell:ignore FLANN
   using index_t = std::uint32_t;
 
   BackgroundModel()
@@ -44,7 +48,7 @@ struct BackgroundModel
     max_point_(
       -std::numeric_limits<float>::max(), -std::numeric_limits<float>::max(),
       -std::numeric_limits<float>::max(), 1.f),
-    pointcloud_(new pcl::PointCloud<PointType>)
+    pointcloud_(new pcl::PointCloud<common_types::PointType>)
   {
   }
 
@@ -53,7 +57,7 @@ struct BackgroundModel
   Eigen::Vector4f min_point_;
   Eigen::Vector4f max_point_;
   std::unordered_set<index_t> set_;
-  pcl::PointCloud<PointType>::Ptr pointcloud_;
+  pcl::PointCloud<common_types::PointType>::Ptr pointcloud_;
   TreeType tree_;
 };
 
diff --git a/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml b/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml
index f6b11b6a..bef9419a 100644
--- a/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml
+++ b/calibrators/marker_radar_lidar_calibrator/launch/calibrator.launch.xml
@@ -4,9 +4,11 @@
   <arg name="calibration_service_name" default="calibrate_radar_lidar"/>
   <arg name="rviz" default="true"/>
   <let name="rviz_profile" value="$(find-pkg-share marker_radar_lidar_calibrator)/rviz/default.rviz"/>
-  <arg name="radar_parallel_frame" default="front_unit_base_link"/>
+  <arg name="radar_optimization_frame" default="base_link"/>
   <arg name="input_lidar_pointcloud" default="/sensing/lidar/front_lower/pointcloud_raw"/>
-  <arg name="input_radar_objects" default="/sensing/radar/front_center/objects_raw"/>
+  <arg name="input_radar_msg" default="/sensing/radar/front_center/objects_raw"/>
+  <arg name="msg_type" default="objects_raw"/>
+  <arg name="transformation_type" default="svd_2d"/>
 
   <arg name="use_lidar_initial_crop_box_filter" default="false"/>
   <arg name="lidar_initial_crop_box_min_x" default="-50.0"/>
@@ -29,11 +31,13 @@
   <group>
     <push-ros-namespace namespace="$(var ns)"/>
     <node pkg="marker_radar_lidar_calibrator" exec="marker_radar_lidar_calibrator" name="marker_radar_lidar_calibrator" output="screen">
-      <param name="radar_parallel_frame" value="$(var radar_parallel_frame)"/>
+      <param name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
+      <param name="msg_type" value="$(var msg_type)"/>
+      <param name="transformation_type" value="$(var transformation_type)"/>
 
       <remap from="extrinsic_calibration" to="$(var calibration_service_name)"/>
       <remap from="input_lidar_pointcloud" to="$(var input_lidar_pointcloud)"/>
-      <remap from="input_radar_objects" to="$(var input_radar_objects)"/>
+      <remap from="input_radar_msg" to="$(var input_radar_msg)"/>
 
       <param name="use_lidar_initial_crop_box_filter" value="$(var use_lidar_initial_crop_box_filter)"/>
       <param name="lidar_initial_crop_box_min_x" value="$(var lidar_initial_crop_box_min_x)"/>
diff --git a/calibrators/marker_radar_lidar_calibrator/package.xml b/calibrators/marker_radar_lidar_calibrator/package.xml
index 19f0bee2..b189ab73 100644
--- a/calibrators/marker_radar_lidar_calibrator/package.xml
+++ b/calibrators/marker_radar_lidar_calibrator/package.xml
@@ -17,6 +17,7 @@
   <depend>autoware_universe_utils</depend>
   <depend>eigen</depend>
   <depend>geometry_msgs</depend>
+  <depend>libceres-dev</depend>
   <depend>pcl_conversions</depend>
   <depend>pcl_ros</depend>
   <depend>radar_msgs</depend>
diff --git a/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz b/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz
index 90edc3bf..60bfcc66 100644
--- a/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz
+++ b/calibrators/marker_radar_lidar_calibrator/rviz/default.rviz
@@ -9,7 +9,7 @@ Panels:
         - /lidar_background_pointcloud1/Topic1
         - /lidar_colored_clusters1/Topic1
       Splitter Ratio: 0.5
-    Tree Height: 1106
+    Tree Height: 725
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -27,7 +27,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: lidar
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -178,7 +178,7 @@ Visualization Manager:
       Enabled: true
       Name: lidar_detections
       Namespaces:
-        "": true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -190,8 +190,7 @@ Visualization Manager:
       Enabled: true
       Name: radar_detections
       Namespaces:
-        center: true
-        line: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -237,8 +236,7 @@ Visualization Manager:
       Enabled: true
       Name: tracking_markers
       Namespaces:
-        calibrated: true
-        initial: true
+        {}
       Topic:
         Depth: 5
         Durability Policy: Volatile
@@ -258,7 +256,7 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /matches_markers
       Value: true
-    - Class: rviz_default_plugins/MarkerArray
+    - Class: rviz_default_plugins/Marker
       Enabled: true
       Name: text_markers
       Namespaces:
@@ -266,6 +264,7 @@ Visualization Manager:
       Topic:
         Depth: 5
         Durability Policy: Volatile
+        Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /text_markers
@@ -344,9 +343,9 @@ Visualization Manager:
       Near Clip Distance: 0.009999999776482582
       Pitch: 0.23479722440242767
       Position:
-        X: -6.539778709411621
-        Y: 0.01612010970711708
-        Z: 3.799168348312378
+        X: -17.043386459350586
+        Y: -0.017056670039892197
+        Z: 6.311741828918457
       Target Frame: <Fixed Frame>
       Value: FPS (rviz_default_plugins)
       Yaw: 0.0031585693359375
@@ -354,10 +353,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1403
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000216000004ddfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004dd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b60000003efc0100000002fb0000000800540069006d00650100000000000009b6000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000079a000004dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000002160000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000252fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000252000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000051e0000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -366,6 +365,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2486
-  X: 1994
-  Y: 0
+  Width: 1850
+  X: 2630
+  Y: 547
diff --git a/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py b/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py
index eeeb92fb..1c0a4810 100755
--- a/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py
+++ b/calibrators/marker_radar_lidar_calibrator/scripts/metrics_plotter_node.py
@@ -30,7 +30,7 @@ def __init__(self):
         self.subplot1 = self.axes[0, 1]
         self.subplot2 = self.axes[1, 0]
         self.subplot3 = self.axes[1, 1]
-        plt.gcf().canvas.set_window_title("Metrics plotter")
+        self.fig.canvas.manager.set_window_title("Metrics plotter")
 
         self.color_distance_o = "C0o-"
         self.color_yaw_o = "C1o-"
diff --git a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp
index 7a17df34..48eb06b8 100644
--- a/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp
+++ b/calibrators/marker_radar_lidar_calibrator/src/marker_radar_lidar_calibrator.cpp
@@ -14,6 +14,8 @@
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp>
+#include <marker_radar_lidar_calibrator/track.hpp>
+#include <marker_radar_lidar_calibrator/transformation_estimator.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
 #include <pcl/ModelCoefficients.h>
@@ -22,7 +24,6 @@
 #include <pcl/common/pca.h>
 #include <pcl/filters/crop_box.h>
 #include <pcl/filters/extract_indices.h>
-#include <pcl/registration/transformation_estimation_svd.h>
 #include <pcl/sample_consensus/method_types.h>
 #include <pcl/sample_consensus/model_types.h>
 #include <pcl/segmentation/extract_clusters.h>
@@ -32,6 +33,7 @@
 
 #include <algorithm>
 #include <chrono>
+#include <cstddef>
 #include <iostream>
 #include <limits>
 #include <memory>
@@ -64,7 +66,6 @@ void update_param(
 
 namespace marker_radar_lidar_calibrator
 {
-
 rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::paramCallback(
   const std::vector<rclcpp::Parameter> & parameters)
 {
@@ -75,7 +76,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para
   Parameters p = parameters_;
 
   try {
-    UPDATE_PARAM(p, radar_parallel_frame);
+    UPDATE_PARAM(p, radar_optimization_frame);
     UPDATE_PARAM(p, use_lidar_initial_crop_box_filter);
     UPDATE_PARAM(p, lidar_initial_crop_box_min_x);
     UPDATE_PARAM(p, lidar_initial_crop_box_min_y);
@@ -110,7 +111,7 @@ rcl_interfaces::msg::SetParametersResult ExtrinsicReflectorBasedCalibrator::para
     UPDATE_PARAM(p, max_matching_distance);
     UPDATE_PARAM(p, max_initial_calibration_translation_error);
     UPDATE_PARAM(p, max_initial_calibration_rotation_error);
-
+    UPDATE_PARAM(p, max_number_of_combination_samples);
     // transaction succeeds, now assign values
     parameters_ = p;
   } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
@@ -127,7 +128,8 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
 {
   tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
-  parameters_.radar_parallel_frame = this->declare_parameter<std::string>("radar_parallel_frame");
+  parameters_.radar_optimization_frame =
+    this->declare_parameter<std::string>("radar_optimization_frame");
 
   parameters_.use_lidar_initial_crop_box_filter =
     this->declare_parameter<bool>("use_lidar_initial_crop_box_filter", true);
@@ -198,8 +200,33 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
   parameters_.reflector_radius = this->declare_parameter<double>("reflector_radius", 0.1);
   parameters_.reflector_max_height = this->declare_parameter<double>("reflector_max_height", 1.2);
   parameters_.max_matching_distance = this->declare_parameter<double>("max_matching_distance", 1.0);
-  parameters_.max_number_of_combination_samples =
-    this->declare_parameter<int>("max_number_of_combination_samples", 10000);
+  parameters_.max_number_of_combination_samples = static_cast<std::size_t>(
+    this->declare_parameter<int>("max_number_of_combination_samples", 10000));
+
+  auto msg_type = this->declare_parameter<std::string>("msg_type");
+  auto transformation_type = this->declare_parameter<std::string>("transformation_type");
+
+  if (msg_type == "radar_tracks") {
+    msg_type_ = MsgType::radar_tracks;
+  } else if (msg_type == "radar_scan") {
+    msg_type_ = MsgType::radar_scan;
+  } else if (msg_type == "radar_cloud") {
+    msg_type_ = MsgType::radar_cloud;
+  } else {
+    throw std::runtime_error("Invalid param value: " + msg_type);
+  }
+
+  if (transformation_type == "svd_2d") {
+    transformation_type_ = TransformationType::svd_2d;
+  } else if (transformation_type == "yaw_only_rotation_2d") {
+    transformation_type_ = TransformationType::yaw_only_rotation_2d;
+  } else if (transformation_type == "svd_3d") {
+    transformation_type_ = TransformationType::svd_3d;
+  } else if (transformation_type == "zero_roll_3d") {
+    transformation_type_ = TransformationType::zero_roll_3d;
+  } else {
+    throw std::runtime_error("Invalid param value: " + transformation_type);
+  }
 
   double initial_lidar_cov = this->declare_parameter<double>("initial_lidar_cov", 0.5);
   double initial_radar_cov = this->declare_parameter<double>("initial_radar_cov", 2.0);
@@ -250,9 +277,22 @@ ExtrinsicReflectorBasedCalibrator::ExtrinsicReflectorBasedCalibrator(
     "input_lidar_pointcloud", rclcpp::SensorDataQoS(),
     std::bind(&ExtrinsicReflectorBasedCalibrator::lidarCallback, this, std::placeholders::_1));
 
-  radar_sub_ = this->create_subscription<radar_msgs::msg::RadarTracks>(
-    "input_radar_objects", rclcpp::SensorDataQoS(),
-    std::bind(&ExtrinsicReflectorBasedCalibrator::radarCallback, this, std::placeholders::_1));
+  if (msg_type_ == MsgType::radar_tracks) {
+    radar_tracks_sub_ = this->create_subscription<radar_msgs::msg::RadarTracks>(
+      "input_radar_msg", rclcpp::SensorDataQoS(),
+      std::bind(
+        &ExtrinsicReflectorBasedCalibrator::radarTracksCallback, this, std::placeholders::_1));
+  } else if (msg_type_ == MsgType::radar_scan) {
+    radar_scan_sub_ = this->create_subscription<radar_msgs::msg::RadarScan>(
+      "input_radar_msg", rclcpp::SensorDataQoS(),
+      std::bind(
+        &ExtrinsicReflectorBasedCalibrator::radarScanCallback, this, std::placeholders::_1));
+  } else if (msg_type_ == MsgType::radar_cloud) {
+    radar_cloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+      "input_radar_msg", rclcpp::SensorDataQoS(),
+      std::bind(
+        &ExtrinsicReflectorBasedCalibrator::radarCloudCallback, this, std::placeholders::_1));
+  }
 
   // The service server runs in a dedicated thread
   calibration_api_srv_callback_group_ =
@@ -371,7 +411,7 @@ void ExtrinsicReflectorBasedCalibrator::backgroundModelRequestCallback(
     }
 
     RCLCPP_WARN_THROTTLE(
-      this->get_logger(), *this->get_clock(), 5000, "Waiting for the calibration to end");
+      this->get_logger(), *this->get_clock(), 5000, "Waiting to extract the background model");
   }
 
   RCLCPP_INFO(this->get_logger(), "Background model estimated");
@@ -440,15 +480,36 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback(
   const sensor_msgs::msg::PointCloud2::SharedPtr msg)
 {
   RCLCPP_INFO(this->get_logger(), "lidarCallback");
-  if (!latest_radar_msgs_ || latest_radar_msgs_->tracks.size() == 0) {
-    RCLCPP_INFO(this->get_logger(), "There were no tracks");
-    return;
+  std::vector<Eigen::Vector3d> radar_detections;
+  if (msg_type_ == MsgType::radar_tracks) {
+    if (!latest_radar_tracks_msgs_ || latest_radar_tracks_msgs_->tracks.size() == 0) {
+      RCLCPP_INFO(this->get_logger(), "There were no radar tracks");
+      return;
+    }
+    pcl::PointCloud<common_types::PointType>::Ptr radar_pointcloud_ptr =
+      extractRadarPointcloud(latest_radar_tracks_msgs_);
+    radar_detections = extractRadarReflectors(radar_pointcloud_ptr);
+    latest_radar_tracks_msgs_->tracks.clear();
+  } else if (msg_type_ == MsgType::radar_scan) {
+    if (!latest_radar_scan_msgs_ || latest_radar_scan_msgs_->returns.size() == 0) {
+      RCLCPP_INFO(this->get_logger(), "There were no radar scans");
+      return;
+    }
+    pcl::PointCloud<common_types::PointType>::Ptr radar_pointcloud_ptr =
+      extractRadarPointcloud(latest_radar_scan_msgs_);
+    radar_detections = extractRadarReflectors(radar_pointcloud_ptr);
+    latest_radar_scan_msgs_->returns.clear();
+  } else {
+    if (!latest_radar_cloud_msgs_) {
+      RCLCPP_INFO(this->get_logger(), "There were no radar pointclouds");
+      return;
+    }
+    pcl::PointCloud<common_types::PointType>::Ptr radar_pointcloud_ptr =
+      extractRadarPointcloud(latest_radar_cloud_msgs_);
+    radar_detections = extractRadarReflectors(radar_pointcloud_ptr);
   }
 
-  auto lidar_detections = extractReflectors(msg);
-  auto radar_detections = extractReflectors(latest_radar_msgs_);
-  latest_radar_msgs_->tracks.clear();
-
+  auto lidar_detections = extractLidarReflectors(msg);
   auto matches = matchDetections(lidar_detections, radar_detections);
 
   bool is_track_converged = trackMatches(matches, msg->header.stamp);
@@ -465,19 +526,37 @@ void ExtrinsicReflectorBasedCalibrator::lidarCallback(
     converged_tracks_.size());
 }
 
-void ExtrinsicReflectorBasedCalibrator::radarCallback(
+void ExtrinsicReflectorBasedCalibrator::radarTracksCallback(
   const radar_msgs::msg::RadarTracks::SharedPtr msg)
 {
-  if (!latest_radar_msgs_) {
-    latest_radar_msgs_ = msg;
+  if (!latest_radar_tracks_msgs_) {
+    latest_radar_tracks_msgs_ = msg;
+  } else {
+    latest_radar_tracks_msgs_->header = msg->header;
+    latest_radar_tracks_msgs_->tracks.insert(
+      latest_radar_tracks_msgs_->tracks.end(), msg->tracks.begin(), msg->tracks.end());
+  }
+}
+
+void ExtrinsicReflectorBasedCalibrator::radarScanCallback(
+  const radar_msgs::msg::RadarScan::SharedPtr msg)
+{
+  if (!latest_radar_scan_msgs_) {
+    latest_radar_scan_msgs_ = msg;
   } else {
-    latest_radar_msgs_->header = msg->header;
-    latest_radar_msgs_->tracks.insert(
-      latest_radar_msgs_->tracks.end(), msg->tracks.begin(), msg->tracks.end());
+    latest_radar_scan_msgs_->header = msg->header;
+    latest_radar_scan_msgs_->returns.insert(
+      latest_radar_scan_msgs_->returns.end(), msg->returns.begin(), msg->returns.end());
   }
 }
 
-std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflectors(
+void ExtrinsicReflectorBasedCalibrator::radarCloudCallback(
+  const sensor_msgs::msg::PointCloud2::SharedPtr msg)
+{
+  latest_radar_cloud_msgs_ = msg;
+}
+
+std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractLidarReflectors(
   const sensor_msgs::msg::PointCloud2::SharedPtr msg)
 {
   lidar_frame_ = msg->header.frame_id;
@@ -492,12 +571,14 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
     valid_background_model = lidar_background_model_.valid_;
   }
 
-  pcl::PointCloud<PointType>::Ptr lidar_pointcloud_ptr(new pcl::PointCloud<PointType>);
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_pointcloud_ptr(
+    new pcl::PointCloud<common_types::PointType>);
   pcl::fromROSMsg(*msg, *lidar_pointcloud_ptr);
 
   if (parameters_.use_lidar_initial_crop_box_filter) {
-    pcl::CropBox<PointType> box_filter;
-    pcl::PointCloud<PointType>::Ptr tmp_lidar_pointcloud_ptr(new pcl::PointCloud<PointType>);
+    pcl::CropBox<common_types::PointType> box_filter;
+    pcl::PointCloud<common_types::PointType>::Ptr tmp_lidar_pointcloud_ptr(
+      new pcl::PointCloud<common_types::PointType>);
     RCLCPP_INFO(this->get_logger(), "pre lidar_pointcloud_ptr=%lu", lidar_pointcloud_ptr->size());
     RCLCPP_WARN(
       this->get_logger(), "crop box parameters=%f | %f | %f",
@@ -530,7 +611,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
     return detections;
   }
 
-  pcl::PointCloud<PointType>::Ptr foreground_pointcloud_ptr;
+  pcl::PointCloud<common_types::PointType>::Ptr foreground_pointcloud_ptr;
   Eigen::Vector4f ground_model;
   extractForegroundPoints(
     lidar_pointcloud_ptr, lidar_background_model_, true, foreground_pointcloud_ptr, ground_model);
@@ -593,11 +674,48 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
   return detections;
 }
 
-std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflectors(
-  const radar_msgs::msg::RadarTracks::SharedPtr msg)
+template <typename RadarMsgType>
+pcl::PointCloud<common_types::PointType>::Ptr
+ExtrinsicReflectorBasedCalibrator::extractRadarPointcloud(const std::shared_ptr<RadarMsgType> & msg)
 {
+  static_assert(
+    std::is_same<RadarMsgType, radar_msgs::msg::RadarTracks>::value ||
+      std::is_same<RadarMsgType, radar_msgs::msg::RadarScan>::value ||
+      std::is_same<RadarMsgType, sensor_msgs::msg::PointCloud2>::value,
+    "Unsupported message type");
+
   radar_frame_ = msg->header.frame_id;
   radar_header_ = msg->header;
+  auto radar_pointcloud_ptr = std::make_shared<pcl::PointCloud<common_types::PointType>>();
+
+  if constexpr (std::is_same<RadarMsgType, radar_msgs::msg::RadarTracks>::value) {
+    radar_pointcloud_ptr->reserve(msg->tracks.size());
+    for (const auto & track : msg->tracks) {
+      radar_pointcloud_ptr->emplace_back(track.position.x, track.position.y, track.position.z);
+    }
+  } else if constexpr (std::is_same<RadarMsgType, radar_msgs::msg::RadarScan>::value) {
+    radar_pointcloud_ptr->reserve(msg->returns.size());
+    for (const auto & radar_return : msg->returns) {
+      float range = radar_return.range;
+      float azimuth = radar_return.azimuth;
+      float elevation = radar_return.elevation;
+
+      float x = range * std::cos(azimuth) * std::cos(elevation);
+      float y = range * std::sin(azimuth) * std::cos(elevation);
+      float z = range * std::sin(elevation);
+
+      radar_pointcloud_ptr->emplace_back(x, y, z);
+    }
+  } else if constexpr (std::is_same<RadarMsgType, sensor_msgs::msg::PointCloud2>::value) {
+    pcl::fromROSMsg(*msg, *radar_pointcloud_ptr);
+  }
+
+  return radar_pointcloud_ptr;
+}
+
+std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractRadarReflectors(
+  pcl::PointCloud<common_types::PointType>::Ptr radar_pointcloud_ptr)
+{
   bool extract_background_model;
   bool valid_background_model;
   std::vector<Eigen::Vector3d> detections;
@@ -608,16 +726,10 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
     valid_background_model = radar_background_model_.valid_;
   }
 
-  pcl::PointCloud<PointType>::Ptr radar_pointcloud_ptr(new pcl::PointCloud<PointType>);
-  radar_pointcloud_ptr->reserve(msg->tracks.size());
-
-  for (const auto & track : msg->tracks) {
-    radar_pointcloud_ptr->emplace_back(track.position.x, track.position.y, track.position.z);
-  }
-
   if (parameters_.use_radar_initial_crop_box_filter) {
-    pcl::CropBox<PointType> box_filter;
-    pcl::PointCloud<PointType>::Ptr tmp_radar_pointcloud_ptr(new pcl::PointCloud<PointType>);
+    pcl::CropBox<common_types::PointType> box_filter;
+    pcl::PointCloud<common_types::PointType>::Ptr tmp_radar_pointcloud_ptr(
+      new pcl::PointCloud<common_types::PointType>);
     box_filter.setMin(Eigen::Vector4f(
       parameters_.radar_initial_crop_box_min_x, parameters_.radar_initial_crop_box_min_y,
       parameters_.radar_initial_crop_box_min_z, 1.0));
@@ -631,7 +743,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
 
   if (extract_background_model && !valid_background_model) {
     extractBackgroundModel(
-      radar_pointcloud_ptr, msg->header, latest_updated_radar_header_, first_radar_header_,
+      radar_pointcloud_ptr, radar_header_, latest_updated_radar_header_, first_radar_header_,
       radar_background_model_);
     return detections;
   }
@@ -640,7 +752,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
     return detections;
   }
 
-  pcl::PointCloud<PointType>::Ptr foreground_pointcloud_ptr;
+  pcl::PointCloud<common_types::PointType>::Ptr foreground_pointcloud_ptr;
   Eigen::Vector4f ground_model;
   extractForegroundPoints(
     radar_pointcloud_ptr, radar_background_model_, false, foreground_pointcloud_ptr, ground_model);
@@ -681,7 +793,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::extractReflector
 }
 
 void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel(
-  const pcl::PointCloud<PointType>::Ptr & sensor_pointcloud_ptr,
+  const pcl::PointCloud<common_types::PointType>::Ptr & sensor_pointcloud_ptr,
   const std_msgs::msg::Header & current_header, std_msgs::msg::Header & last_updated_header,
   std_msgs::msg::Header & first_header, BackgroundModel & background_model)
 {
@@ -727,7 +839,7 @@ void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel(
     const auto & it = background_model.set_.emplace(index);
 
     if (it.second) {
-      PointType p_center;
+      common_types::PointType p_center;
       p_center.x = background_model.min_point_.x() + background_model.leaf_size_ * (x_index + 0.5f);
       p_center.y = background_model.min_point_.y() + background_model.leaf_size_ * (y_index + 0.5f);
       p_center.z = background_model.min_point_.z() + background_model.leaf_size_ * (z_index + 0.5f);
@@ -772,16 +884,18 @@ void ExtrinsicReflectorBasedCalibrator::extractBackgroundModel(
 }
 
 void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints(
-  const pcl::PointCloud<PointType>::Ptr & sensor_pointcloud_ptr,
+  const pcl::PointCloud<common_types::PointType>::Ptr & sensor_pointcloud_ptr,
   const BackgroundModel & background_model, bool use_ransac,
-  pcl::PointCloud<PointType>::Ptr & foreground_pointcloud_ptr, Eigen::Vector4f & ground_model)
+  pcl::PointCloud<common_types::PointType>::Ptr & foreground_pointcloud_ptr,
+  Eigen::Vector4f & ground_model)
 {
   RCLCPP_INFO(this->get_logger(), "Extracting foreground");
   RCLCPP_INFO(this->get_logger(), "\t initial points: %lu", sensor_pointcloud_ptr->size());
 
   // Crop box
-  pcl::PointCloud<PointType>::Ptr cropped_pointcloud_ptr(new pcl::PointCloud<PointType>);
-  pcl::CropBox<PointType> crop_filter;
+  pcl::PointCloud<common_types::PointType>::Ptr cropped_pointcloud_ptr(
+    new pcl::PointCloud<common_types::PointType>);
+  pcl::CropBox<common_types::PointType> crop_filter;
   crop_filter.setMin(background_model.min_point_);
   crop_filter.setMax(background_model.max_point_);
   crop_filter.setInputCloud(sensor_pointcloud_ptr);
@@ -789,7 +903,8 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints(
   RCLCPP_INFO(this->get_logger(), "\t cropped points: %lu", cropped_pointcloud_ptr->size());
 
   // Fast hash
-  pcl::PointCloud<PointType>::Ptr voxel_filtered_pointcloud_ptr(new pcl::PointCloud<PointType>);
+  pcl::PointCloud<common_types::PointType>::Ptr voxel_filtered_pointcloud_ptr(
+    new pcl::PointCloud<common_types::PointType>);
   voxel_filtered_pointcloud_ptr->reserve(cropped_pointcloud_ptr->size());
 
   index_t x_cells = (background_model.max_point_.x() - background_model.min_point_.x()) /
@@ -814,7 +929,8 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints(
     this->get_logger(), "\t voxel filtered points: %lu", voxel_filtered_pointcloud_ptr->size());
 
   // K-search
-  pcl::PointCloud<PointType>::Ptr tree_filtered_pointcloud_ptr(new pcl::PointCloud<PointType>);
+  pcl::PointCloud<common_types::PointType>::Ptr tree_filtered_pointcloud_ptr(
+    new pcl::PointCloud<common_types::PointType>);
   tree_filtered_pointcloud_ptr->reserve(voxel_filtered_pointcloud_ptr->size());
   float min_foreground_square_distance =
     parameters_.min_foreground_distance * parameters_.min_foreground_distance;
@@ -841,10 +957,11 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints(
   // Plane ransac (since the orientation changes slightly between data, this one does not use the
   // background model)
   pcl::ModelCoefficients::Ptr coefficients_ptr(new pcl::ModelCoefficients);
-  pcl::PointCloud<PointType>::Ptr ransac_filtered_pointcloud_ptr(new pcl::PointCloud<PointType>);
+  pcl::PointCloud<common_types::PointType>::Ptr ransac_filtered_pointcloud_ptr(
+    new pcl::PointCloud<common_types::PointType>);
   pcl::PointIndices::Ptr inliers_ptr(new pcl::PointIndices);
-  pcl::SACSegmentation<PointType> seg;
-  pcl::ExtractIndices<PointType> extract;
+  pcl::SACSegmentation<common_types::PointType> seg;
+  pcl::ExtractIndices<common_types::PointType> extract;
   seg.setOptimizeCoefficients(true);
   seg.setModelType(pcl::SACMODEL_PLANE);  // cSpell:ignore SACMODEL
   seg.setMethodType(pcl::SAC_RANSAC);
@@ -873,16 +990,17 @@ void ExtrinsicReflectorBasedCalibrator::extractForegroundPoints(
     coefficients_ptr->values[3]);
 }
 
-std::vector<pcl::PointCloud<ExtrinsicReflectorBasedCalibrator::PointType>::Ptr>
+std::vector<pcl::PointCloud<common_types::PointType>::Ptr>
 ExtrinsicReflectorBasedCalibrator::extractClusters(
-  const pcl::PointCloud<PointType>::Ptr & foreground_pointcloud_ptr,
+  const pcl::PointCloud<common_types::PointType>::Ptr & foreground_pointcloud_ptr,
   const double cluster_max_tolerance, const int cluster_min_points, const int cluster_max_points)
 {
-  pcl::search::KdTree<PointType>::Ptr tree_ptr(new pcl::search::KdTree<PointType>);
+  pcl::search::KdTree<common_types::PointType>::Ptr tree_ptr(
+    new pcl::search::KdTree<common_types::PointType>);
   tree_ptr->setInputCloud(foreground_pointcloud_ptr);
 
   std::vector<pcl::PointIndices> cluster_indices;
-  pcl::EuclideanClusterExtraction<PointType> cluster_extractor;
+  pcl::EuclideanClusterExtraction<common_types::PointType> cluster_extractor;
   cluster_extractor.setClusterTolerance(cluster_max_tolerance);
   cluster_extractor.setMinClusterSize(cluster_min_points);
   cluster_extractor.setMaxClusterSize(cluster_max_points);
@@ -893,10 +1011,11 @@ ExtrinsicReflectorBasedCalibrator::extractClusters(
   RCLCPP_INFO(
     this->get_logger(), "Cluster extraction input size: %lu", foreground_pointcloud_ptr->size());
 
-  std::vector<pcl::PointCloud<PointType>::Ptr> cluster_vector;
+  std::vector<pcl::PointCloud<common_types::PointType>::Ptr> cluster_vector;
 
   for (const auto & cluster : cluster_indices) {
-    pcl::PointCloud<PointType>::Ptr cluster_pointcloud_ptr(new pcl::PointCloud<PointType>);
+    pcl::PointCloud<common_types::PointType>::Ptr cluster_pointcloud_ptr(
+      new pcl::PointCloud<common_types::PointType>);
     cluster_pointcloud_ptr->reserve(cluster.indices.size());
 
     for (const auto & idx : cluster.indices) {
@@ -916,7 +1035,7 @@ ExtrinsicReflectorBasedCalibrator::extractClusters(
 }
 
 std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::findReflectorsFromClusters(
-  const std::vector<pcl::PointCloud<PointType>::Ptr> & clusters,
+  const std::vector<pcl::PointCloud<common_types::PointType>::Ptr> & clusters,
   const Eigen::Vector4f & ground_model)
 {
   std::vector<Eigen::Vector3d> reflector_centers;
@@ -924,7 +1043,7 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::findReflectorsFr
 
   for (const auto & cluster_pointcloud_ptr : clusters) {
     float max_h = -std::numeric_limits<float>::max();
-    PointType highest_point;
+    common_types::PointType highest_point;
 
     for (const auto & p : cluster_pointcloud_ptr->points) {
       float height =
@@ -939,7 +1058,8 @@ std::vector<Eigen::Vector3d> ExtrinsicReflectorBasedCalibrator::findReflectorsFr
       continue;
     }
 
-    pcl::search::KdTree<PointType>::Ptr tree_ptr(new pcl::search::KdTree<PointType>);
+    pcl::search::KdTree<common_types::PointType>::Ptr tree_ptr(
+      new pcl::search::KdTree<common_types::PointType>);
     tree_ptr->setInputCloud(cluster_pointcloud_ptr);
 
     std::vector<int> indexes;
@@ -986,11 +1106,18 @@ bool ExtrinsicReflectorBasedCalibrator::checkInitialTransforms()
     initial_radar_to_lidar_eigen_ = tf2::transformToEigen(initial_radar_to_lidar_msg_);
     calibrated_radar_to_lidar_eigen_ = initial_radar_to_lidar_eigen_;
 
-    radar_parallel_to_lidar_msg_ =
-      tf_buffer_->lookupTransform(parameters_.radar_parallel_frame, lidar_frame_, t, timeout)
+    radar_optimization_to_lidar_msg_ =
+      tf_buffer_->lookupTransform(parameters_.radar_optimization_frame, lidar_frame_, t, timeout)
         .transform;
 
-    radar_parallel_to_lidar_eigen_ = tf2::transformToEigen(radar_parallel_to_lidar_msg_);
+    radar_optimization_to_lidar_eigen_ = tf2::transformToEigen(radar_optimization_to_lidar_msg_);
+
+    initial_radar_optimization_to_radar_msg_ =
+      tf_buffer_->lookupTransform(parameters_.radar_optimization_frame, radar_frame_, t, timeout)
+        .transform;
+
+    initial_radar_optimization_to_radar_eigen_ =
+      tf2::transformToEigen(initial_radar_optimization_to_radar_msg_);
 
     got_initial_transform_ = true;
   } catch (tf2::TransformException & ex) {
@@ -1019,9 +1146,14 @@ ExtrinsicReflectorBasedCalibrator::matchDetections(
   std::transform(
     lidar_detections.cbegin(), lidar_detections.cend(),
     std::back_inserter(lidar_detections_transformed),
-    [&radar_to_lidar_transform](const auto & lidar_detection) {
+    [&radar_to_lidar_transform,
+     &transformation_type = this->transformation_type_](const auto & lidar_detection) {
       auto transformed_point = radar_to_lidar_transform * lidar_detection;
-      transformed_point.z() = 0.f;
+      if (
+        transformation_type == TransformationType::yaw_only_rotation_2d ||
+        transformation_type == TransformationType::svd_2d) {
+        transformed_point.z() = 0.f;
+      }
       return transformed_point;
     });
 
@@ -1175,34 +1307,19 @@ bool ExtrinsicReflectorBasedCalibrator::trackMatches(
   return is_track_converged;
 }
 
-std::tuple<
-  pcl::PointCloud<ExtrinsicReflectorBasedCalibrator::PointType>::Ptr,
-  pcl::PointCloud<ExtrinsicReflectorBasedCalibrator::PointType>::Ptr, double, double>
-ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta()
+std::tuple<double, double> ExtrinsicReflectorBasedCalibrator::get2DRotationDelta(
+  std::vector<Track> converged_tracks, bool is_crossval)
 {
-  // Define two sets of 2D points (just 3D points with z=0)
-  // Note: pcs=parallel coordinate system rcs=radar coordinate system
-  pcl::PointCloud<PointType>::Ptr lidar_points_pcs(new pcl::PointCloud<PointType>);
-  pcl::PointCloud<PointType>::Ptr radar_points_rcs(new pcl::PointCloud<PointType>);
-  lidar_points_pcs->reserve(converged_tracks_.size());
-  radar_points_rcs->reserve(converged_tracks_.size());
-
   double delta_cos_sum = 0.0;
   double delta_sin_sum = 0.0;
 
-  auto eigen_to_pcl_2d = [](const auto & p) { return PointType(p.x(), p.y(), 0.0); };
-
-  for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) {
-    auto track = converged_tracks_[track_index];
+  for (std::size_t track_index = 0; track_index < converged_tracks.size(); track_index++) {
+    auto track = converged_tracks[track_index];
     // lidar coordinates
     const auto & lidar_estimation = track.getLidarEstimation();
-    // to radar parallel coordinates
-    const auto & lidar_estimation_pcs = radar_parallel_to_lidar_eigen_ * lidar_estimation;
     // to radar coordinates
     const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation;
     const auto & radar_estimation_rcs = track.getRadarEstimation();
-    lidar_points_pcs->emplace_back(eigen_to_pcl_2d(lidar_estimation_pcs));
-    radar_points_rcs->emplace_back(eigen_to_pcl_2d(radar_estimation_rcs));
 
     const double lidar_transformed_norm = lidar_transformed_estimation.norm();
     const double lidar_transformed_cos = lidar_transformed_estimation.x() / lidar_transformed_norm;
@@ -1220,6 +1337,57 @@ ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta()
     delta_sin_sum += delta_angle_sin;
     delta_cos_sum += delta_angle_cos;
 
+    if (!is_crossval) {
+      // logging
+      RCLCPP_INFO_STREAM(this->get_logger(), "lidar_estimation:\n" << lidar_estimation.matrix());
+      RCLCPP_INFO_STREAM(
+        this->get_logger(), "lidar_transformed_estimation:\n"
+                              << lidar_transformed_estimation.matrix());
+      RCLCPP_INFO_STREAM(
+        this->get_logger(), "radar_estimation_rcs:\n"
+                              << radar_estimation_rcs.matrix());
+    }
+  }
+  double delta_cos = delta_cos_sum / converged_tracks.size();
+  double delta_sin = -delta_sin_sum / converged_tracks.size();
+
+  return {delta_cos, delta_sin};
+}
+
+std::tuple<
+  pcl::PointCloud<common_types::PointType>::Ptr, pcl::PointCloud<common_types::PointType>::Ptr>
+ExtrinsicReflectorBasedCalibrator::getPointsSet()
+{
+  // Note: ocs=radar optimization coordinate system rcs=radar coordinate system
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs(
+    new pcl::PointCloud<common_types::PointType>);
+  pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs(
+    new pcl::PointCloud<common_types::PointType>);
+  lidar_points_ocs->reserve(converged_tracks_.size());
+  radar_points_rcs->reserve(converged_tracks_.size());
+
+  auto eigen_to_pcl_2d = [](const auto & p) { return common_types::PointType(p.x(), p.y(), 0.0); };
+  auto eigen_to_pcl_3d = [](const auto & p) {
+    return common_types::PointType(p.x(), p.y(), p.z());
+  };
+
+  for (std::size_t track_index = 0; track_index < converged_tracks_.size(); track_index++) {
+    auto track = converged_tracks_[track_index];
+    // lidar coordinates
+    const auto & lidar_estimation = track.getLidarEstimation();
+    // to radar optimization coordinates
+    const auto & lidar_estimation_ocs = radar_optimization_to_lidar_eigen_ * lidar_estimation;
+    // to radar coordinates
+    const auto & lidar_transformed_estimation = initial_radar_to_lidar_eigen_ * lidar_estimation;
+    const auto & radar_estimation_rcs = track.getRadarEstimation();
+
+    if (transformation_type_ == TransformationType::svd_2d) {
+      lidar_points_ocs->emplace_back(eigen_to_pcl_2d(lidar_estimation_ocs));
+      radar_points_rcs->emplace_back(eigen_to_pcl_2d(radar_estimation_rcs));
+    } else {
+      lidar_points_ocs->emplace_back(eigen_to_pcl_3d(lidar_estimation_ocs));
+      radar_points_rcs->emplace_back(eigen_to_pcl_3d(radar_estimation_rcs));
+    }
     // logging
     RCLCPP_INFO_STREAM(this->get_logger(), "lidar_estimation:\n" << lidar_estimation.matrix());
     RCLCPP_INFO_STREAM(
@@ -1229,7 +1397,7 @@ ExtrinsicReflectorBasedCalibrator::getPointsSetAndDelta()
       this->get_logger(), "radar_estimation_rcs:\n"
                             << radar_estimation_rcs.matrix());
   }
-  return {lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum};
+  return {lidar_points_ocs, radar_points_rcs};
 }
 
 std::pair<double, double> ExtrinsicReflectorBasedCalibrator::computeCalibrationError(
@@ -1242,10 +1410,8 @@ std::pair<double, double> ExtrinsicReflectorBasedCalibrator::computeCalibrationE
     auto lidar_estimation = track.getLidarEstimation();
     auto radar_estimation = track.getRadarEstimation();
     auto lidar_estimation_transformed = radar_to_lidar_isometry * lidar_estimation;
-    lidar_estimation_transformed.z() = 0.0;
-    radar_estimation.z() = 0.0;
 
-    distance_error += (lidar_estimation_transformed - radar_estimation).norm();
+    distance_error += getDistanceError(lidar_estimation_transformed, radar_estimation);
     yaw_error += getYawError(lidar_estimation_transformed, radar_estimation);
   }
 
@@ -1255,82 +1421,74 @@ std::pair<double, double> ExtrinsicReflectorBasedCalibrator::computeCalibrationE
   return std::make_pair(distance_error, yaw_error);
 }
 
-void ExtrinsicReflectorBasedCalibrator::estimateTransformation(
-  pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
-  pcl::PointCloud<PointType>::Ptr radar_points_rcs, double delta_cos_sum, double delta_sin_sum)
+TransformationResult ExtrinsicReflectorBasedCalibrator::estimateTransformation()
 {
-  // Note: pcs=parallel coordinate system rcs=radar coordinate system
-  // Estimate full transformation using SVD
-  pcl::registration::TransformationEstimationSVD<PointType, PointType> estimator;
-  Eigen::Matrix4f full_radar_to_radar_parallel_transformation;
-  estimator.estimateRigidTransformation(
-    *lidar_points_pcs, *radar_points_rcs, full_radar_to_radar_parallel_transformation);
-  Eigen::Isometry3d calibrated_2d_radar_to_radar_parallel_transformation(
-    full_radar_to_radar_parallel_transformation.cast<double>());
-
-  // Check that it is actually a 2D transformation
-  auto calibrated_2d_radar_to_radar_parallel_rpy = autoware::universe_utils::getRPY(
-    tf2::toMsg(calibrated_2d_radar_to_radar_parallel_transformation).orientation);
-  double calibrated_2d_radar_to_radar_parallel_z =
-    calibrated_2d_radar_to_radar_parallel_transformation.translation().z();
-  double calibrated_2d_radar_to_radar_parallel_roll = calibrated_2d_radar_to_radar_parallel_rpy.x;
-  double calibrated_2d_radar_to_radar_parallel_pitch = calibrated_2d_radar_to_radar_parallel_rpy.y;
-
-  if (
-    calibrated_2d_radar_to_radar_parallel_z != 0.0 ||
-    calibrated_2d_radar_to_radar_parallel_roll != 0.0 ||
-    calibrated_2d_radar_to_radar_parallel_pitch != 0.0) {
-    RCLCPP_ERROR(
+  TransformationResult transformation_result;
+  TransformationEstimator estimator(
+    initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_,
+    radar_optimization_to_lidar_eigen_);
+
+  if (transformation_type_ == TransformationType::yaw_only_rotation_2d) {
+    auto [delta_cos, delta_sin] = get2DRotationDelta(converged_tracks_, false);
+    estimator.set2DRotationDelta(delta_cos, delta_sin);
+    estimator.estimateYawOnlyTransformation();
+    transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation();
+    RCLCPP_INFO_STREAM(
+      this->get_logger(), "Initial radar->lidar transform:\n"
+                            << initial_radar_to_lidar_eigen_.matrix());
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      "Pure rotation calibration radar->lidar transform:\n"
+        << transformation_result.calibrated_radar_to_lidar_transformation.matrix());
+  } else if (transformation_type_ == TransformationType::svd_2d) {
+    std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) =
+      getPointsSet();
+    estimator.setPoints(
+      transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs);
+    estimator.estimateSVDTransformation(transformation_type_);
+    transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation();
+    RCLCPP_INFO_STREAM(
+      this->get_logger(), "Initial radar->lidar transform:\n"
+                            << initial_radar_to_lidar_eigen_.matrix());
+    RCLCPP_INFO_STREAM(
+      this->get_logger(),
+      "2D calibration radar->lidar transform:\n"
+        << transformation_result.calibrated_radar_to_lidar_transformation.matrix());
+  } else if (
+    transformation_type_ == TransformationType::svd_3d ||
+    transformation_type_ == TransformationType::zero_roll_3d) {
+    std::tie(transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs) =
+      getPointsSet();
+    estimator.setPoints(
+      transformation_result.lidar_points_ocs, transformation_result.radar_points_rcs);
+
+    if (transformation_type_ == TransformationType::zero_roll_3d)
+      estimator.estimateZeroRollTransformation();
+    else if (transformation_type_ == TransformationType::svd_3d)
+      estimator.estimateSVDTransformation(transformation_type_);
+
+    transformation_result.calibrated_radar_to_lidar_transformation = estimator.getTransformation();
+    RCLCPP_INFO_STREAM(
+      this->get_logger(), "Initial radar->lidar transform:\n"
+                            << initial_radar_to_lidar_eigen_.matrix());
+    RCLCPP_INFO_STREAM(
       this->get_logger(),
-      "The estimated 2D translation was not really 2D. Continue at your own risk. z=%.3f roll=%.3f "
-      "pitch=%.3f",
-      calibrated_2d_radar_to_radar_parallel_z, calibrated_2d_radar_to_radar_parallel_roll,
-      calibrated_2d_radar_to_radar_parallel_pitch);
+      "3D calibration radar->lidar transform:\n"
+        << transformation_result.calibrated_radar_to_lidar_transformation.matrix());
   }
 
-  calibrated_2d_radar_to_radar_parallel_transformation.translation().z() =
-    (initial_radar_to_lidar_eigen_ * radar_parallel_to_lidar_eigen_.inverse()).translation().z();
-  Eigen::Isometry3d calibrated_2d_radar_to_lidar_transformation =
-    calibrated_2d_radar_to_radar_parallel_transformation * radar_parallel_to_lidar_eigen_;
-
-  // Estimate the 2D transformation estimating only yaw
-  double delta_cos = delta_cos_sum / converged_tracks_.size();
-  double delta_sin = -delta_sin_sum / converged_tracks_.size();
-
-  Eigen::Matrix3d delta_rotation;
-  delta_rotation << delta_cos, -delta_sin, 0.0, delta_sin, delta_cos, 0.0, 0.0, 0.0, 1.0;
-  Eigen::Isometry3d delta_transformation = Eigen::Isometry3d::Identity();
-  delta_transformation.linear() = delta_rotation;
-  Eigen::Isometry3d calibrated_rotation_radar_to_lidar_transformation =
-    delta_transformation * initial_radar_to_lidar_eigen_;
+  return transformation_result;
+}
 
+void ExtrinsicReflectorBasedCalibrator::evaluateTransformation(
+  Eigen::Isometry3d calibrated_radar_to_lidar_transformation)
+{
   // Estimate the pre & post calibration error
   auto [initial_distance_error, initial_yaw_error] =
     computeCalibrationError(initial_radar_to_lidar_eigen_);
-  auto [calibrated_2d_distance_error, calibrated_2d_yaw_error] =
-    computeCalibrationError(calibrated_2d_radar_to_lidar_transformation);
-  auto [calibrated_rotation_distance_error, calibrated_rotation_yaw_error] =
-    computeCalibrationError(calibrated_rotation_radar_to_lidar_transformation);
-
-  RCLCPP_INFO_STREAM(
-    this->get_logger(), "Initial radar->lidar transform:\n"
-                          << initial_radar_to_lidar_eigen_.matrix());
-  RCLCPP_INFO_STREAM(
-    this->get_logger(), "2D calibration radar->lidar transform:\n"
-                          << calibrated_2d_radar_to_lidar_transformation.matrix());
-  RCLCPP_INFO_STREAM(
-    this->get_logger(), "Pure rotation calibration radar->lidar transform:\n"
-                          << calibrated_rotation_radar_to_lidar_transformation.matrix());
-
-  // Evaluate the different calibrations and decide on an output
-  auto compute_transformation_difference =
-    [](const Eigen::Isometry3d & t1, const Eigen::Isometry3d & t2) -> std::pair<double, double> {
-    double translation_difference = (t2.inverse() * t1).translation().norm();
-    double rotation_difference =
-      std::acos(std::min(1.0, 0.5 * ((t2.rotation().inverse() * t1.rotation()).trace() - 1.0)));
+  auto [calibrated_distance_error, calibrated_yaw_error] =
+    computeCalibrationError(calibrated_radar_to_lidar_transformation);
 
-    return std::make_pair(translation_difference, rotation_difference);
-  };
   RCLCPP_INFO(
     this->get_logger(),
     "Initial calibration error: detection2detection.distance=%.4fm yaw=%.4f degrees",
@@ -1338,68 +1496,55 @@ void ExtrinsicReflectorBasedCalibrator::estimateTransformation(
   RCLCPP_INFO(
     this->get_logger(),
     "Final calibration error: detection2detection.distance=%.4fm yaw=%.4f degrees",
-    calibrated_2d_distance_error, calibrated_2d_yaw_error);
-  RCLCPP_INFO(
-    this->get_logger(),
-    "Final calibration error (rotation only): detection2detection.distance=%.4fm yaw=%.4f degrees",
-    calibrated_rotation_distance_error, calibrated_rotation_yaw_error);
+    calibrated_distance_error, calibrated_yaw_error);
 
-  auto [calibrated_2d_translation_difference, calibrated_2d_rotation_difference] =
-    compute_transformation_difference(
-      initial_radar_to_lidar_eigen_, calibrated_2d_radar_to_lidar_transformation);
-  auto [calibrated_rotation_translation_difference, calibrated_rotation_rotation_difference] =
+  auto compute_transformation_difference =
+    [](const Eigen::Isometry3d & t1, const Eigen::Isometry3d & t2) -> std::pair<double, double> {
+    double translation_difference = (t2.inverse() * t1).translation().norm();
+    double rotation_difference =
+      std::acos(std::min(1.0, 0.5 * ((t2.rotation().inverse() * t1.rotation()).trace() - 1.0)));
+
+    return std::make_pair(translation_difference, rotation_difference);
+  };
+
+  auto [calibrated_translation_difference, calibrated_rotation_difference] =
     compute_transformation_difference(
-      initial_radar_to_lidar_eigen_, calibrated_rotation_radar_to_lidar_transformation);
+      initial_radar_to_lidar_eigen_, calibrated_radar_to_lidar_transformation);
 
   std::unique_lock<std::mutex> lock(mutex_);
   if (
-    calibrated_2d_translation_difference < parameters_.max_initial_calibration_translation_error &&
-    calibrated_2d_rotation_difference < parameters_.max_initial_calibration_rotation_error) {
-    RCLCPP_INFO(
-      this->get_logger(), "The 2D calibration pose was chosen as the output calibration pose");
-    calibrated_radar_to_lidar_eigen_ = calibrated_2d_radar_to_lidar_transformation;
-    calibration_valid_ = true;
-    calibration_distance_score_ = calibrated_2d_distance_error;
-    calibration_yaw_score_ = calibrated_2d_yaw_error;
-  } else if (
-    calibrated_rotation_translation_difference <
-      parameters_.max_initial_calibration_translation_error &&
-    calibrated_rotation_rotation_difference < parameters_.max_initial_calibration_rotation_error) {
-    RCLCPP_WARN(
-      this->get_logger(),
-      "The pure rotation calibration pose was chosen as the output calibration pose. This may mean "
-      "you need to collect more points");
-    calibrated_radar_to_lidar_eigen_ = calibrated_rotation_radar_to_lidar_transformation;
+    calibrated_translation_difference < parameters_.max_initial_calibration_translation_error &&
+    calibrated_rotation_difference < parameters_.max_initial_calibration_rotation_error) {
+    calibrated_radar_to_lidar_eigen_ = calibrated_radar_to_lidar_transformation;
     calibration_valid_ = true;
-    calibration_distance_score_ = calibrated_rotation_distance_error;
-    calibration_yaw_score_ = calibrated_rotation_yaw_error;
+    calibration_distance_score_ = calibrated_distance_error;
+    calibration_yaw_score_ = calibrated_yaw_error;
   } else {
     RCLCPP_WARN(
       this->get_logger(),
       "The calibrated poses differ considerably with the initial calibration. This may be either a "
       "fault of the algorithm or a bad calibration initialization");
   }
-
   output_metrics_.push_back(static_cast<float>(converged_tracks_.size()));
-  output_metrics_.push_back(static_cast<float>(calibrated_2d_distance_error));
-  output_metrics_.push_back(static_cast<float>(calibrated_2d_yaw_error));
+  output_metrics_.push_back(static_cast<float>(calibrated_distance_error));
+  output_metrics_.push_back(static_cast<float>(calibrated_yaw_error));
 }
 
 void ExtrinsicReflectorBasedCalibrator::findCombinations(
-  int n, int k, std::vector<int> & curr, int first_num,
-  std::vector<std::vector<int>> & combinations)
+  std::size_t n, std::size_t k, std::vector<std::size_t> & curr, std::size_t first_num,
+  std::vector<std::vector<std::size_t>> & combinations)
 {
-  int curr_size = static_cast<int>(curr.size());
+  auto curr_size = curr.size();
   if (curr_size == k) {
     combinations.push_back(curr);
     return;
   }
 
-  int need = k - curr_size;
-  int remain = n - first_num + 1;
-  int available = remain - need;
+  auto need = k - curr_size;
+  auto remain = n - first_num + 1;
+  auto available = remain - need;
 
-  for (int num = first_num; num <= first_num + available; num++) {
+  for (auto num = first_num; num <= first_num + available; num++) {
     curr.push_back(num);
     findCombinations(n, k, curr, num + 1, combinations);
     curr.pop_back();
@@ -1408,108 +1553,130 @@ void ExtrinsicReflectorBasedCalibrator::findCombinations(
   return;
 }
 
-void ExtrinsicReflectorBasedCalibrator::crossValEvaluation(
-  pcl::PointCloud<PointType>::Ptr lidar_points_pcs,
-  pcl::PointCloud<PointType>::Ptr radar_points_rcs)
+void ExtrinsicReflectorBasedCalibrator::selectCombinations(
+  std::size_t tracks_size, std::size_t num_of_samples,
+  std::vector<std::vector<std::size_t>> & combinations)
 {
-  // Note: pcs=parallel coordinate system rcs=radar coordinate system
-  int tracks_size = static_cast<int>(converged_tracks_.size());
-  if (tracks_size <= 3) return;
-
-  pcl::PointCloud<PointType>::Ptr crossval_lidar_points_pcs(new pcl::PointCloud<PointType>);
-  pcl::PointCloud<PointType>::Ptr crossval_radar_points_rcs(new pcl::PointCloud<PointType>);
-  pcl::registration::TransformationEstimationSVD<PointType, PointType> crossval_estimator;
-  Eigen::Matrix4f crossval_radar_to_radar_parallel_transformation;
-  Eigen::Isometry3d crossval_calibrated_2d_radar_to_radar_parallel_transformation;
-  Eigen::Isometry3d crossval_calibrated_2d_radar_to_lidar_transformation;
-
-  for (int num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) {
-    crossval_lidar_points_pcs->reserve(num_of_samples);
-    crossval_radar_points_rcs->reserve(num_of_samples);
-    std::vector<std::vector<int>> combinations;
-    std::vector<int> curr;
-    std::vector<double> crossval_calibrated_2d_distance_error_vector;
-    std::vector<double> crossval_calibrated_2d_yaw_error_vector;
-    double total_crossval_calibrated_2d_distance_error = 0.0;
-    double total_crossval_calibrated_2d_yaw_error = 0.0;
-
-    findCombinations(tracks_size - 1, num_of_samples, curr, 0, combinations);
-
-    RCLCPP_INFO(
+  RCLCPP_INFO(
+    this->get_logger(),
+    "Current number of combinations is: %zu, converged_tracks_size: %zu, num_of_samples: %zu",
+    combinations.size(), tracks_size, num_of_samples);
+
+  // random select the combinations if the number of combinations is too large
+  if (combinations.size() > parameters_.max_number_of_combination_samples) {
+    std::random_device rd;
+    std::mt19937 mt(rd());
+    std::shuffle(combinations.begin(), combinations.end(), mt);
+    combinations.resize(parameters_.max_number_of_combination_samples);
+    RCLCPP_WARN(
       this->get_logger(),
-      "The number of combinations is: %d, converged_tracks_size: %d, num_of_samples: %d",
-      static_cast<int>(combinations.size()), tracks_size, num_of_samples);
+      "The number of combinations is set to: %zu, because it exceeds the maximum number of "
+      "combination samples: %zu",
+      combinations.size(), parameters_.max_number_of_combination_samples);
+  }
+}
 
-    // random select the combinations if the number of combinations is too large
-    if (
-      combinations.size() >
-      static_cast<std::size_t>(parameters_.max_number_of_combination_samples)) {
-      std::random_device rd;
-      std::mt19937 mt(rd());
-      std::shuffle(combinations.begin(), combinations.end(), mt);
-      combinations.resize(parameters_.max_number_of_combination_samples);
-      RCLCPP_WARN(
-        this->get_logger(),
-        "The number of combinations is set to: %d, because it exceeds the maximum number of "
-        "combination samples: %d",
-        static_cast<int>(combinations.size()), parameters_.max_number_of_combination_samples);
-    }
+void ExtrinsicReflectorBasedCalibrator::evaluateCombinations(
+  std::vector<std::vector<std::size_t>> & combinations, std::size_t num_of_samples,
+  TransformationResult transformation_result)
+{
+  TransformationEstimator crossval_estimator(
+    initial_radar_to_lidar_eigen_, initial_radar_optimization_to_radar_eigen_,
+    radar_optimization_to_lidar_eigen_);
+
+  pcl::PointCloud<common_types::PointType>::Ptr crossval_lidar_points_ocs(
+    new pcl::PointCloud<common_types::PointType>);
+  pcl::PointCloud<common_types::PointType>::Ptr crossval_radar_points_rcs(
+    new pcl::PointCloud<common_types::PointType>);
+  std::vector<Track> crossval_converged_tracks_;
+  crossval_lidar_points_ocs->reserve(num_of_samples);
+  crossval_radar_points_rcs->reserve(num_of_samples);
+  crossval_converged_tracks_.reserve(num_of_samples);
+
+  double total_crossval_calibrated_distance_error = 0.0;
+  double total_crossval_calibrated_yaw_error = 0.0;
+  std::vector<double> crossval_calibrated_distance_error_vector;
+  std::vector<double> crossval_calibrated_yaw_error_vector;
+
+  for (const auto & combination : combinations) {
+    if (transformation_type_ == TransformationType::yaw_only_rotation_2d) {
+      crossval_converged_tracks_.clear();
+
+      for (std::size_t i = 0; i < combination.size(); i++) {
+        crossval_converged_tracks_.push_back(converged_tracks_[i]);
+      }
+      auto [delta_cos, delta_sin] = get2DRotationDelta(crossval_converged_tracks_, true);
 
-    for (const auto & combination : combinations) {
-      // clear the lidar radar pcs
-      crossval_lidar_points_pcs->clear();
+      crossval_estimator.set2DRotationDelta(delta_cos, delta_sin);
+      crossval_estimator.estimateYawOnlyTransformation();
+    } else {
+      crossval_lidar_points_ocs->clear();
       crossval_radar_points_rcs->clear();
+
       // calculate the transformation.
-      for (int j = 0; j < num_of_samples; j++) {
-        crossval_lidar_points_pcs->emplace_back(lidar_points_pcs->points[combination[j]]);
-        crossval_radar_points_rcs->emplace_back(radar_points_rcs->points[combination[j]]);
+      for (std::size_t i = 0; i < combination.size(); i++) {
+        crossval_lidar_points_ocs->emplace_back(
+          transformation_result.lidar_points_ocs->points[combination[i]]);
+        crossval_radar_points_rcs->emplace_back(
+          transformation_result.radar_points_rcs->points[combination[i]]);
       }
-      crossval_estimator.estimateRigidTransformation(
-        *crossval_lidar_points_pcs, *crossval_radar_points_rcs,
-        crossval_radar_to_radar_parallel_transformation);
-      crossval_calibrated_2d_radar_to_radar_parallel_transformation =
-        crossval_radar_to_radar_parallel_transformation.cast<double>();
-      crossval_calibrated_2d_radar_to_radar_parallel_transformation.translation().z() =
-        (initial_radar_to_lidar_eigen_ * radar_parallel_to_lidar_eigen_.inverse())
-          .translation()
-          .z();
-      crossval_calibrated_2d_radar_to_lidar_transformation =
-        crossval_calibrated_2d_radar_to_radar_parallel_transformation *
-        radar_parallel_to_lidar_eigen_;
-
-      // calculate the error.
-      auto [crossval_calibrated_2d_distance_error, crossval_calibrated_2d_yaw_error] =
-        computeCalibrationError(crossval_calibrated_2d_radar_to_lidar_transformation);
-
-      total_crossval_calibrated_2d_distance_error += crossval_calibrated_2d_distance_error;
-      total_crossval_calibrated_2d_yaw_error += crossval_calibrated_2d_yaw_error;
-      crossval_calibrated_2d_distance_error_vector.push_back(crossval_calibrated_2d_distance_error);
-      crossval_calibrated_2d_yaw_error_vector.push_back(crossval_calibrated_2d_yaw_error);
+      crossval_estimator.setPoints(crossval_lidar_points_ocs, crossval_radar_points_rcs);
+      if (transformation_type_ == TransformationType::zero_roll_3d)
+        crossval_estimator.estimateZeroRollTransformation();
+      else
+        crossval_estimator.estimateSVDTransformation(transformation_type_);
     }
 
-    auto calculate_std = [](std::vector<double> & data, double mean) -> double {
-      double sum = 0.0;
-      for (size_t i = 0; i < data.size(); i++) {
-        sum += (data[i] - mean) * (data[i] - mean);
-      }
-      double variance = sum / data.size();
-      return std::sqrt(variance);
-    };
-
-    double avg_crossval_calibrated_2d_distance_error =
-      total_crossval_calibrated_2d_distance_error / combinations.size();
-    double avg_crossval_calibrated_2d_yaw_error =
-      total_crossval_calibrated_2d_yaw_error / combinations.size();
-    output_metrics_.push_back(static_cast<float>(num_of_samples));
-    output_metrics_.push_back(static_cast<float>(avg_crossval_calibrated_2d_distance_error));
-    output_metrics_.push_back(static_cast<float>(avg_crossval_calibrated_2d_yaw_error));
-
-    double std_crossval_calibrated_2d_distance_error = calculate_std(
-      crossval_calibrated_2d_distance_error_vector, avg_crossval_calibrated_2d_distance_error);
-    double std_crossval_calibrated_2d_yaw_error =
-      calculate_std(crossval_calibrated_2d_yaw_error_vector, avg_crossval_calibrated_2d_yaw_error);
-    output_metrics_.push_back(static_cast<float>(std_crossval_calibrated_2d_distance_error));
-    output_metrics_.push_back(static_cast<float>(std_crossval_calibrated_2d_yaw_error));
+    Eigen::Isometry3d crossval_calibrated_radar_to_lidar_transformation =
+      crossval_estimator.getTransformation();
+    // calculate the error.
+    auto [crossval_calibrated_distance_error, crossval_calibrated_yaw_error] =
+      computeCalibrationError(crossval_calibrated_radar_to_lidar_transformation);
+
+    total_crossval_calibrated_distance_error += crossval_calibrated_distance_error;
+    total_crossval_calibrated_yaw_error += crossval_calibrated_yaw_error;
+    crossval_calibrated_distance_error_vector.push_back(crossval_calibrated_distance_error);
+    crossval_calibrated_yaw_error_vector.push_back(crossval_calibrated_yaw_error);
+  }
+
+  auto calculate_std = [](std::vector<double> & data, double mean) -> double {
+    double sum = 0.0;
+    for (std::size_t i = 0; i < data.size(); i++) {
+      sum += (data[i] - mean) * (data[i] - mean);
+    }
+    double variance = sum / data.size();
+    return std::sqrt(variance);
+  };
+
+  double avg_crossval_calibrated_distance_error =
+    total_crossval_calibrated_distance_error / combinations.size();
+  double avg_crossval_calibrated_yaw_error =
+    total_crossval_calibrated_yaw_error / combinations.size();
+  output_metrics_.push_back(static_cast<float>(num_of_samples));
+  output_metrics_.push_back(static_cast<float>(avg_crossval_calibrated_distance_error));
+  output_metrics_.push_back(static_cast<float>(avg_crossval_calibrated_yaw_error));
+
+  double std_crossval_calibrated_distance_error = calculate_std(
+    crossval_calibrated_distance_error_vector, avg_crossval_calibrated_distance_error);
+  double std_crossval_calibrated_yaw_error =
+    calculate_std(crossval_calibrated_yaw_error_vector, avg_crossval_calibrated_yaw_error);
+  output_metrics_.push_back(static_cast<float>(std_crossval_calibrated_distance_error));
+  output_metrics_.push_back(static_cast<float>(std_crossval_calibrated_yaw_error));
+}
+
+void ExtrinsicReflectorBasedCalibrator::crossValEvaluation(
+  TransformationResult transformation_result)
+{
+  auto tracks_size = converged_tracks_.size();
+  if (tracks_size <= 3) return;
+
+  for (std::size_t num_of_samples = 3; num_of_samples < tracks_size; num_of_samples++) {
+    std::vector<std::vector<std::size_t>> combinations;
+    std::vector<std::size_t> curr;
+
+    findCombinations(tracks_size - 1, num_of_samples, curr, 0, combinations);
+    selectCombinations(tracks_size, num_of_samples, combinations);
+    evaluateCombinations(combinations, num_of_samples, transformation_result);
   }
 }
 
@@ -1534,13 +1701,10 @@ void ExtrinsicReflectorBasedCalibrator::calibrateSensors()
     }
     return;
   }
-
   output_metrics_.clear();
-
-  // Note: pcs=parallel coordinate system rcs=radar coordinate system
-  auto [lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum] = getPointsSetAndDelta();
-  estimateTransformation(lidar_points_pcs, radar_points_rcs, delta_cos_sum, delta_sin_sum);
-  crossValEvaluation(lidar_points_pcs, radar_points_rcs);
+  auto transformation_result = estimateTransformation();
+  evaluateTransformation(transformation_result.calibrated_radar_to_lidar_transformation);
+  crossValEvaluation(transformation_result);
   publishMetrics();
 }
 
@@ -1821,9 +1985,21 @@ geometry_msgs::msg::Point ExtrinsicReflectorBasedCalibrator::eigenToPointMsg(
   return p;
 }
 
-double ExtrinsicReflectorBasedCalibrator::getYawError(
-  const Eigen::Vector3d & v1, const Eigen::Vector3d & v2)
+double ExtrinsicReflectorBasedCalibrator::getDistanceError(Eigen::Vector3d v1, Eigen::Vector3d v2)
+{
+  if (
+    transformation_type_ == TransformationType::yaw_only_rotation_2d ||
+    transformation_type_ == TransformationType::svd_2d) {
+    v1.z() = 0.0;
+    v2.z() = 0.0;
+  }
+  return (v1 - v2).norm();
+}
+
+double ExtrinsicReflectorBasedCalibrator::getYawError(Eigen::Vector3d v1, Eigen::Vector3d v2)
 {
+  v1.z() = 0.0;
+  v2.z() = 0.0;
   return std::abs(std::acos(v1.dot(v2) / (v1.norm() * v2.norm())));
 }
 
diff --git a/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp
new file mode 100644
index 00000000..72681bf8
--- /dev/null
+++ b/calibrators/marker_radar_lidar_calibrator/src/transformation_estimator.cpp
@@ -0,0 +1,189 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <marker_radar_lidar_calibrator/marker_radar_lidar_calibrator.hpp>
+#include <marker_radar_lidar_calibrator/sensor_residual.hpp>
+#include <marker_radar_lidar_calibrator/transformation_estimator.hpp>
+
+#include <numeric>
+
+namespace marker_radar_lidar_calibrator
+{
+
+TransformationEstimator::TransformationEstimator(
+  Eigen::Isometry3d initial_radar_to_lidar_eigen,
+  Eigen::Isometry3d initial_radar_optimization_to_radar_eigen,
+  Eigen::Isometry3d radar_optimization_to_lidar_eigen)
+{
+  initial_radar_to_lidar_eigen_ = initial_radar_to_lidar_eigen;
+  initial_radar_optimization_to_radar_eigen_ = initial_radar_optimization_to_radar_eigen;
+  radar_optimization_to_lidar_eigen_ = radar_optimization_to_lidar_eigen;
+}
+
+void TransformationEstimator::setPoints(
+  pcl::PointCloud<common_types::PointType>::Ptr lidar_points_ocs,
+  pcl::PointCloud<common_types::PointType>::Ptr radar_points_rcs)
+{
+  lidar_points_ocs_ = lidar_points_ocs;
+  radar_points_rcs_ = radar_points_rcs;
+}
+
+void TransformationEstimator::set2DRotationDelta(double delta_cos, double delta_sin)
+{
+  delta_cos_ = delta_cos;
+  delta_sin_ = delta_sin;
+}
+
+void TransformationEstimator::estimateYawOnlyTransformation()
+{
+  RCLCPP_INFO(
+    rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate yaw only 2d transformation");
+  Eigen::Matrix3d delta_rotation;
+  delta_rotation << delta_cos_, -delta_sin_, 0.0, delta_sin_, delta_cos_, 0.0, 0.0, 0.0, 1.0;
+  Eigen::Isometry3d delta_transformation = Eigen::Isometry3d::Identity();
+  delta_transformation.linear() = delta_rotation;
+
+  calibrated_radar_to_lidar_transformation_ = delta_transformation * initial_radar_to_lidar_eigen_;
+}
+
+void TransformationEstimator::estimateSVDTransformation(
+  ExtrinsicReflectorBasedCalibrator::TransformationType transformation_type)
+{
+  if (transformation_type == ExtrinsicReflectorBasedCalibrator::TransformationType::svd_2d) {
+    RCLCPP_INFO(
+      rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate 2D SVD transformation");
+  } else {
+    RCLCPP_INFO(
+      rclcpp::get_logger("marker_radar_lidar_calibrator"), "Estimate 3D SVD transformation");
+  }
+
+  pcl::registration::TransformationEstimationSVD<common_types::PointType, common_types::PointType>
+    estimator;
+  Eigen::Matrix4f full_radar_to_radar_optimization_transformation;
+  estimator.estimateRigidTransformation(
+    *lidar_points_ocs_, *radar_points_rcs_, full_radar_to_radar_optimization_transformation);
+  Eigen::Isometry3d calibrated_radar_to_radar_optimization_transformation(
+    full_radar_to_radar_optimization_transformation.cast<double>());
+
+  if (transformation_type == ExtrinsicReflectorBasedCalibrator::TransformationType::svd_2d) {
+    // Check that is is actually a 2D transformation
+    auto calibrated_radar_to_radar_optimization_rpy = autoware::universe_utils::getRPY(
+      tf2::toMsg(calibrated_radar_to_radar_optimization_transformation).orientation);
+    double calibrated_radar_to_radar_optimization_z =
+      calibrated_radar_to_radar_optimization_transformation.translation().z();
+    double calibrated_radar_to_radar_optimization_roll =
+      calibrated_radar_to_radar_optimization_rpy.x;
+    double calibrated_radar_to_radar_optimization_pitch =
+      calibrated_radar_to_radar_optimization_rpy.y;
+
+    if (
+      calibrated_radar_to_radar_optimization_z != 0.0 ||
+      calibrated_radar_to_radar_optimization_roll != 0.0 ||
+      calibrated_radar_to_radar_optimization_pitch != 0.0) {
+      RCLCPP_ERROR(
+        rclcpp::get_logger("marker_radar_lidar_calibrator"),
+        "The estimated 2D translation was not really 2D. Continue at your own risk. z=%.3f "
+        "roll=%.3f "
+        "pitch=%.3f",
+        calibrated_radar_to_radar_optimization_z, calibrated_radar_to_radar_optimization_roll,
+        calibrated_radar_to_radar_optimization_pitch);
+    }
+
+    calibrated_radar_to_radar_optimization_transformation.translation().z() =
+      (initial_radar_to_lidar_eigen_ * radar_optimization_to_lidar_eigen_.inverse())
+        .translation()
+        .z();
+  }
+
+  calibrated_radar_to_lidar_transformation_ =
+    calibrated_radar_to_radar_optimization_transformation * radar_optimization_to_lidar_eigen_;
+}
+
+void TransformationEstimator::estimateZeroRollTransformation()
+{
+  RCLCPP_INFO(
+    rclcpp::get_logger("marker_radar_lidar_calibrator"),
+    "Estimate the 3D transformation by restricting the roll to zero");
+
+  ceres::Problem problem;
+
+  Eigen::Vector3d translation = initial_radar_optimization_to_radar_eigen_.translation();
+  Eigen::Matrix3d rotation = initial_radar_optimization_to_radar_eigen_.rotation();
+  Eigen::Vector3d euler_angle = rotation.eulerAngles(0, 1, 2);
+
+  // params: x, y, z, pitch, yaw
+  std::vector<double> params = {
+    translation[0], translation[1], translation[2], euler_angle[1], euler_angle[2]};
+
+  std::string initial_params_str = std::accumulate(
+    std::next(params.begin()), params.end(),
+    std::to_string(params.front()),  // Initialize with the first element
+    [](const std::string & a, const auto & b) { return a + " " + std::to_string(b); });
+  std::string initial_params_msg = "initial params (x,y,z,pith,yaw): " + initial_params_str;
+  RCLCPP_INFO(
+    rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", initial_params_msg.c_str());
+
+  for (std::size_t i = 0; i < lidar_points_ocs_->points.size(); i++) {
+    auto lidar_point = lidar_points_ocs_->points[i];
+    auto radar_point = radar_points_rcs_->points[i];
+
+    Eigen::Vector4d radar_point_eigen(radar_point.x, radar_point.y, radar_point.z, 1);
+    Eigen::Vector4d lidar_point_eigen(lidar_point.x, lidar_point.y, lidar_point.z, 1);
+
+    ceres::CostFunction * cost_function = new ceres::AutoDiffCostFunction<SensorResidual, 3, 5>(
+      new SensorResidual(radar_point_eigen, lidar_point_eigen));
+    problem.AddResidualBlock(cost_function, nullptr, params.data());
+  }
+
+  // Solve
+  ceres::Solver::Options options;
+  options.linear_solver_type = ceres::DENSE_SCHUR;  // cSpell:ignore schur
+  options.minimizer_progress_to_stdout = true;
+  options.max_num_iterations = 500;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+  std::string report = summary.FullReport();
+  RCLCPP_INFO(rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", report.c_str());
+
+  std::string calibrated_params_str = std::accumulate(
+    std::next(params.begin()), params.end(),
+    std::to_string(params.front()),  // Initialize with the first element
+    [](const std::string & a, const auto & b) { return a + " " + std::to_string(b); });
+  std::string calibrated_params_msg =
+    "calibrated params (x,y,z,pitch,yaw): " + calibrated_params_str;
+  RCLCPP_INFO(
+    rclcpp::get_logger("marker_radar_lidar_calibrator"), "%s", calibrated_params_msg.c_str());
+
+  Eigen::Isometry3d calibrated_3d_radar_optimization_to_radar_transformation =
+    Eigen::Isometry3d::Identity();
+  calibrated_3d_radar_optimization_to_radar_transformation.pretranslate(
+    Eigen::Vector3d(params[0], params[1], params[2]));
+  Eigen::Quaterniond q(
+    Eigen::AngleAxisd(params[4], Eigen::Vector3d::UnitZ()) *
+    Eigen::AngleAxisd(params[3], Eigen::Vector3d::UnitY()) *
+    Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
+  calibrated_3d_radar_optimization_to_radar_transformation.rotate(q);
+
+  calibrated_radar_to_lidar_transformation_ =
+    calibrated_3d_radar_optimization_to_radar_transformation.inverse() *
+    radar_optimization_to_lidar_eigen_;
+}
+
+Eigen::Isometry3d TransformationEstimator::getTransformation()
+{
+  return calibrated_radar_to_lidar_transformation_;
+}
+
+}  // namespace marker_radar_lidar_calibrator
diff --git a/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml
index 2393b0e7..310278af 100644
--- a/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml
+++ b/sensor_calibration_manager/launch/default_project/marker_radar_lidar_calibrator.launch.xml
@@ -2,11 +2,13 @@
 <launch>
   <arg name="rviz" default="true"/>
 
-  <arg name="radar_parallel_frame"/>
+  <arg name="radar_optimization_frame"/>
   <arg name="radar_frame"/>
   <arg name="lidar_frame"/>
+  <arg name="msg_type"/>
+  <arg name="transformation_type"/>
 
-  <arg name="input_radar_objects"/>
+  <arg name="input_radar_msg"/>
   <arg name="input_lidar_pointcloud"/>
 
   <node pkg="tf2_ros" exec="static_transform_publisher" name="lidar_broadcaster" output="screen" args="0 0 0 0 0 0 $(var radar_frame) radar_frame"/>
@@ -15,8 +17,10 @@
   <include file="$(find-pkg-share marker_radar_lidar_calibrator)/launch/calibrator.launch.xml">
     <arg name="rviz" value="$(var rviz)"/>
     <arg name="calibration_service_name" value="calibrate_radar_lidar"/>
-    <arg name="radar_parallel_frame" value="$(var radar_parallel_frame)"/>
+    <arg name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
     <arg name="input_lidar_pointcloud" value="$(var input_lidar_pointcloud)"/>
-    <arg name="input_radar_objects" value="$(var input_radar_objects)"/>
+    <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
+    <arg name="msg_type" value="$(var msg_type)"/>
+    <arg name="transformation_type" value="$(var transformation_type)"/>
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml
index e4034e2e..9f706f81 100644
--- a/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml
+++ b/sensor_calibration_manager/launch/rdv/marker_radar_lidar_calibrator.launch.xml
@@ -9,13 +9,30 @@
     <choice value="rear_right"/>
   </arg>
 
+  <arg name="msg_type" default="radar_tracks">
+    <choice value="radar_tracks"/>
+    <choice value="radar_scan"/>
+    <choice value="radar_cloud"/>
+  </arg>
+
+  <arg name="transformation_type" default="svd_2d">
+    <choice value="svd_2d"/>
+    <choice value="yaw_only_rotation_2d"/>
+    <choice value="svd_3d"/>
+    <choice value="zero_roll_3d"/>
+  </arg>
+
   <arg name="rviz" default="true"/>
 
-  <let name="radar_parallel_frame" value="base_link"/>
+  <let name="radar_optimization_frame" value="base_link"/>
   <let name="radar_frame" value="$(var radar_name)/radar_link"/>
   <let name="lidar_frame" value="pandar_top"/>
+  <let name="msg_type" value="$(var msg_type)"/>
+  <let name="transformation_type" value="$(var transformation_type)"/>
 
-  <let name="input_radar_objects" value="/sensing/radar/$(var radar_name)/objects_raw"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/objects_raw" if="$(eval &quot;'$(var msg_type)'=='radar_tracks'&quot;)"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/scan_raw" if="$(eval &quot;'$(var msg_type)'=='radar_scan'&quot;)"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/detection_points" if="$(eval &quot;'$(var msg_type)'=='radar_cloud'&quot;)"/>
   <let name="input_lidar_pointcloud" value="/sensing/lidar/top/pointcloud_raw"/>
 
   <node pkg="tf2_ros" exec="static_transform_publisher" name="lidar_broadcaster" output="screen" args="0 0 0 0 0 0 $(var radar_frame) radar_frame"/>
@@ -24,8 +41,10 @@
   <include file="$(find-pkg-share marker_radar_lidar_calibrator)/launch/calibrator.launch.xml">
     <arg name="rviz" value="$(var rviz)"/>
     <arg name="calibration_service_name" value="calibrate_radar_lidar"/>
-    <arg name="radar_parallel_frame" value="$(var radar_parallel_frame)"/>
+    <arg name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
     <arg name="input_lidar_pointcloud" value="$(var input_lidar_pointcloud)"/>
-    <arg name="input_radar_objects" value="$(var input_radar_objects)"/>
+    <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
+    <arg name="msg_type" value="$(var msg_type)"/>
+    <arg name="transformation_type" value="$(var transformation_type)"/>
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml
index 10b47c0a..80ed5000 100644
--- a/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml
+++ b/sensor_calibration_manager/launch/x2/marker_radar_lidar_calibrator.launch.xml
@@ -9,14 +9,29 @@
     <choice value="rear_right"/>
   </arg>
 
+  <arg name="msg_type" default="radar_tracks">
+    <choice value="radar_tracks"/>
+    <choice value="radar_scan"/>
+    <choice value="radar_cloud"/>
+  </arg>
+
+  <arg name="transformation_type" default="svd_2d">
+    <choice value="svd_2d"/>
+    <choice value="yaw_only_rotation_2d"/>
+    <choice value="svd_3d"/>
+    <choice value="zero_roll_3d"/>
+  </arg>
+
   <arg name="rviz" default="true"/>
 
-  <let name="radar_parallel_frame" value="front_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_left' &quot;)"/>
-  <let name="radar_parallel_frame" value="front_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_center' &quot;)"/>
-  <let name="radar_parallel_frame" value="front_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_right' &quot;)"/>
-  <let name="radar_parallel_frame" value="rear_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_left' &quot;)"/>
-  <let name="radar_parallel_frame" value="rear_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_center' &quot;)"/>
-  <let name="radar_parallel_frame" value="rear_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_right' &quot;)"/>
+  <!-- For x2 project, do not change radar_optimization_frame to base_link, if you are using radar that doesn't support elevation or z axis-->
+  <let name="radar_optimization_frame" value="front_unit_base_link"/>
+  <let name="radar_parent_frame" value="front_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_left' &quot;)"/>
+  <let name="radar_parent_frame" value="front_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_center' &quot;)"/>
+  <let name="radar_parent_frame" value="front_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'front_right' &quot;)"/>
+  <let name="radar_parent_frame" value="rear_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_left' &quot;)"/>
+  <let name="radar_parent_frame" value="rear_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_center' &quot;)"/>
+  <let name="radar_parent_frame" value="rear_unit_base_link" if="$(eval &quot;'$(var radar_name)' == 'rear_right' &quot;)"/>
 
   <let name="radar_frame" value="$(var radar_name)/radar_link"/>
 
@@ -27,7 +42,9 @@
   <let name="lidar_frame" value="pandar_40p_rear" if="$(eval &quot;'$(var radar_name)' == 'rear_center' &quot;)"/>
   <let name="lidar_frame" value="pandar_40p_rear" if="$(eval &quot;'$(var radar_name)' == 'rear_right' &quot;)"/>
 
-  <let name="input_radar_objects" value="/sensing/radar/$(var radar_name)/objects_raw"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/objects_raw" if="$(eval &quot;'$(var msg_type)'=='radar_tracks'&quot;)"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/scan_raw" if="$(eval &quot;'$(var msg_type)'=='radar_scan'&quot;)"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/detection_points" if="$(eval &quot;'$(var msg_type)'=='radar_cloud'&quot;)"/>
 
   <let name="input_lidar_pointcloud" value="/sensing/lidar/front_lower/pointcloud_raw" if="$(eval &quot;'$(var radar_name)' == 'front_left' &quot;)"/>
   <let name="input_lidar_pointcloud" value="/sensing/lidar/front_lower/pointcloud_raw" if="$(eval &quot;'$(var radar_name)' == 'front_center' &quot;)"/>
@@ -42,8 +59,10 @@
   <include file="$(find-pkg-share marker_radar_lidar_calibrator)/launch/calibrator.launch.xml">
     <arg name="rviz" value="$(var rviz)"/>
     <arg name="calibration_service_name" value="calibrate_radar_lidar"/>
-    <arg name="radar_parallel_frame" value="$(var radar_parallel_frame)"/>
+    <arg name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
     <arg name="input_lidar_pointcloud" value="$(var input_lidar_pointcloud)"/>
-    <arg name="input_radar_objects" value="$(var input_radar_objects)"/>
+    <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
+    <arg name="msg_type" value="$(var msg_type)"/>
+    <arg name="transformation_type" value="$(var transformation_type)"/>
   </include>
 </launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/ground_plane_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/ground_plane_calibrator.launch.xml
new file mode 100644
index 00000000..11b9c7ce
--- /dev/null
+++ b/sensor_calibration_manager/launch/xx1_gen2/ground_plane_calibrator.launch.xml
@@ -0,0 +1,54 @@
+<launch>
+  <let name="base_frame" value="base_link"/>
+  <let name="lidar_frame" value="hesai_top"/>
+  <let name="pointcloud_topic" value="/sensing/lidar/top/pointcloud_raw_ex"/>
+
+  <arg name="max_inlier_distance" default="0.03" description="Maximum distance in meters to consider inliers in plane estimation"/>
+  <arg name="min_plane_points" default="500" description="Minimum number of points for an hypothesis for be considered valid"/>
+  <arg name="min_plane_points_percentage" default="20.0" description="Minimum percentage of the total points to consider an hypothesis valid"/>
+  <arg name="max_cos_distance" default="0.2" description="Maximum cosine distance between an hypothesis and the initial calibration for an hypothesis to be considered valid"/>
+  <arg name="max_iterations" default="500" description="Number of maximum iterations in the plane estimation algorithm"/>
+  <arg name="verbose" default="true" description="Verbose mode"/>
+  <arg name="overwrite_xy_yaw" default="false" description="Overwrite x, y, and yaw values with the initial calibration"/>
+  <arg name="filter_estimations" default="true" description="Flag to enable filtering estimations"/>
+
+  <arg name="use_crop_box_filter" default="true" description="Use an optional crop box filter to accelerate processing and ignore walls or other planes"/>
+  <arg name="crop_box_min_x" default="-50.0" description="Minimum x value of the crop box filter"/>
+  <arg name="crop_box_min_y" default="-50.0" description="Minimum y value of the crop box filter"/>
+  <arg name="crop_box_min_z" default="-50.0" description="Minimum z value of the crop box filter"/>
+  <arg name="crop_box_max_x" default="50.0" description="Maximum x value of the crop box filter"/>
+  <arg name="crop_box_max_y" default="50.0" description="Maximum y value of the crop box filter"/>
+  <arg name="crop_box_max_z" default="50.0" description="Maximum z value of the crop box filter"/>
+  <arg name="use_pca_rough_normal" default="false" description="Whether to use PCA or the initial calibration for normal plane estimation"/>
+
+  <arg name="rviz" default="true" description="Launch rviz"/>
+
+  <!-- base-lidar calibrator -->
+  <include file="$(find-pkg-share ground_plane_calibrator)/launch/calibrator.launch.xml">
+    <arg name="calibration_service_name" value="calibrate_base_lidar"/>
+
+    <arg name="rviz" value="$(var rviz)"/>
+    <arg name="base_frame" value="$(var base_frame)"/>
+    <arg name="lidar_frame" value="$(var lidar_frame)"/>
+    <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
+    <arg name="calibration_service_name" value="$(var calibration_service_name)"/>
+
+    <arg name="max_inlier_distance" value="$(var max_inlier_distance)"/>
+    <arg name="min_plane_points" value="$(var min_plane_points)"/>
+    <arg name="min_plane_points_percentage" value="$(var min_plane_points_percentage)"/>
+    <arg name="max_cos_distance" value="$(var max_cos_distance)"/>
+    <arg name="max_iterations" value="$(var max_iterations)"/>
+    <arg name="verbose" value="$(var verbose)"/>
+    <arg name="overwrite_xy_yaw" value="$(var overwrite_xy_yaw)"/>
+    <arg name="filter_estimations" value="$(var filter_estimations)"/>
+
+    <arg name="use_crop_box_filter" value="$(var use_crop_box_filter)"/>
+    <arg name="crop_box_min_x" value="$(var crop_box_min_x)"/>
+    <arg name="crop_box_min_y" value="$(var crop_box_min_y)"/>
+    <arg name="crop_box_min_z" value="$(var crop_box_min_z)"/>
+    <arg name="crop_box_max_x" value="$(var crop_box_max_x)"/>
+    <arg name="crop_box_max_y" value="$(var crop_box_max_y)"/>
+    <arg name="crop_box_max_z" value="$(var crop_box_max_z)"/>
+    <arg name="use_pca_rough_normal" value="$(var use_pca_rough_normal)"/>
+  </include>
+</launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/mapping_based_base_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/mapping_based_base_lidar_calibrator.launch.xml
new file mode 100644
index 00000000..54d61d8e
--- /dev/null
+++ b/sensor_calibration_manager/launch/xx1_gen2/mapping_based_base_lidar_calibrator.launch.xml
@@ -0,0 +1,76 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="base_frame" default="base_link"/>
+
+  <arg name="rviz" default="true"/>
+
+  <arg name="lost_frame_max_acceleration" default="10.0"/>
+  <arg name="calibration_skip_keyframes" default="7"/>
+  <arg name="mapping_registrator" default="ndt" description="ndt or gicp"/>
+
+  <let name="calibrate_base_frame" value="true"/>
+  <arg name="base_lidar_crop_box_min_x" default="-5.0"/>
+  <arg name="base_lidar_crop_box_min_y" default="-5.0"/>
+  <arg name="base_lidar_crop_box_min_z" default="-5.0"/>
+  <arg name="base_lidar_crop_box_max_x" default="10.0"/>
+  <arg name="base_lidar_crop_box_max_y" default="5.0"/>
+  <arg name="base_lidar_crop_box_max_z" default="5.0"/>
+  <arg name="base_lidar_min_plane_points_percentage" default="10.0"/>
+  <arg name="base_lidar_max_inlier_distance" default="0.03"/>
+  <arg name="base_lidar_min_plane_points" default="500"/>
+  <arg name="base_lidar_max_cos_distance" default="0.2"/>
+  <arg name="base_lidar_max_iterations" default="500"/>
+  <arg name="base_lidar_overwrite_xy_yaw" default="false"/>
+
+  <let name="calibration_camera_optical_link_frames" value="['']"/>
+
+  <let name="calibration_lidar_frames" value="['']"/>
+
+  <let name="mapping_lidar_frame" value="hesai_top"/>
+  <let name="mapping_pointcloud" value="/sensing/lidar/top/pointcloud"/>
+  <let name="detected_objects" value="/perception/object_recognition/detection/objects"/>
+
+  <let name="calibration_camera_info_topics" value="['']"/>
+
+  <let name="calibration_image_topics" value="[
+    '']"/>
+
+  <let name="calibration_pointcloud_topics" value="['']"/>
+
+  <!-- mapping based calibrator -->
+  <include file="$(find-pkg-share mapping_based_calibrator)/launch/calibrator.launch.xml">
+    <arg name="ns" value=""/>
+    <arg name="calibration_service_name" value="calibrate_base_lidar"/>
+
+    <arg name="rviz" value="$(var rviz)"/>
+    <arg name="base_frame" value="$(var base_frame)"/>
+
+    <arg name="calibration_camera_optical_link_frames" value="$(var calibration_camera_optical_link_frames)"/>
+    <arg name="calibration_lidar_frames" value="$(var calibration_lidar_frames)"/>
+    <arg name="mapping_lidar_frame" value="$(var mapping_lidar_frame)"/>
+
+    <arg name="mapping_pointcloud" value="$(var mapping_pointcloud)"/>
+    <arg name="detected_objects" value="$(var detected_objects)"/>
+
+    <arg name="calibration_camera_info_topics" value="$(var calibration_camera_info_topics)"/>
+    <arg name="calibration_image_topics" value="$(var calibration_image_topics)"/>
+    <arg name="calibration_pointcloud_topics" value="$(var calibration_pointcloud_topics)"/>
+
+    <arg name="mapping_registrator" value="$(var mapping_registrator)"/>
+    <arg name="lost_frame_max_acceleration" value="$(var lost_frame_max_acceleration)"/>
+    <arg name="calibration_skip_keyframes" value="$(var calibration_skip_keyframes)"/>
+
+    <arg name="base_lidar_crop_box_min_x" value="$(var base_lidar_crop_box_min_x)"/>
+    <arg name="base_lidar_crop_box_min_y" value="$(var base_lidar_crop_box_min_y)"/>
+    <arg name="base_lidar_crop_box_min_z" value="$(var base_lidar_crop_box_min_z)"/>
+    <arg name="base_lidar_crop_box_max_x" value="$(var base_lidar_crop_box_max_x)"/>
+    <arg name="base_lidar_crop_box_max_y" value="$(var base_lidar_crop_box_max_y)"/>
+    <arg name="base_lidar_crop_box_max_z" value="$(var base_lidar_crop_box_max_z)"/>
+    <arg name="base_lidar_min_plane_points_percentage" value="$(var base_lidar_min_plane_points_percentage)"/>
+    <arg name="base_lidar_max_inlier_distance" value="$(var base_lidar_max_inlier_distance)"/>
+    <arg name="base_lidar_min_plane_points" value="$(var base_lidar_min_plane_points)"/>
+    <arg name="base_lidar_max_cos_distance" value="$(var base_lidar_max_cos_distance)"/>
+    <arg name="base_lidar_max_iterations" value="$(var base_lidar_max_iterations)"/>
+    <arg name="base_lidar_overwrite_xy_yaw" value="$(var base_lidar_overwrite_xy_yaw)"/>
+  </include>
+</launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/mapping_based_lidar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/mapping_based_lidar_lidar_calibrator.launch.xml
new file mode 100644
index 00000000..de7e2855
--- /dev/null
+++ b/sensor_calibration_manager/launch/xx1_gen2/mapping_based_lidar_lidar_calibrator.launch.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="base_frame" default="base_link"/>
+  <arg name="lost_frame_max_acceleration" default="10.0"/>
+  <arg name="calibration_skip_keyframes" default="7"/>
+  <arg name="mapping_registrator" default="ndt" description="ndt or gicp"/>
+  <arg name="rviz" default="true"/>
+
+  <let name="calibration_camera_optical_link_frames" value="['']"/>
+
+  <let name="calibration_lidar_frames" value="[
+    hesai_left,
+    hesai_right,
+    hesai_front_left,
+    hesai_front_right]"/>
+
+  <let name="mapping_lidar_frame" value="hesai_top"/>
+  <let name="mapping_pointcloud" value="/sensing/lidar/top/pointcloud"/>
+  <let name="detected_objects" value="/perception/object_recognition/detection/objects"/>
+
+  <let name="calibration_camera_info_topics" value="['']"/>
+
+  <let name="calibration_image_topics" value="[
+    '']"/>
+
+  <let
+    name="calibration_pointcloud_topics"
+    value="[
+    /sensing/lidar/left/rectified/pointcloud_ex,
+    /sensing/lidar/right/rectified/pointcloud_ex,
+    /sensing/lidar/front_left/rectified/pointcloud_ex,
+    /sensing/lidar/front_right/rectified/pointcloud_ex]"
+  />
+
+  <!-- mapping based calibrator -->
+  <include file="$(find-pkg-share mapping_based_calibrator)/launch/calibrator.launch.xml">
+    <arg name="ns" value=""/>
+    <arg name="calibration_service_name" value="calibrate_lidar_lidar"/>
+
+    <arg name="rviz" value="$(var rviz)"/>
+    <arg name="base_frame" value="$(var base_frame)"/>
+
+    <arg name="calibration_camera_optical_link_frames" value="$(var calibration_camera_optical_link_frames)"/>
+    <arg name="calibration_lidar_frames" value="$(var calibration_lidar_frames)"/>
+    <arg name="mapping_lidar_frame" value="$(var mapping_lidar_frame)"/>
+
+    <arg name="mapping_pointcloud" value="$(var mapping_pointcloud)"/>
+    <arg name="detected_objects" value="$(var detected_objects)"/>
+
+    <arg name="calibration_camera_info_topics" value="$(var calibration_camera_info_topics)"/>
+    <arg name="calibration_image_topics" value="$(var calibration_image_topics)"/>
+    <arg name="calibration_pointcloud_topics" value="$(var calibration_pointcloud_topics)"/>
+
+    <arg name="mapping_registrator" value="$(var mapping_registrator)"/>
+    <arg name="lost_frame_max_acceleration" value="$(var lost_frame_max_acceleration)"/>
+    <arg name="calibration_skip_keyframes" value="$(var calibration_skip_keyframes)"/>
+  </include>
+</launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml
new file mode 100644
index 00000000..c4ec2de4
--- /dev/null
+++ b/sensor_calibration_manager/launch/xx1_gen2/marker_radar_lidar_calibrator.launch.xml
@@ -0,0 +1,51 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="radar_name" default="front_center">
+    <choice value="front_left"/>
+    <choice value="front_center"/>
+    <choice value="front_right"/>
+    <choice value="rear_left"/>
+    <choice value="rear_center"/>
+    <choice value="rear_right"/>
+  </arg>
+
+  <arg name="msg_type" default="radar_tracks">
+    <choice value="radar_tracks"/>
+    <choice value="radar_scan"/>
+    <choice value="radar_cloud"/>
+  </arg>
+
+  <arg name="transformation_type" default="svd_2d">
+    <choice value="svd_2d"/>
+    <choice value="yaw_only_rotation_2d"/>
+    <choice value="svd_3d"/>
+    <choice value="zero_roll_3d"/>
+  </arg>
+
+  <arg name="rviz" default="true"/>
+
+  <let name="radar_optimization_frame" value="base_link"/>
+  <let name="radar_frame" value="$(var radar_name)/radar_link"/>
+  <let name="lidar_frame" value="hesai_top"/>
+  <let name="msg_type" value="$(var msg_type)"/>
+  <let name="transformation_type" value="$(var transformation_type)"/>
+
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/objects_raw" if="$(eval &quot;'$(var msg_type)'=='radar_tracks'&quot;)"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/scan_raw" if="$(eval &quot;'$(var msg_type)'=='radar_scan'&quot;)"/>
+  <let name="input_radar_msg" value="/sensing/radar/$(var radar_name)/detection_points" if="$(eval &quot;'$(var msg_type)'=='radar_cloud'&quot;)"/>
+
+  <let name="input_lidar_pointcloud" value="/sensing/lidar/top/pointcloud_raw"/>
+
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="lidar_broadcaster" output="screen" args="0 0 0 0 0 0 $(var radar_frame) radar_frame"/>
+
+  <!-- marker radar-lidar calibrator -->
+  <include file="$(find-pkg-share marker_radar_lidar_calibrator)/launch/calibrator.launch.xml">
+    <arg name="rviz" value="$(var rviz)"/>
+    <arg name="calibration_service_name" value="calibrate_radar_lidar"/>
+    <arg name="radar_optimization_frame" value="$(var radar_optimization_frame)"/>
+    <arg name="input_lidar_pointcloud" value="$(var input_lidar_pointcloud)"/>
+    <arg name="input_radar_msg" value="$(var input_radar_msg)"/>
+    <arg name="msg_type" value="$(var msg_type)"/>
+    <arg name="transformation_type" value="$(var transformation_type)"/>
+  </include>
+</launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/tag_based_pnp_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/tag_based_pnp_calibrator.launch.xml
new file mode 100644
index 00000000..f6e0e462
--- /dev/null
+++ b/sensor_calibration_manager/launch/xx1_gen2/tag_based_pnp_calibrator.launch.xml
@@ -0,0 +1,84 @@
+<launch>
+  <arg name="camera_name" default="camera0">
+    <choice value="camera0"/>
+    <choice value="camera1"/>
+    <choice value="camera2"/>
+    <choice value="camera3"/>
+    <choice value="camera4"/>
+    <choice value="camera5"/>
+    <choice value="camera6"/>
+    <choice value="camera7"/>
+    <choice value="camera8"/>
+    <choice value="camera9"/>
+    <choice value="camera10"/>
+  </arg>
+
+  <arg name="view_only_ui" default="true" description="By default we use a minimal UI"/>
+  <arg name="calibration_pairs" default="9" description="Number of lidar-image pairs for the calibration to converge"/>
+  <arg name="calibration_pairs_min_distance" default="1.5" description="Minimum allowed between a new detection and current pairs"/>
+
+  <!-- we do not use the standard image_raw name to avoid naming conflicts -->
+  <let name="image_decompressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/decompressed"/>
+  <let name="image_compressed_topic" value="/sensing/camera/$(var camera_name)/image_raw/compressed"/>
+
+  <let name="camera_info_topic" value="/sensing/camera/$(var camera_name)/camera_info"/>
+  <let name="pointcloud_topic" value="/sensing/lidar/top/pointcloud_raw"/>
+
+  <let name="camera_frame" value="$(var camera_name)/camera_link"/>
+  <let name="rviz_profile" value="$(find-pkg-share tag_based_pnp_calibrator)/rviz/default_profile.rviz"/>
+  <let name="lidar_model" value="velodyne_vls128"/>
+  <!--Note: The OT128 should work with the same profile as the VLS128 -->
+  <let name="lidar_frame" value="hesai_top"/>
+  <let name="use_rectified_image" value="false"/>
+  <!-- We assume that LDC=OFF & we calibrate using the image coming from the driver -->
+
+  <group>
+    <!-- image decompressor -->
+    <node pkg="image_transport_decompressor" exec="image_transport_decompressor_node" name="decompressor" output="screen">
+      <remap from="decompressor/input/compressed_image" to="$(var image_compressed_topic)"/>
+      <remap from="decompressor/output/raw_image" to="$(var image_decompressed_topic)"/>
+    </node>
+
+    <!-- tag based calibrator -->
+    <include file="$(find-pkg-share tag_based_pnp_calibrator)/launch/calibrator.launch.xml">
+      <arg name="image_topic" value="$(var image_decompressed_topic)"/>
+      <arg name="camera_info_topic" value="$(var camera_info_topic)"/>
+      <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
+      <arg name="pointcloud_topic_ex" value="$(var camera_info_topic)"/>
+      <arg name="lidar_model" value="$(var lidar_model)"/>
+      <arg name="calibration_service_name" value="calibrate_camera_lidar"/>
+
+      <arg name="use_rectified_image" value="$(var use_rectified_image)"/>
+      <arg name="calibration_pairs" value="$(var calibration_pairs)"/>
+      <arg name="calibration_pairs_min_distance" value="$(var calibration_pairs_min_distance)"/>
+    </include>
+
+    <!-- interactive calibrator -->
+    <node pkg="interactive_camera_lidar_calibrator" exec="interactive_calibrator" name="interactive_calibrator" output="screen" if="$(eval &quot;'$(var view_only_ui)' == 'false' &quot;)">
+      <remap from="pointcloud" to="$(var pointcloud_topic)"/>
+      <remap from="image" to="$(var image_compressed_topic)"/>
+      <remap from="camera_info" to="$(var camera_info_topic)"/>
+      <remap from="calibration_points_input" to="calibration_points"/>
+
+      <param name="camera_frame" value="$(var camera_frame)"/>
+      <param name="use_calibration_api" value="false"/>
+      <param name="can_publish_tf" value="false"/>
+    </node>
+
+    <!-- camera view -->
+    <node pkg="tier4_calibration_views" exec="image_view_node.py" name="image_view_node_py" output="screen" if="$(eval &quot;'$(var view_only_ui)' == 'true' &quot;)">
+      <remap from="pointcloud" to="$(var pointcloud_topic)"/>
+      <remap from="image" to="$(var image_compressed_topic)"/>
+      <remap from="camera_info" to="$(var camera_info_topic)"/>
+      <remap from="calibration_points_input" to="calibration_points"/>
+    </node>
+
+    <!-- create a placeholder lidar frame to make the rviz profile generic -->
+    <node pkg="tf2_ros" exec="static_transform_publisher" name="tf_broadcaster" output="screen" args="0 0 0 0 0 0 $(var lidar_frame) lidar_frame"/>
+
+    <!-- remap the pointcloud topic to make the rviz profile generic -->
+    <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_profile)">
+      <remap from="pointcloud_topic_placeholder" to="$(var pointcloud_topic)"/>
+    </node>
+  </group>
+</launch>
diff --git a/sensor_calibration_manager/launch/xx1_gen2/tag_based_sfm_base_lidar_calibrator.launch.xml b/sensor_calibration_manager/launch/xx1_gen2/tag_based_sfm_base_lidar_calibrator.launch.xml
new file mode 100644
index 00000000..9bb0f7ba
--- /dev/null
+++ b/sensor_calibration_manager/launch/xx1_gen2/tag_based_sfm_base_lidar_calibrator.launch.xml
@@ -0,0 +1,50 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="rviz" default="true"/>
+
+  <!-- Parameters -->
+  <arg name="config_file" default="omiya_calibration_room_2023.param.yaml"/>
+  <arg name="publish_tfs" default="true"/>
+  <arg name="fixed_ground_plane_model" default="false"/>
+
+  <!-- Frame configuration -->
+  <let name="base_frame" value="base_link"/>
+  <let name="main_calibration_sensor_frame" value="hesai_top"/>
+
+  <!-- Lidar configuration -->
+  <let name="calibration_lidar_model" value="velodyne_vls128"/>
+
+  <!-- Topic configuration -->
+  <let name="calibration_lidar_topic" value="/sensing/lidar/top/pointcloud_raw"/>
+
+  <arg
+    name="auxiliar_tag_ids"
+    default="[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49]"
+  />
+  <arg name="waypoint_tag_ids" default="[0, 1, 2, 3, 4, 5]"/>
+  <arg
+    name="ground_tag_ids"
+    default="[50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99]"
+  />
+  <arg name="left_wheel_tag_id" default="22"/>
+  <arg name="right_wheel_tag_id" default="26"/>
+
+  <!-- tag_based_sfm_base_lidar calibrator -->
+  <include file="$(find-pkg-share tag_based_sfm_calibrator)/launch/calibrator.launch.xml">
+    <arg name="calibration_service_name" value="calibrate_base_lidar"/>
+    <arg name="publish_tfs" value="$(var publish_tfs)"/>
+    <arg name="fixed_ground_plane_model" value="$(var fixed_ground_plane_model)"/>
+
+    <arg name="base_frame" value="$(var base_frame)"/>
+    <arg name="main_calibration_sensor_frame" value="$(var main_calibration_sensor_frame)"/>
+    <arg name="calibration_lidar_0_frame" value="$(var main_calibration_sensor_frame)"/>
+    <arg name="calibration_lidar_0_model" value="$(var calibration_lidar_model)"/>
+    <arg name="calibration_lidar_0_topic" value="$(var calibration_lidar_topic)"/>
+
+    <arg name="auxiliar_tag_ids" value="$(var auxiliar_tag_ids)"/>
+    <arg name="waypoint_tag_ids" value="$(var waypoint_tag_ids)"/>
+    <arg name="ground_tag_ids" value="$(var ground_tag_ids)"/>
+    <arg name="left_wheel_tag_id" value="$(var left_wheel_tag_id)"/>
+    <arg name="right_wheel_tag_id" value="$(var right_wheel_tag_id)"/>
+  </include>
+</launch>
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py
index 0d7af762..6b2e44a9 100644
--- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/__init__.py
@@ -4,3 +4,4 @@
 from .x2 import *  # noqa: F401, F403
 from .xx1 import *  # noqa: F401, F403
 from .xx1_15 import *  # noqa: F401, F403
+from .xx1_gen2 import *  # noqa: F401, F403
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py
index 9a9010f9..46510ba5 100644
--- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/default_project/marker_radar_lidar_calibrator.py
@@ -33,11 +33,13 @@ class MarkerRadarLidarCalibrator(CalibratorBase):
     def __init__(self, ros_interface: RosInterface, **kwargs):
         super().__init__(ros_interface)
 
-        self.radar_parallel_frame = kwargs["radar_parallel_frame"]
+        self.radar_optimization_frame = kwargs["radar_optimization_frame"]
         self.radar_frame = kwargs["radar_frame"]
         self.lidar_frame = kwargs["lidar_frame"]
 
-        self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame])
+        self.required_frames.extend(
+            [self.radar_optimization_frame, self.radar_frame, self.lidar_frame]
+        )
 
         self.add_calibrator(
             service_name="calibrate_radar_lidar",
@@ -47,15 +49,17 @@ def __init__(self, ros_interface: RosInterface, **kwargs):
         )
 
     def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
-        lidar_to_radar_parallel_transform = self.get_transform_matrix(
-            self.lidar_frame, self.radar_parallel_frame
+        lidar_to_radar_optimization_transform = self.get_transform_matrix(
+            self.lidar_frame, self.radar_optimization_frame
         )
 
-        radar_parallel_to_radar_transform = np.linalg.inv(
+        radar_optimization_to_radar_transform = np.linalg.inv(
             calibration_transforms[self.radar_frame][self.lidar_frame]
-            @ lidar_to_radar_parallel_transform
+            @ lidar_to_radar_optimization_transform
         )
 
-        results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}}
+        results = {
+            self.radar_optimization_frame: {self.radar_frame: radar_optimization_to_radar_transform}
+        }
 
         return results
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py
index 0c59f005..bd1968cb 100644
--- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/rdv/marker_radar_lidar_calibrator.py
@@ -33,11 +33,13 @@ class MarkerRadarLidarCalibrator(CalibratorBase):
     def __init__(self, ros_interface: RosInterface, **kwargs):
         super().__init__(ros_interface)
 
-        self.radar_parallel_frame = kwargs["radar_parallel_frame"]
+        self.radar_optimization_frame = kwargs["radar_optimization_frame"]
         self.radar_frame = kwargs["radar_frame"]
         self.lidar_frame = kwargs["lidar_frame"]
 
-        self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame])
+        self.required_frames.extend(
+            [self.radar_optimization_frame, self.radar_frame, self.lidar_frame]
+        )
 
         self.add_calibrator(
             service_name="calibrate_radar_lidar",
@@ -47,15 +49,17 @@ def __init__(self, ros_interface: RosInterface, **kwargs):
         )
 
     def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
-        lidar_to_radar_parallel_transform = self.get_transform_matrix(
-            self.lidar_frame, self.radar_parallel_frame
+        lidar_to_radar_optimization_transform = self.get_transform_matrix(
+            self.lidar_frame, self.radar_optimization_frame
         )
 
-        radar_parallel_to_radar_transform = np.linalg.inv(
+        radar_optimization_to_radar_transform = np.linalg.inv(
             calibration_transforms[self.radar_frame][self.lidar_frame]
-            @ lidar_to_radar_parallel_transform
+            @ lidar_to_radar_optimization_transform
         )
 
-        results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}}
+        results = {
+            self.radar_optimization_frame: {self.radar_frame: radar_optimization_to_radar_transform}
+        }
 
         return results
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py
index ca0b78a7..0f304874 100644
--- a/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/x2/marker_radar_lidar_calibrator.py
@@ -33,11 +33,24 @@ class MarkerRadarLidarCalibrator(CalibratorBase):
     def __init__(self, ros_interface: RosInterface, **kwargs):
         super().__init__(ros_interface)
 
-        self.radar_parallel_frame = kwargs["radar_parallel_frame"]
+        self.radar_optimization_frame = kwargs["radar_optimization_frame"]
+        self.radar_parent_frame = kwargs["radar_parent_frame"]
         self.radar_frame = kwargs["radar_frame"]
         self.lidar_frame = kwargs["lidar_frame"]
 
-        self.required_frames.extend([self.radar_parallel_frame, self.radar_frame, self.lidar_frame])
+        if self.radar_optimization_frame == self.radar_parent_frame:
+            self.required_frames.extend(
+                [self.radar_optimization_frame, self.radar_frame, self.lidar_frame]
+            )
+        else:
+            self.required_frames.extend(
+                [
+                    self.radar_optimization_frame,
+                    self.radar_parent_frame,
+                    self.radar_frame,
+                    self.lidar_frame,
+                ]
+            )
 
         self.add_calibrator(
             service_name="calibrate_radar_lidar",
@@ -47,15 +60,29 @@ def __init__(self, ros_interface: RosInterface, **kwargs):
         )
 
     def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
-        lidar_to_radar_parallel_transform = self.get_transform_matrix(
-            self.lidar_frame, self.radar_parallel_frame
+        lidar_to_radar_optimization_transform = self.get_transform_matrix(
+            self.lidar_frame, self.radar_optimization_frame
         )
 
-        radar_parallel_to_radar_transform = np.linalg.inv(
+        radar_optimization_to_radar_transform = np.linalg.inv(
             calibration_transforms[self.radar_frame][self.lidar_frame]
-            @ lidar_to_radar_parallel_transform
+            @ lidar_to_radar_optimization_transform
         )
 
-        results = {self.radar_parallel_frame: {self.radar_frame: radar_parallel_to_radar_transform}}
+        if self.radar_optimization_frame == self.radar_parent_frame:
+            results = {
+                self.radar_optimization_frame: {
+                    self.radar_frame: radar_optimization_to_radar_transform
+                }
+            }
+        else:
+            radar_parent_to_radar_optimization_transform = self.get_transform_matrix(
+                self.radar_parent_frame, self.radar_optimization_frame
+            )
+
+            radar_parent_to_radar_transform = (
+                radar_parent_to_radar_optimization_transform @ radar_optimization_to_radar_transform
+            )
+            results = {self.radar_parent_frame: {self.radar_frame: radar_parent_to_radar_transform}}
 
         return results
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/__init__.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/__init__.py
new file mode 100644
index 00000000..34ed79a9
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/__init__.py
@@ -0,0 +1,15 @@
+from .ground_plane_calibrator import GroundPlaneCalibrator
+from .mapping_based_base_lidar_calibrator import MappingBasedBaseLidarCalibrator
+from .mapping_based_lidar_lidar_calibrator import MappingBasedLidarLidarCalibrator
+from .marker_radar_lidar_calibrator import MarkerRadarLidarCalibrator
+from .tag_based_pnp_calibrator import TagBasedPNPCalibrator
+from .tag_based_sfm_base_lidar_calibrator import TagBasedSfmBaseLidarCalibrator
+
+__all__ = [
+    "GroundPlaneCalibrator",
+    "MappingBasedBaseLidarCalibrator",
+    "MappingBasedLidarLidarCalibrator",
+    "MarkerRadarLidarCalibrator",
+    "TagBasedPNPCalibrator",
+    "TagBasedSfmBaseLidarCalibrator",
+]
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/ground_plane_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/ground_plane_calibrator.py
new file mode 100644
index 00000000..8cd025d8
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/ground_plane_calibrator.py
@@ -0,0 +1,62 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="xx1_gen2", calibrator_name="ground_plane_calibrator"
+)
+class GroundPlaneCalibrator(CalibratorBase):
+    required_frames = []
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.base_frame = "base_link"
+        self.sensor_kit_frame = "sensor_kit_base_link"
+        self.lidar_frame = "hesai_top"
+
+        self.required_frames.extend([self.base_frame, self.sensor_kit_frame, self.lidar_frame])
+
+        self.add_calibrator(
+            service_name="calibrate_base_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=self.base_frame, child=self.lidar_frame),
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        base_to_lidar_transform = calibration_transforms[self.base_frame][self.lidar_frame]
+
+        sensor_kit_to_lidar_transform = self.get_transform_matrix(
+            self.sensor_kit_frame, self.lidar_frame
+        )
+
+        base_to_sensor_kit_transform = base_to_lidar_transform @ np.linalg.inv(
+            sensor_kit_to_lidar_transform
+        )
+
+        result = {self.base_frame: {self.sensor_kit_frame: base_to_sensor_kit_transform}}
+
+        return result
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/mapping_based_base_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/mapping_based_base_lidar_calibrator.py
new file mode 100644
index 00000000..08bf9dc3
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/mapping_based_base_lidar_calibrator.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="xx1_gen2", calibrator_name="mapping_based_base_lidar_calibrator"
+)
+class MappingBasedBaseLidarCalibrator(CalibratorBase):
+    required_frames = []
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.base_frame = "base_link"
+        self.sensor_kit_frame = "sensor_kit_base_link"
+
+        self.mapping_lidar_frame = "hesai_top"
+
+        self.required_frames.extend(
+            [self.base_frame, self.sensor_kit_frame, self.mapping_lidar_frame]
+        )
+
+        self.add_calibrator(
+            service_name="calibrate_base_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=self.mapping_lidar_frame, child=self.base_frame)
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix(
+            self.sensor_kit_frame, self.mapping_lidar_frame
+        )
+
+        base_to_top_sensor_kit_transform = np.linalg.inv(
+            sensor_kit_to_mapping_lidar_transform
+            @ calibration_transforms[self.mapping_lidar_frame][self.base_frame]
+        )
+        results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}}
+
+        return results
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/mapping_based_lidar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/mapping_based_lidar_lidar_calibrator.py
new file mode 100644
index 00000000..4b48b22f
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/mapping_based_lidar_lidar_calibrator.py
@@ -0,0 +1,98 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="xx1_gen2", calibrator_name="mapping_based_lidar_lidar_calibrator"
+)
+class MappingBasedLidarLidarCalibrator(CalibratorBase):
+    required_frames = []
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.sensor_kit_frame = "sensor_kit_base_link"
+        self.mapping_lidar_frame = "hesai_top"
+        self.calibration_lidar_frames = [
+            "hesai_left",
+            "hesai_right",
+            "hesai_front_left",
+            "hesai_front_right",
+        ]
+        self.calibration_base_lidar_frames = [
+            "hesai_left_base_link",
+            "hesai_right_base_link",
+            "hesai_front_left_base_link",
+            "hesai_front_right_base_link",
+        ]
+
+        self.required_frames.extend(
+            [
+                self.sensor_kit_frame,
+                self.mapping_lidar_frame,
+                *self.calibration_lidar_frames,
+                *self.calibration_base_lidar_frames,
+            ]
+        )
+
+        self.add_calibrator(
+            service_name="calibrate_lidar_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=self.mapping_lidar_frame, child=calibration_lidar_frame)
+                for calibration_lidar_frame in self.calibration_lidar_frames
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        sensor_kit_to_lidar_transform = self.get_transform_matrix(
+            self.sensor_kit_frame, self.mapping_lidar_frame
+        )
+
+        calibration_lidar_to_base_lidar_transforms = [
+            self.get_transform_matrix(calibration_lidar_frame, calibration_base_lidar_frame)
+            for calibration_lidar_frame, calibration_base_lidar_frame in zip(
+                self.calibration_lidar_frames, self.calibration_base_lidar_frames
+            )
+        ]
+
+        sensor_kit_to_calibration_lidar_transforms = [
+            sensor_kit_to_lidar_transform
+            @ calibration_transforms[self.mapping_lidar_frame][calibration_lidar_frame]
+            @ calibration_lidar_to_base_lidar_transform
+            for calibration_lidar_frame, calibration_lidar_to_base_lidar_transform in zip(
+                self.calibration_lidar_frames, calibration_lidar_to_base_lidar_transforms
+            )
+        ]
+
+        result = {
+            self.sensor_kit_frame: {
+                calibration_base_lidar_frame: transform
+                for calibration_base_lidar_frame, transform in zip(
+                    self.calibration_base_lidar_frames, sensor_kit_to_calibration_lidar_transforms
+                )
+            }
+        }
+
+        return result
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/marker_radar_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/marker_radar_lidar_calibrator.py
new file mode 100644
index 00000000..db8e85b6
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/marker_radar_lidar_calibrator.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="xx1_gen2", calibrator_name="marker_radar_lidar_calibrator"
+)
+class MarkerRadarLidarCalibrator(CalibratorBase):
+    required_frames = []
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.radar_optimization_frame = kwargs["radar_optimization_frame"]
+        self.radar_frame = kwargs["radar_frame"]
+        self.lidar_frame = kwargs["lidar_frame"]
+
+        self.required_frames.extend(
+            [self.radar_optimization_frame, self.radar_frame, self.lidar_frame]
+        )
+
+        self.add_calibrator(
+            service_name="calibrate_radar_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=self.radar_frame, child=self.lidar_frame)
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        lidar_to_radar_optimization_transform = self.get_transform_matrix(
+            self.lidar_frame, self.radar_optimization_frame
+        )
+
+        radar_optimization_to_radar_transform = np.linalg.inv(
+            calibration_transforms[self.radar_frame][self.lidar_frame]
+            @ lidar_to_radar_optimization_transform
+        )
+
+        results = {
+            self.radar_optimization_frame: {self.radar_frame: radar_optimization_to_radar_transform}
+        }
+
+        return results
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/tag_based_pnp_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/tag_based_pnp_calibrator.py
new file mode 100644
index 00000000..b114ec0f
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/tag_based_pnp_calibrator.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="xx1_gen2", calibrator_name="tag_based_pnp_calibrator"
+)
+class TagBasedPNPCalibrator(CalibratorBase):
+    required_frames = ["sensor_kit_base_link", "hesai_top"]
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.camera_name = kwargs["camera_name"]
+        self.required_frames.append(f"{self.camera_name}/camera_link")
+        self.required_frames.append(f"{self.camera_name}/camera_optical_link")
+
+        self.add_calibrator(
+            service_name="calibrate_camera_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=f"{self.camera_name}/camera_optical_link", child="hesai_top"),
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        camera_to_lidar_transform = calibration_transforms[
+            f"{self.camera_name}/camera_optical_link"
+        ]["hesai_top"]
+        sensor_kit_to_lidar_transform = self.get_transform_matrix(
+            "sensor_kit_base_link", "hesai_top"
+        )
+        camera_to_optical_link_transform = self.get_transform_matrix(
+            f"{self.camera_name}/camera_link", f"{self.camera_name}/camera_optical_link"
+        )
+        sensor_kit_camera_link_transform = np.linalg.inv(
+            camera_to_optical_link_transform
+            @ camera_to_lidar_transform
+            @ np.linalg.inv(sensor_kit_to_lidar_transform)
+        )
+
+        result = {
+            "sensor_kit_base_link": {
+                f"{self.camera_name}/camera_link": sensor_kit_camera_link_transform
+            }
+        }
+        return result
diff --git a/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/tag_based_sfm_base_lidar_calibrator.py b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/tag_based_sfm_base_lidar_calibrator.py
new file mode 100644
index 00000000..3265443f
--- /dev/null
+++ b/sensor_calibration_manager/sensor_calibration_manager/calibrators/xx1_gen2/tag_based_sfm_base_lidar_calibrator.py
@@ -0,0 +1,63 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Tier IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Dict
+
+import numpy as np
+
+from sensor_calibration_manager.calibrator_base import CalibratorBase
+from sensor_calibration_manager.calibrator_registry import CalibratorRegistry
+from sensor_calibration_manager.ros_interface import RosInterface
+from sensor_calibration_manager.types import FramePair
+
+
+@CalibratorRegistry.register_calibrator(
+    project_name="xx1_gen2", calibrator_name="tag_based_sfm_base_lidar_calibrator"
+)
+class TagBasedSfmBaseLidarCalibrator(CalibratorBase):
+    required_frames = []
+
+    def __init__(self, ros_interface: RosInterface, **kwargs):
+        super().__init__(ros_interface)
+
+        self.base_frame = kwargs["base_frame"]
+        self.sensor_kit_frame = "sensor_kit_base_link"
+
+        self.main_sensor_frame = kwargs["main_calibration_sensor_frame"]
+
+        self.required_frames.extend(
+            [self.base_frame, self.sensor_kit_frame, self.main_sensor_frame]
+        )
+
+        self.add_calibrator(
+            service_name="calibrate_base_lidar",
+            expected_calibration_frames=[
+                FramePair(parent=self.main_sensor_frame, child=self.base_frame)
+            ],
+        )
+
+    def post_process(self, calibration_transforms: Dict[str, Dict[str, np.array]]):
+        sensor_kit_to_mapping_lidar_transform = self.get_transform_matrix(
+            self.sensor_kit_frame, self.main_sensor_frame
+        )
+
+        base_to_top_sensor_kit_transform = np.linalg.inv(
+            sensor_kit_to_mapping_lidar_transform
+            @ calibration_transforms[self.main_sensor_frame][self.base_frame]
+        )
+        results = {self.base_frame: {self.sensor_kit_frame: base_to_top_sensor_kit_transform}}
+
+        return results