-
Notifications
You must be signed in to change notification settings - Fork 2.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add Doppler ICP in tensor registration pipeline (#5237)
We would like to contribute the implementation of our Doppler ICP algorithm for point clouds captured by FMCW LiDARs. This PR adds support for Doppler ICP in the tensor registration pipeline within Open3D. This is the implementation of the following paper:
@INPROCEEDINGS{Hexsel-RSS-22,
AUTHOR = {Bruno Hexsel AND Heethesh Vhavle AND Yi Chen},
TITLE = {{DICP: Doppler Iterative Closest Point Algorithm}},
BOOKTITLE = {Proceedings of Robotics: Science and Systems},
YEAR = {2022},
ADDRESS = {New York City, NY, USA},
MONTH = {June},
DOI = {10.15607/RSS.2022.XVIII.015}
}
Additions
Added CPU and CUDA kernels for computing the DICP residuals and Jacobians.
Added macro for dual robust function dispatch.
I separated the ICP loop for the Doppler variant since this needs additional arguments such as the time period, calibration, and other DICP related parameters.
Added TransformationToPose converter along with their kernels (based on the existing Eigen implementation of utility::TransformMatrix4dToVector6d).
Added Python bindings for all the new Doppler ICP methods.
Updates to existing API
Made matmul3x3_3x1 kernel available for __host__ in addition to __device__.
Added num_iterations and converged field to RegistrationResult.
Rename correspondences_ to correspondence_set to match the legacy Python parameters.
---------
Co-authored-by: Heethesh Vhavle <heethesh@aeva.ai>- Loading branch information
Showing
29 changed files
with
2,227 additions
and
42 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,121 @@ | ||
| // ---------------------------------------------------------------------------- | ||
| // - Open3D: www.open3d.org - | ||
| // ---------------------------------------------------------------------------- | ||
| // Copyright (c) 2018-2023 www.open3d.org | ||
| // SPDX-License-Identifier: MIT | ||
| // ---------------------------------------------------------------------------- | ||
|
|
||
| #include <json/json.h> | ||
|
|
||
| #include <Eigen/Geometry> | ||
| #include <fstream> | ||
| #include <iomanip> | ||
| #include <sstream> | ||
| #include <string> | ||
| #include <vector> | ||
|
|
||
| #include "open3d/data/Dataset.h" | ||
| #include "open3d/utility/IJsonConvertible.h" | ||
| #include "open3d/utility/Logging.h" | ||
|
|
||
| namespace open3d { | ||
| namespace data { | ||
|
|
||
| namespace { | ||
|
|
||
| bool ReadJSONFromFile(const std::string& path, Json::Value& json) { | ||
| std::ifstream file(path); | ||
| if (!file.is_open()) { | ||
| utility::LogWarning("Failed to open: {}", path); | ||
| return false; | ||
| } | ||
|
|
||
| Json::CharReaderBuilder builder; | ||
| builder["collectComments"] = false; | ||
| Json::String errs; | ||
| bool is_parse_successful = parseFromStream(builder, file, &json, &errs); | ||
| if (!is_parse_successful) { | ||
| utility::LogWarning("Read JSON failed: {}.", errs); | ||
| return false; | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| std::vector<std::pair<double, Eigen::Matrix4d>> LoadTUMTrajectory( | ||
| const std::string& filename) { | ||
| std::vector<std::pair<double, Eigen::Matrix4d>> trajectory; | ||
|
|
||
| std::ifstream file(filename); | ||
| if (!file.is_open()) { | ||
| utility::LogError("Failed to open: {}", filename); | ||
| } | ||
|
|
||
| std::string line; | ||
| while (std::getline(file, line)) { | ||
| std::istringstream iss(line); | ||
| double timestamp, x, y, z, qx, qy, qz, qw; | ||
| if (!(iss >> timestamp >> x >> y >> z >> qx >> qy >> qz >> qw)) { | ||
| utility::LogError("Error parsing line: {}", line); | ||
| } | ||
|
|
||
| Eigen::Affine3d transform = Eigen::Affine3d::Identity(); | ||
| transform.translate(Eigen::Vector3d(x, y, z)); | ||
| transform.rotate(Eigen::Quaterniond(qw, qx, qy, qz)); | ||
|
|
||
| trajectory.emplace_back(timestamp, transform.matrix()); | ||
| } | ||
|
|
||
| return trajectory; | ||
| } | ||
|
|
||
| } // namespace | ||
|
|
||
| const static DataDescriptor data_descriptor = { | ||
| Open3DDownloadsPrefix() + | ||
| "doppler-icp-data/carla-town05-curved-walls.zip", | ||
| "73a9828fb7790481168124c02398ee01"}; | ||
|
|
||
| DemoDopplerICPSequence::DemoDopplerICPSequence(const std::string& data_root) | ||
| : DownloadDataset("DemoDopplerICPSequence", data_descriptor, data_root) { | ||
| for (int i = 1; i <= 100; ++i) { | ||
| std::stringstream ss; | ||
| ss << std::setw(5) << std::setfill('0') << i; | ||
| paths_.push_back(GetExtractDir() + "/xyzd_sequence/" + ss.str() + | ||
| ".xyzd"); | ||
| } | ||
|
|
||
| calibration_path_ = GetExtractDir() + "/calibration.json"; | ||
| trajectory_path_ = GetExtractDir() + "/ground_truth_poses.txt"; | ||
| } | ||
|
|
||
| std::string DemoDopplerICPSequence::GetPath(std::size_t index) const { | ||
| if (index > 99) { | ||
| utility::LogError( | ||
| "Invalid index. Expected index between 0 to 99 but got {}.", | ||
| index); | ||
| } | ||
| return paths_[index]; | ||
| } | ||
|
|
||
| bool DemoDopplerICPSequence::GetCalibration(Eigen::Matrix4d& calibration, | ||
| double& period) const { | ||
| Json::Value calibration_data; | ||
| Eigen::Matrix4d calibration_temp; | ||
| if (ReadJSONFromFile(calibration_path_, calibration_data) && | ||
| utility::IJsonConvertible::EigenMatrix4dFromJsonArray( | ||
| calibration_temp, | ||
| calibration_data["transform_vehicle_to_sensor"])) { | ||
| calibration = calibration_temp.transpose(); | ||
| period = calibration_data["period"].asDouble(); | ||
| return true; | ||
| } | ||
| return false; | ||
| } | ||
|
|
||
| std::vector<std::pair<double, Eigen::Matrix4d>> | ||
| DemoDopplerICPSequence::GetTrajectory() const { | ||
| return LoadTUMTrajectory(trajectory_path_); | ||
| } | ||
|
|
||
| } // namespace data | ||
| } // namespace open3d |
Oops, something went wrong.