feat(calibration_tools): new api and 2.0 release (#148)

* Implementing the new calibration manager Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * Did some heavy refactoring in the tag-based camera-lidar calibration Added an option to use rectified images Deleted the raw sqpnp method since it is included in the standard opencv now Enabled the use of hardcoded system time (receiving time) for when sensors are now synchronized Parameterized rviz profiles to avoid having multiple profiles (for this method one should be enough for all use cases) Exposed the number of pnp pairs for calibration and the distance between calibration data and new detections to ease the work of field engineers Changed timers since the default work now (for years now though) Still have not done the spell checking Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * Fixed spellings in the tag based camera lidar calibrator Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: adapter the lidar-lidar 2d to the new API Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fixed spellings Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: adapted the ground plane calibrator to the new api Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: applied the changes required for the mapping-based lidar-lidar & base-lidar calibrators Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: fixed non-optional parameters in the launcher configuration ui Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: removed unused dummy calibrator Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: partial implementation of the changes for radar-lidar and implementation of a native way to edit the launchers Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: partial implementation of the changes for radar-lidar and implementation of a native way to edit the launchers Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: fixed the camera-lidar calibrator that was broken due to some new parameters ant the new launcher configuration schmeme Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: complete implementation for the radar-lidar, partial implementation for the rdv, and others Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: attempting to fix CI/CD Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: implemented the new api changes for the sfm calibrator and fixed bugs got discovered while doing do. Need to integrate all the options and the products Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: base-lidars working on the rdv Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: sfm calibrator integrated completely in rdv and although untested in the xx1. can use the initial calibration as a fixed ground plane Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: integrated all the sfm methods to the x2 Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: integrated the camera-lidar into the remaining project/products Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: refactored the itneractive ui, integrated it into the new api, and implemented the fixes for the spell checking Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: deleted the old manager/client and renamed the new calibrators Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: deleted deprecated mesasges Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fixed extra mispells found in ci/cd Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: more spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: fixed compile error (eigen vs. opencv) Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: applied the fix to another file Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fixed dependencies for ci cd Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: typo in launcher (thx vivid) Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix: forgot to set the initial solution after the refactoring in the ground plane calibrator Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: added explanations regarding the coordinate systems Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fixed typo in launcher Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: updated documentation and added missing marker in rviz profile Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: forgot to ass the mapping base-lidar for the default project Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: deleted the new part of the service Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: added the radar lidar calibrator to the default project Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: replaced all instances of transl -> translation and updated the copyrights Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: replaced rot -> rotation and missing transl -> translation Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: code explanation Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: removed redundant pass Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: typo Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: added explanation of a variable in the header file Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * ci(pre-commit): autofix * chore: attempt to fix pep257 Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fixing ci/cd and standalone installation due to changes in autoware Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: more pep stuff Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: fized pep Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: deleted redundant pass statements Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: updated old name Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: updated misleading names Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> Co-authored-by: yabuta <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Apr 11, 2024 · a8a4b6a · a8a4b6a
1 parent 43ad25d
commit a8a4b6a
Show file tree

Hide file tree

Showing 423 changed files with 14,893 additions and 40,078 deletions.
diff --git a/.cspell.json b/.cspell.json
@@ -1,17 +1,108 @@
 {
   "words": [
-    "apriltags",
+    "3dpoints",
     "Rodrigues",
-    "subsampled",
-    "undistortion",
-    "uniformingly",
+    "antialiasing",
+    "apriltag",
+    "apriltags",
+    "arange",
+    "autoware",
+    "astype",
+    "auxiliar",
+    "axisd",
+    "beforementioned",
+    "calib",
+    "cmap",
+    "coeffs",
+    "crossval",
+    "crossvalidation",
+    "opencv",
+    "discretization",
+    "distro",
+    "downsampling",
+    "downsample",
+    "dtype",
+    "eigen",
+    "eulers",
+    "extrinsics",
+    "figsize",
+    "gicp",
+    "hesai",
+    "homography",
     "hsize",
+    "icp",
+    "idless",
+    "idxs",
+    "imdecode",
+    "imread",
+    "imshow",
+    "imwrite",
+    "intrinsics",
+    "kalman",
+    "keyframes",
+    "libceres",
+    "lidars",
+    "lidartag",
+    "lidartags",
+    "linalg",
+    "matplotlib",
+    "matx",
+    "meshgrid",
+    "misdetection",
+    "nanosec",
+    "neighbours",
+    "ncols",
+    "nrows",
+    "omiya",
+    "overfits",
+    "pandar",
+    "permutate",
+    "pixmap",
+    "pnp",
+    "pointcloud",
+    "pointclouds",
+    "polyline",
+    "prerejective",
+    "pydot",
+    "pyplot",
+    "qcolor",
+    "quaterniond",
+    "ransac",
+    "rclcpp",
+    "rclpy",
+    "registrator",
+    "registrators",
+    "remappings",
+    "representer",
+    "reprojected",
+    "reprojection",
+    "rosbag",
+    "rosidl",
+    "ruamel",
     "rvec",
-    "tvec",
     "rvecs",
+    "rviz",
+    "slerp",
+    "solvepnp",
+    "sqpnp",
+    "srvs",
+    "subsampled",
+    "subsamples",
+    "subsampling",
+    "tvec",
     "tvecs",
-    "nrows",
-    "ncols",
-    "crossval"
+    "undistort",
+    "undistortion",
+    "uniformingly",
+    "velodyne",
+    "vectord",
+    "voxel",
+    "voxels",
+    "xaxis",
+    "xlabel",
+    "xlim",
+    "yaxis",
+    "ylabel",
+    "ylim"
   ]
 }
diff --git a/build_depends.repos b/build_depends.repos
@@ -19,6 +19,10 @@ repositories:
     type: git
     url: https://github.com/astuff/astuff_sensor_msgs.git
     version: 3.2.0
+  core/autoware_internal_msgs:
+    type: git
+    url: https://github.com/autowarefoundation/autoware_internal_msgs.git
+    version: main
   core/common:
     type: git
     url: https://github.com/autowarefoundation/autoware_common.git

diff --git a/...or/point_cloud_accumulator/CMakeLists.txt → ...on/point_cloud_accumulator/CMakeLists.txt b/...or/point_cloud_accumulator/CMakeLists.txt → ...on/point_cloud_accumulator/CMakeLists.txt
diff --git a/...rinsic_interactive_calibrator/__init__.py → common/point_cloud_accumulator/COLCON_IGNORE b/...rinsic_interactive_calibrator/__init__.py → common/point_cloud_accumulator/COLCON_IGNORE
diff --git a/...d_accumulator/point_cloud_accumulator.hpp → ...d_accumulator/point_cloud_accumulator.hpp b/...d_accumulator/point_cloud_accumulator.hpp → ...d_accumulator/point_cloud_accumulator.hpp
diff --git a/...launch/point_cloud_accumulator.launch.xml → ...launch/point_cloud_accumulator.launch.xml b/...launch/point_cloud_accumulator.launch.xml → ...launch/point_cloud_accumulator.launch.xml
diff --git a/sensor/point_cloud_accumulator/package.xml → common/point_cloud_accumulator/package.xml b/sensor/point_cloud_accumulator/package.xml → common/point_cloud_accumulator/package.xml
diff --git a/...cumulator/src/point_cloud_accumulator.cpp → ...cumulator/src/point_cloud_accumulator.cpp b/...cumulator/src/point_cloud_accumulator.cpp → ...cumulator/src/point_cloud_accumulator.cpp
diff --git a/common/tier4_calibration_msgs/CMakeLists.txt b/common/tier4_calibration_msgs/CMakeLists.txt
@@ -17,6 +17,7 @@ ament_auto_find_build_dependencies()
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/BoolStamped.msg"
   "msg/CalibrationPoints.msg"
+  "msg/CalibrationResult.msg"
   "msg/Files.msg"
   "msg/Float32Stamped.msg"
   "msg/EstimationResult.msg"
@@ -25,10 +26,9 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/FilesSrv.srv"
   "srv/FilesListSrv.srv"
   "srv/CalibrationDatabase.srv"
-  "srv/ExtrinsicCalibrationManager.srv"
-  "srv/ExtrinsicCalibrator.srv"
   "srv/Frame.srv"
   "srv/IntrinsicsOptimizer.srv"
+  "srv/ExtrinsicCalibrator.srv"
   DEPENDENCIES
     std_msgs
     sensor_msgs

diff --git a/common/tier4_calibration_msgs/msg/CalibrationResult.msg b/common/tier4_calibration_msgs/msg/CalibrationResult.msg
@@ -0,0 +1,4 @@
+geometry_msgs/TransformStamped transform_stamped
+bool success
+float32 score
+std_msgs/String message
diff --git a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrationManager.srv
diff --git a/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv b/common/tier4_calibration_msgs/srv/ExtrinsicCalibrator.srv
@@ -1,6 +1,2 @@
-geometry_msgs/Pose initial_pose
 ---
-geometry_msgs/Pose result_pose
-bool success
-float32 score
-sensor_msgs/PointCloud2 debug_pointcloud
+tier4_calibration_msgs/CalibrationResult[] results
diff --git a/...alibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp b/...alibration_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// 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.

diff --git a/...ation_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp b/...ation_pcl_extensions/include/tier4_calibration_pcl_extensions/joint_icp_extended_impl.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// 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.
@@ -354,7 +354,7 @@ void pcl::JointIterativeClosestPointExtended<PointSource, PointTarget, Scalar>::
 
     ++nr_iterations_;
 
-    // Update the vizualization of icp convergence
+    // Update the visualization of icp convergence
     // if (update_visualizer_ != 0)
     //  update_visualizer_(output, source_indices_good, *target_, target_indices_good );
 

diff --git a/...libration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp b/...libration_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// 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.
@@ -82,7 +82,7 @@ struct cloud_point_index_idx_triplets
   {
     return (idx0 < p.idx0) || (idx0 == p.idx0 && idx1 < p.idx1) ||
            (idx0 == p.idx0 && idx1 == p.idx1 && idx2 < p.idx2);
-  }  // test brancheless agains branched version with a dataset at some point \ along with some sort
+  }  // test branchless against branched version with a dataset at some point \ along with some sort
      // implementations
 };
 
@@ -102,7 +102,7 @@ class VoxelGridTriplets : public VoxelGrid<PointT>
   using VoxelGrid<PointT>::min_b_;
   using VoxelGrid<PointT>::max_b_;
   using VoxelGrid<PointT>::div_b_;
-  using VoxelGrid<PointT>::divb_mul_;
+  using VoxelGrid<PointT>::divb_mul_;  // cSpell:ignore divb
   using VoxelGrid<PointT>::min_points_per_voxel_;
   using VoxelGrid<PointT>::save_leaf_layout_;
   using VoxelGrid<PointT>::leaf_layout_;

diff --git a/...tion_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp b/...tion_pcl_extensions/include/tier4_calibration_pcl_extensions/voxel_grid_triplets_impl.hpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// 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.
@@ -123,6 +123,7 @@ void pcl::VoxelGridTriplets<PointT>::applyFilter(PointCloud & output)
   div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones();
   div_b_[3] = 0;
 
+  // cSpell:ignore divb
   // Set up the division multiplier
   divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0);
 
@@ -199,6 +200,8 @@ void pcl::VoxelGridTriplets<PointT>::applyFilter(PointCloud & output)
     }
   }
 
+  // cSpell:ignore spreadsort
+  // cSpell:ignore rightshift
   // Second pass: sort the index_vector vector using value representing target cell as index
   // in effect all points belonging to the same output cell will be next to each other
   // auto rightshift_func = [](const cloud_point_index_idx_triplets &x, const unsigned offset) {

diff --git a/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp b/common/tier4_calibration_pcl_extensions/src/joint_icp_extended.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// 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.

diff --git a/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp b/common/tier4_calibration_pcl_extensions/src/voxel_grid_triplets.cpp
@@ -1,4 +1,4 @@
-// Copyright 2023 Tier IV, Inc.
+// 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.

diff --git a/common/tier4_calibration_views/CMakeLists.txt b/common/tier4_calibration_views/CMakeLists.txt
@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.5)
+project(tier4_calibration_views)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(autoware_cmake REQUIRED)
+
+autoware_package()
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  scripts/image_view_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_export_dependencies(ament_cmake)
+ament_export_dependencies(ament_cmake_python)
+ament_package()
diff --git a/common/tier4_calibration_views/package.xml b/common/tier4_calibration_views/package.xml
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>tier4_calibration_views</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">Kenzo Lobos Tsunekawa</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <exec_depend>python3-matplotlib</exec_depend>
+  <exec_depend>python3-pyside2.qtquick</exec_depend>
+  <exec_depend>python3-transforms3d</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>ros2_numpy</exec_depend>
+  <exec_depend>ros2launch</exec_depend>
+  <exec_depend>tier4_calibration_msgs</exec_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/...resource/extrinsic_interactive_calibrator → ...on_views/resource/tier4_calibration_views b/...resource/extrinsic_interactive_calibrator → ...on_views/resource/tier4_calibration_views
diff --git a/common/tier4_calibration_views/scripts/image_view_node.py b/common/tier4_calibration_views/scripts/image_view_node.py
@@ -0,0 +1,53 @@
+#!/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.
+
+import logging
+import os
+import signal
+import sys
+
+from PySide2.QtWidgets import QApplication
+import rclpy
+from tier4_calibration_views.image_view_ros_interface import ImageViewRosInterface
+from tier4_calibration_views.image_view_ui import ImageViewUI
+
+
+def main(args=None):
+    os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = ""
+    app = QApplication(sys.argv)
+
+    rclpy.init(args=args)
+
+    try:
+        signal.signal(signal.SIGINT, sigint_handler)
+
+        ros_interface = ImageViewRosInterface()
+        ex = ImageViewUI(ros_interface)  # noqa: F841
+
+        ros_interface.spin()
+
+        sys.exit(app.exec_())
+    except (KeyboardInterrupt, SystemExit):
+        logging.info("Received sigint. Quitting...")
+        rclpy.shutdown()
+
+
+def sigint_handler(*args):
+    QApplication.quit()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/...nteractive_calibrator/test/test_pep257.py → ...er4_calibration_views/test/test_pep257.py b/...nteractive_calibrator/test/test_pep257.py → ...er4_calibration_views/test/test_pep257.py
diff --git a/common/tier4_calibration_views/tier4_calibration_views/__init__.py b/common/tier4_calibration_views/tier4_calibration_views/__init__.py