diff --git a/.github/workflows/on_main.yml b/.github/workflows/on_main.yml index f6ea8a15..dd24f7a8 100644 --- a/.github/workflows/on_main.yml +++ b/.github/workflows/on_main.yml @@ -79,4 +79,4 @@ jobs: - name: Github Release env: GITHUB_TOKEN: ${{ secrets.RELEASE_PLEASE_GITHUB_TOKEN }} - run: gh release upload -R ${{ github.repository }} ${{ needs.release.outputs.tag_name }} ./dist/* \ No newline at end of file + run: gh release upload -R ${{ github.repository }} ${{ needs.release.outputs.tag_name }} ./dist/* diff --git a/cpp/bindings/pipeline.h b/cpp/bindings/pipeline.h index 5f1bd83a..a2ea97b7 100644 --- a/cpp/bindings/pipeline.h +++ b/cpp/bindings/pipeline.h @@ -21,10 +21,6 @@ class PyPipeline: public evalio::Pipeline { NB_TRAMPOLINE(Pipeline, 9); // Getters - const evalio::SE3 pose() override { - NB_OVERRIDE_PURE(pose); - } - const std::map> map() override { NB_OVERRIDE_PURE(map); } @@ -56,8 +52,7 @@ class PyPipeline: public evalio::Pipeline { NB_OVERRIDE_PURE(add_imu, mm); } - std::map> - add_lidar(evalio::LidarMeasurement mm) override { + void add_lidar(evalio::LidarMeasurement mm) override { NB_OVERRIDE_PURE(add_lidar, mm); } }; // namespace evalio @@ -81,7 +76,31 @@ inline void makeBasePipeline(nb::module_& m) { &evalio::Pipeline::version, "Version of the pipeline." ) - .def("pose", &evalio::Pipeline::pose, "Most recent pose estimate.") + .def( + "saved_estimates", + &evalio::Pipeline::saved_estimates, + "Most recent stamped pose estimates. Will be removed after this method is called." + ) + .def( + "saved_features", + &evalio::Pipeline::saved_features, + "Most recent stamped feature scans. Will be removed after this method is called." + ) + .def( + "saved_maps", + &evalio::Pipeline::saved_maps, + "Most recent stamped maps. Will be removed after this method is called." + ) + .def( + "saved_features_matrix", + &evalio::Pipeline::saved_features_matrix, + "Most recent stamped feature scans as matrices. Will be removed after this method is called." + ) + .def( + "saved_maps_matrix", + &evalio::Pipeline::saved_maps_matrix, + "Most recent stamped maps as matrices. Will be removed after this method is called." + ) .def("map", &evalio::Pipeline::map, "Map of the environment.") .def( "initialize", @@ -126,6 +145,31 @@ inline void makeBasePipeline(nb::module_& m) { "T"_a, "Set the transformation from IMU to LiDAR frame." ) + .def( + "set_visualizing", + &evalio::Pipeline::set_visualizing, + "visualize"_a.none(), + "Enable or disable visualization." + ) + .def( + "save", + nb::overload_cast( + &evalio::Pipeline::save + ), + "stamp"_a, + "pose"_a, + "Save the current pose estimate at the given timestamp." + ) + // Use a lambda here, as nb::overload_cast has issues finding the non-template methods + .def( + "save", + [](Pipeline& self, const Stamp& stamp, const Map<>& features) { + self.save(stamp, features); + }, + "stamp"_a, + "features"_a, + "Save the current feature map at the given timestamp." + ) .doc() = "Base class for all pipelines. This class defines the interface " "for interacting with pipelines, and is intended to be " diff --git a/cpp/bindings/pipelines/ct_icp.h b/cpp/bindings/pipelines/ct_icp.h index 48d952e2..5c77a46e 100644 --- a/cpp/bindings/pipelines/ct_icp.h +++ b/cpp/bindings/pipelines/ct_icp.h @@ -204,14 +204,8 @@ class CTICP: public ev::Pipeline { // clang-format on // Getters - const ev::SE3 pose() override { - const auto pose = ct_icp_->Trajectory().back(); - return ev::convert(pose) * lidar_T_imu_; - } - const std::map> map() override { - auto map = ct_icp_->GetLocalMap(); - return ev::convert_map({{"planar", map}}); + return ev::make_map("planar", ct_icp_->GetLocalMap()); } // Setters @@ -233,8 +227,7 @@ class CTICP: public ev::Pipeline { void add_imu(ev::ImuMeasurement mm) override {} - std::map> - add_lidar(ev::LidarMeasurement mm) override { + void add_lidar(ev::LidarMeasurement mm) override { // Convert auto pc = ev::convert_iter>(mm.points); @@ -260,11 +253,14 @@ class CTICP: public ev::Pipeline { // Run through pipeline const auto summary = ct_icp_->RegisterFrame(pc); - scan_idx_++; - // Return the used points - return ev::convert_map>( - {{"planar", summary.keypoints}} - ); + // Save the estimate + const auto pose = ct_icp_->Trajectory().back(); + this->save(mm.stamp, ev::convert(pose) * lidar_T_imu_); + + // Save the used points + this->save(mm.stamp, "planar", summary.keypoints); + + scan_idx_++; } }; diff --git a/cpp/bindings/pipelines/genz_icp.h b/cpp/bindings/pipelines/genz_icp.h index 90d0fb50..5d4cd326 100644 --- a/cpp/bindings/pipelines/genz_icp.h +++ b/cpp/bindings/pipelines/genz_icp.h @@ -52,16 +52,8 @@ class GenZICP: public ev::Pipeline { // clang-format on // Getters - const ev::SE3 pose() override { - const auto pose = - !genz_icp_->poses().empty() ? genz_icp_->poses().back() : Sophus::SE3d(); - return ev::convert(pose * lidar_T_imu_); - } - const std::map> map() override { - return ev::convert_map>( - {{"map", genz_icp_->LocalMap()}} - ); + return ev::make_map("map", genz_icp_->LocalMap()); } // Setters @@ -84,34 +76,35 @@ class GenZICP: public ev::Pipeline { void add_imu(ev::ImuMeasurement mm) override {} - std::map> - add_lidar(ev::LidarMeasurement mm) override { + void add_lidar(ev::LidarMeasurement mm) override { // Set everything up auto points = ev::convert_iter>(mm.points); auto timestamps = ev::convert_iter>(mm.points); // Run through pipeline auto [planar, nonplanar] = genz_icp_->RegisterFrame(points, timestamps); - auto lidar_T_world = genz_icp_->poses().back().inverse(); + auto world_T_lidar = genz_icp_->poses().back(); + auto lidar_T_world = world_T_lidar.inverse(); + + // Save the estimate + this->save(mm.stamp, world_T_lidar * lidar_T_imu_); // These are all in the global frame, so we need to convert them std::transform( planar.begin(), planar.end(), planar.begin(), - [&](auto point) { return lidar_T_imu_ * point; } + [&](auto point) { return lidar_T_world * point; } ); std::transform( nonplanar.begin(), nonplanar.end(), nonplanar.begin(), - [&](auto point) { return lidar_T_imu_ * point; } + [&](auto point) { return lidar_T_world * point; } ); - // Return the used points - return ev::convert_map>( - {{"planar", planar}, {"nonplanar", nonplanar}} - ); + // Save the used points + this->save(mm.stamp, "planar", planar, "nonplanar", nonplanar); } private: diff --git a/cpp/bindings/pipelines/kiss_icp.h b/cpp/bindings/pipelines/kiss_icp.h index a9f30d38..44c87865 100644 --- a/cpp/bindings/pipelines/kiss_icp.h +++ b/cpp/bindings/pipelines/kiss_icp.h @@ -39,15 +39,8 @@ class KissICP: public ev::Pipeline { ); // Getters - const ev::SE3 pose() override { - const Sophus::SE3d pose = kiss_icp_->pose() * lidar_T_imu_; - return ev::convert(pose); - } - const std::map> map() override { - return ev::convert_map>( - {{"point", kiss_icp_->LocalMap()}} - ); + return ev::make_map("point", kiss_icp_->LocalMap()); } // Setters @@ -69,8 +62,7 @@ class KissICP: public ev::Pipeline { void add_imu(ev::ImuMeasurement mm) override {} - std::map> - add_lidar(ev::LidarMeasurement mm) override { + void add_lidar(ev::LidarMeasurement mm) override { // Convert inputs auto points = ev::convert_iter>(mm.points); auto timestamps = ev::convert_iter>(mm.points); @@ -78,10 +70,11 @@ class KissICP: public ev::Pipeline { // Run through pipeline const auto& [_, used_points] = kiss_icp_->RegisterFrame(points, timestamps); - // Convert outputs - return ev::convert_map>( - {{"point", used_points}} - ); + // Save the estimate + this->save(mm.stamp, kiss_icp_->pose() * lidar_T_imu_); + + // Save features + this->save(mm.stamp, "point", used_points); } private: diff --git a/cpp/bindings/pipelines/lio_sam.h b/cpp/bindings/pipelines/lio_sam.h index c2d21c89..b5b91caa 100644 --- a/cpp/bindings/pipelines/lio_sam.h +++ b/cpp/bindings/pipelines/lio_sam.h @@ -35,14 +35,14 @@ inline Point convert(const lio_sam::PointType& in) { template<> inline lio_sam::PointXYZIRT convert(const ev::Point& in) { - return lio_sam::PointXYZIRT { - .x = static_cast(in.x), - .y = static_cast(in.y), - .z = static_cast(in.z), - .intensity = static_cast(in.intensity), - .ring = static_cast(in.row), - .time = static_cast(in.t.to_sec()), - }; + lio_sam::PointXYZIRT out; + out.x = static_cast(in.x); + out.y = static_cast(in.y); + out.z = static_cast(in.z); + out.intensity = static_cast(in.intensity); + out.ring = static_cast(in.row); + out.time = static_cast(in.t.to_sec()); + return out; } // IMU conversions @@ -117,14 +117,8 @@ class LioSam: public ev::Pipeline { // clang-format on // Getters - const ev::SE3 pose() override { - return ev::convert(lio_sam_->getPose()) * lidar_T_imu_; - } - const std::map> map() override { - return ev::convert_map>( - {{"point", *lio_sam_->getMap()}} - ); + return ev::make_map("map", *lio_sam_->getMap()); } // Setters @@ -158,8 +152,7 @@ class LioSam: public ev::Pipeline { lio_sam_->addImuMeasurement(ev::convert(mm)); } - std::map> - add_lidar(ev::LidarMeasurement mm) override { + void add_lidar(ev::LidarMeasurement mm) override { // Set everything up auto cloud = ev::convert_iter>(mm.points) @@ -169,10 +162,12 @@ class LioSam: public ev::Pipeline { // Run through pipeline lio_sam_->addLidarMeasurement(mm.stamp.to_sec(), cloud); - // Return features - return ev::convert_map>( - {{"point", *lio_sam_->getMostRecentFrame()}} - ); + // Save pose + const auto pose = ev::convert(lio_sam_->getPose()) * lidar_T_imu_; + this->save(mm.stamp, pose); + + // Save features + this->save(mm.stamp, "features", *lio_sam_->getMostRecentFrame()); } private: diff --git a/cpp/bindings/pipelines/loam.h b/cpp/bindings/pipelines/loam.h index 0088afdc..045ed016 100644 --- a/cpp/bindings/pipelines/loam.h +++ b/cpp/bindings/pipelines/loam.h @@ -63,10 +63,6 @@ class LOAM: public ev::Pipeline { // clang-format on // Getters - const ev::SE3 pose() override { - return ev::convert(current_estimated_pose) * lidar_T_imu_; - } - const std::map> map() override { return features_to_points( transform_features(map_features(), current_estimated_pose) @@ -94,8 +90,7 @@ class LOAM: public ev::Pipeline { void add_imu(ev::ImuMeasurement mm) override {} - std::map> - add_lidar(ev::LidarMeasurement mm) override { + void add_lidar(ev::LidarMeasurement mm) override { // Handle Edge case of the first scan if (past_k_scans_.size() == 0) { // Extract Features from the first scan @@ -106,9 +101,9 @@ class LOAM: public ev::Pipeline { std::make_pair(loam::Pose3d::Identity(), scan_features) ); // Initialize the odometry frame pose - current_estimated_pose = loam::Pose3d::Identity(); + this->save(mm.stamp, evalio::SE3::identity() * lidar_T_imu_); // Return the initial scan features - return features_to_points(scan_features); + this->save(mm.stamp, features_to_points(scan_features)); } // Default case for all iterations except the first else { @@ -134,8 +129,11 @@ class LOAM: public ev::Pipeline { } // Update the Odometry frame pose current_estimated_pose = current_estimated_pose.compose(map_T_scan); + const auto pose_ev = + ev::convert(current_estimated_pose) * lidar_T_imu_; + this->save(mm.stamp, pose_ev); // Return the points associated with the used features - return features_to_points(scan_features); + this->save(mm.stamp, features_to_points(scan_features)); } } diff --git a/cpp/bindings/pipelines/mad_icp.h b/cpp/bindings/pipelines/mad_icp.h index fae11a4c..ea8e9990 100644 --- a/cpp/bindings/pipelines/mad_icp.h +++ b/cpp/bindings/pipelines/mad_icp.h @@ -61,14 +61,8 @@ class MadICP: public ev::Pipeline { ); // Getters - const ev::SE3 pose() override { - return ev::convert(mad_icp_->currentPose()) * lidar_T_imu_; - } - const std::map> map() override { - return ev::convert_map>( - {{"planar", mad_icp_->modelLeaves()}} - ); + return ev::make_map("planar", mad_icp_->modelLeaves()); } // Setters @@ -109,8 +103,7 @@ class MadICP: public ev::Pipeline { void add_imu(ev::ImuMeasurement mm) override {} - std::map> - add_lidar(ev::LidarMeasurement mm) override { + void add_lidar(ev::LidarMeasurement mm) override { // filter out points that are out of range mm.points.erase( // remove_if puts the elements to be removed at the end of the vector @@ -133,10 +126,13 @@ class MadICP: public ev::Pipeline { // Run through pipeline mad_icp_->compute(mm.stamp.to_sec(), points); - // Return current leaves - return ev::convert_map>( - {{"planar", mad_icp_->currentLeaves()}} - ); + // Save the current estimate + const auto pose = + ev::convert(mad_icp_->currentPose()) * lidar_T_imu_; + this->save(mm.stamp, pose); + + // Save current leaves + this->save(mm.stamp, "planar", mad_icp_->currentLeaves()); } private: diff --git a/cpp/bindings/types.h b/cpp/bindings/types.h index 6ab3fba9..a3855f05 100644 --- a/cpp/bindings/types.h +++ b/cpp/bindings/types.h @@ -2,7 +2,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -19,6 +21,13 @@ namespace evalio { // TODO: Check if copy/deepcopy works or not inline void makeTypes(nb::module_& m) { + nb::enum_(m, "VisOption", nb::is_flag()) + .value("MAP", VisOption::MAP, "Visualize the map.") + .value("FEATURES", VisOption::FEATURES, "Visualize the features.") + .value("SCAN", VisOption::SCAN, "Visualize the scan.") + .value("IMAGE", VisOption::IMAGE, "Visualize the image.") + .doc() = "Visualization options for the evalio pipeline."; + nb::class_(m, "Duration") .def_static( "from_sec", diff --git a/cpp/evalio/CMakeLists.txt b/cpp/evalio/CMakeLists.txt index 030ab4eb..4bd522f0 100644 --- a/cpp/evalio/CMakeLists.txt +++ b/cpp/evalio/CMakeLists.txt @@ -17,7 +17,6 @@ install(TARGETS evalio EXPORT evalioConfig) install(EXPORT evalioConfig DESTINATION "${CMAKE_INSTALL_DATADIR}/cmake/evalio") # headers -# TODO: Verify this is handling nested conversion files correctly install( DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} diff --git a/cpp/evalio/convert/base.h b/cpp/evalio/convert/base.h index 501518e6..e67b82b6 100644 --- a/cpp/evalio/convert/base.h +++ b/cpp/evalio/convert/base.h @@ -11,7 +11,17 @@ namespace evalio { /// /// Overloads and specializations may be imported from other files or defined by the user. template -inline Out convert(const In& in) = delete; +inline Out convert(const In& in) { + // Allow identity conversions by default + if constexpr (std::is_same_v) { + return in; + } else { + static_assert( + sizeof(Out) == 0, + "No conversion defined between the specified types." + ); + } +} /// @brief Convert evalio::Point to timestamps as doubles. template<> @@ -19,17 +29,18 @@ inline double convert(const evalio::Point& in) { return in.t.to_sec(); } -/// @brief Convert a container of In to a container of Out using convert(in). +/// @brief Convert a container of In to a container of Out using convert(in). /// /// Assumes the following are defined: +/// - In::value_type. /// - In::size(). /// - In is iterable with range-based for loops. /// - Out is default-constructible. /// - Out::value_type. /// - Out::reserve(size_t). /// - Out::push_back(Out::value_type). -/// - convert(In::value_type). - +/// - convert(in_value). +/// These should all generally be true to std containers. template inline Out convert_iter(const In& in) { Out out; @@ -42,21 +53,40 @@ inline Out convert_iter(const In& in) { return out; } -/// @brief Convert a map of arbitrary containers to a map of std::vector. +// recursive case +template +inline void _make_map_impl( + std::map>& out, + std::string&& k, + T&& v, + Args&&... args +) { + auto converted = convert_iter>(std::forward(v)); + out.insert({std::forward(k), converted}); + if constexpr (sizeof...(args) > 0) { + _make_map_impl(out, std::forward(args)...); + } +} + +/// @brief Create a map from variadic key-value pairs. /// -/// Useful for converting to final evalio map types. Unlike other convert functions, -/// this one only requires the input type to be specified. -/// Assumes the following are defined: -/// - In::size(). -/// - In is iterable with range-based for loops. -/// - convert(In::value_type). -template +/// Allows a pretty usage like: +/// ```cpp +/// auto my_map = evalio::make_map("planar", planar_data); +/// ``` +/// This allows easy construction of maps without having to specify the full type +/// or performing conversions by hand. Requires convert_iter, T>. +/// Not 100% a conversion, but felt appropriate to include here. +template inline std::map> -convert_map(const std::map& in) { +make_map(std::string&& k, T&& v, Args&&... args) { std::map> out; - for (const auto& [key, val] : in) { - out[key] = convert_iter>(val); - } + _make_map_impl( + out, + std::forward(k), + std::forward(v), + std::forward(args)... + ); return out; } diff --git a/cpp/evalio/convert/eigen.h b/cpp/evalio/convert/eigen.h index a2e1695b..62449f02 100644 --- a/cpp/evalio/convert/eigen.h +++ b/cpp/evalio/convert/eigen.h @@ -26,6 +26,14 @@ inline Eigen::Vector3d convert(const Point& in) { return {in.x, in.y, in.z}; } +inline Eigen::MatrixX3d convert(const std::vector& in) { + Eigen::MatrixX3d out(in.size(), 3); + for (size_t i = 0; i < in.size(); ++i) { + out.row(i) = convert(in[i]); + } + return out; +} + // ------------------------- Poses ------------------------- // template<> inline SE3 convert(const Eigen::Isometry3d& in) { diff --git a/cpp/evalio/pipeline.h b/cpp/evalio/pipeline.h index d7cc1be6..9d72a7cf 100644 --- a/cpp/evalio/pipeline.h +++ b/cpp/evalio/pipeline.h @@ -2,8 +2,12 @@ #include #include +#include +#include #include +#include "evalio/convert/base.h" +#include "evalio/convert/eigen.h" #include "evalio/macros.h" #include "evalio/types.h" @@ -26,12 +30,22 @@ namespace evalio { +/// @brief A variant type for parameters. using Param = std::variant; +/// @brief A map from string keys to containers of points. +template> +using Map = std::map; + +/// @brief A tuple of a timestamp and arbitrary other types. +template +using Stamped = std::tuple; + class PYBIND11_EXPORT Pipeline { public: virtual ~Pipeline() {} + // ------------------------- Things to override/use ------------------------- // // Info static std::string version() { return "0.0.0"; @@ -45,26 +59,134 @@ class PYBIND11_EXPORT Pipeline { throw std::runtime_error("Not implemented"); } + // Should be implemented with EVALIO_SETUP_PARAMS static std::map default_params() { throw std::runtime_error("Not implemented"); } // Getters - virtual const SE3 pose() = 0; - virtual const std::map> map() = 0; + virtual const Map<> map() = 0; // Setters virtual void set_imu_params(ImuParams params) = 0; virtual void set_lidar_params(LidarParams params) = 0; virtual void set_imu_T_lidar(SE3 T) = 0; + // Should be implemented with EVALIO_SETUP_PARAMS virtual std::map set_params(std::map params) = 0; // Doers virtual void initialize() = 0; virtual void add_imu(ImuMeasurement mm) = 0; - virtual std::map> - add_lidar(LidarMeasurement mm) = 0; + virtual void add_lidar(LidarMeasurement mm) = 0; + + // ------------------------- For saving results ------------------------- // + /// @brief Save an estimated pose at a given timestamp. + /// + /// This accepts any type that has an impl of convert. + template + void save(const Stamp& stamp, const T& pose) { + saved_poses_.emplace_back(stamp, convert(pose)); + } + + /// @brief Save features and map at a given timestamp. + /// + /// This overload is mostly useful for python bindings. + void save(const Stamp& stamp, const Map<>& features) { + // Only convert & save if they'll be visualized + if (vis_options_ + && vis_options_->find(VisOption::FEATURES) != vis_options_->end()) { + saved_features_.emplace_back(stamp, features); + } + + // Only query the map if it'll be visualized + if (vis_options_ + && vis_options_->find(VisOption::MAP) != vis_options_->end()) { + saved_maps_.emplace_back(stamp, map()); + } + } + + /// @brief Save features and map at a given timestamp. + /// + /// This overload is useful for C++ usage with variadic arguments. + /// This allows both (a) delayed conversion of convert, T> and + /// (b) a pretty API for passing them. + template + void save(const Stamp& stamp, std::string&& key, T&& feat, Args&&... args) { + // Only convert & save if they'll be visualized + if (vis_options_ + && vis_options_->find(VisOption::FEATURES) != vis_options_->end()) { + saved_features_.emplace_back( + stamp, + make_map( + std::forward(key), + std::forward(feat), + std::forward(args)... + ) + ); + } + + // Only query the map if it'll be visualized + if (vis_options_ + && vis_options_->find(VisOption::MAP) != vis_options_->end()) { + saved_maps_.emplace_back(stamp, map()); + } + } + + // ------------------------- For Internal Usage ------------------------- // + const std::vector> saved_estimates() { + std::vector> copy; + copy.swap(saved_poses_); + return copy; + } + + const std::vector>> saved_features() { + std::vector>> copy; + copy.swap(saved_features_); + return copy; + } + + const std::vector>> saved_maps() { + std::vector>> copy; + copy.swap(saved_maps_); + return copy; + } + + // Save as Eigen matrices for easier use in Python + const std::vector>> saved_features_matrix() { + return eigen(saved_features()); + } + + const std::vector>> saved_maps_matrix() { + return eigen(saved_maps()); + } + + void set_visualizing(const std::optional>& options) { + vis_options_ = options; + } + +protected: + // Helper to convert std::vector to Eigen matrices + static inline std::vector>> + eigen(const std::vector>>& saved) { + std::vector>> cleaned; + // Iterate over stamps + for (const auto& [stamp, original] : saved) { + // Iterate over map entries + Map eigen; + for (const auto& [key, points] : original) { + eigen[key] = convert(points); + } + cleaned.emplace_back(stamp, eigen); + } + return cleaned; + } + +private: + std::vector> saved_poses_; + std::vector>> saved_features_; + std::vector>> saved_maps_; + std::optional> vis_options_ = std::nullopt; }; } // namespace evalio diff --git a/cpp/evalio/types.h b/cpp/evalio/types.h index b629f203..f5941ab3 100644 --- a/cpp/evalio/types.h +++ b/cpp/evalio/types.h @@ -8,6 +8,13 @@ namespace evalio { +enum VisOption { + MAP = 1, + FEATURES = 2, + SCAN = 3, + IMAGE = 4, +}; + struct Duration { // Also tried saving this in seconds, but found we had occasional floating // point errors when adding/subtracting durations. diff --git a/python/evalio/cli/run.py b/python/evalio/cli/run.py index 5a06e28d..5dd39199 100644 --- a/python/evalio/cli/run.py +++ b/python/evalio/cli/run.py @@ -1,5 +1,6 @@ import multiprocessing from pathlib import Path +from evalio._cpp.types import VisOption # type: ignore from evalio.cli.completions import DatasetOpt, PipelineOpt from evalio.types.base import Trajectory from evalio.utils import print_warning @@ -7,7 +8,7 @@ import yaml from evalio import datasets as ds, pipelines as pl, types as ty -from evalio.rerun import RerunVis, VisArgs +from evalio.rerun import RerunVis # from .stats import evaluate @@ -21,26 +22,22 @@ app = typer.Typer() -# def save_config( -# pipelines: Sequence[PipelineBuilder], -# datasets: Sequence[DatasetBuilder], -# output: Path, -# ): -# # If it's just a file, don't save the entire config file -# if output.suffix == ".csv": -# return +def parse_vis(opts: str) -> set[VisOption]: + out = set[VisOption]() + for o in opts: + match o: + case "m": + out.add(VisOption.MAP) + case "i": + out.add(VisOption.IMAGE) + case "s": + out.add(VisOption.SCAN) + case "f": + out.add(VisOption.FEATURES) + case _: + raise typer.BadParameter(f"Unknown visualization option {o}") -# print(f"Saving config to {output}") - -# output.mkdir(parents=True, exist_ok=True) -# path = output / "config.yaml" - -# out = dict() -# out["datasets"] = [d.as_dict() for d in datasets] -# out["pipelines"] = [p.as_dict() for p in pipelines] - -# with open(path, "w") as f: -# yaml.dump(out, f) + return out @app.command(no_args_is_help=True, name="run", help="Run pipelines on datasets") @@ -90,13 +87,13 @@ def run_from_cli( ), ] = False, show: Annotated[ - Optional[VisArgs], + Optional[set[VisOption]], typer.Option( "-s", "--show", help="Show visualization options (m: map, i: image, s: scan, f: features). Automatically implies -v.", show_default=False, - parser=VisArgs.parse, + parser=parse_vis, ), ] = None, rerun_failed: Annotated[ @@ -212,8 +209,12 @@ def run_from_cli( print(f"Output will be saved to {out}\n") # Go through visualization options + vis_args: Optional[set[VisOption]] if show is None: - vis_args = VisArgs(show=visualize) + if visualize: + vis_args = set() + else: + vis_args = None else: vis_args = show vis = RerunVis(vis_args, [p[0] for p in pipelines]) @@ -343,10 +344,11 @@ def run_single( print_warning(f"Error setting up experiment {exp.name}: {output}") return pipe, dataset = output + pipe.set_visualizing(vis.args) exp.status = ty.ExperimentStatus.Started traj = ty.Trajectory(metadata=exp) - traj.open(exp.file) - vis.new_pipe(exp.name) + traj.open() + vis.new_pipe(exp.name, len(pipe.map())) time_running = 0.0 time_max = 0.0 @@ -360,8 +362,7 @@ def run_single( time_running += time() - start elif isinstance(data, ty.LidarMeasurement): # type: ignore start = time() - features = pipe.add_lidar(data) - pose = pipe.pose() + pipe.add_lidar(data) time_running += time() - start time_total += time_running @@ -369,13 +370,22 @@ def run_single( time_max = time_running time_running = 0.0 - traj.append(data.stamp, pose) - vis.log(data, features, pose, pipe) + vis.log_scan(data) loop.update() - if loop.n >= exp.sequence_length: - loop.close() - break + + # Save and visualize any new information + for stamp, pose in pipe.saved_estimates(): + traj.append(stamp, pose) + vis.log_pose(stamp, pose) + for stamp, features in pipe.saved_features_matrix(): + vis.log_features(stamp, features) + for stamp, map in pipe.saved_maps_matrix(): + vis.log_map(stamp, map) + + if loop.n >= exp.sequence_length: + loop.close() + break loop.close() traj.metadata.status = ty.ExperimentStatus.Complete diff --git a/python/evalio/rerun.py b/python/evalio/rerun.py index 406b4f75..0a614014 100644 --- a/python/evalio/rerun.py +++ b/python/evalio/rerun.py @@ -1,15 +1,12 @@ -from dataclasses import dataclass -from typing import Any, Literal, Optional, Sequence, TypedDict, cast, overload +from typing import Any, Literal, Optional, TypedDict, cast, overload from typing_extensions import TypeVar from uuid import UUID, uuid4 import distinctipy import numpy as np -import typer from numpy.typing import NDArray from evalio.datasets import Dataset -from evalio.pipelines import Pipeline from evalio.types import ( SE3, GroundTruth, @@ -22,6 +19,7 @@ ) from evalio.utils import print_warning from evalio._cpp.helpers import closest # type: ignore +from evalio._cpp.types import VisOption # type: ignore # These colors are pulled directly from the rerun skybox colors @@ -46,33 +44,6 @@ def skybox_light_rgb(dir: NDArray[np.float64]) -> tuple[float, float, float]: ) # Color for ground truth in rerun -@dataclass -class VisArgs: - show: bool - map: bool = False - image: bool = False - scan: bool = False - features: bool = False - - @staticmethod - def parse(opts: str) -> "VisArgs": - out = VisArgs(show=True) - for o in opts: - match o: - case "m": - out.map = True - case "i": - out.image = True - case "s": - out.scan = True - case "f": - out.features = True - case _: - raise typer.BadParameter(f"Unknown visualization option {o}") - - return out - - try: import rerun as rr import rerun.blueprint as rrb @@ -83,21 +54,23 @@ def parse(opts: str) -> "VisArgs": ) class RerunVis: # type: ignore - def __init__(self, args: VisArgs, pipeline_names: list[str]): + def __init__(self, args: Optional[set[VisOption]], pipeline_names: list[str]): self.args = args + self.pipeline_names = pipeline_names - # To be set during new_recording + # To be set during new_dataset self.lidar_params: Optional[LidarParams] = None + self.imu_T_lidar: Optional[SE3] = None self.gt: Optional[Trajectory[GroundTruth]] = None - self.pipeline_names = pipeline_names - # To be found during log - self.gt_o_T_imu_o: Optional[SE3] = None + # To be set during new_pipe self.trajectory: Optional[Trajectory] = None - self.imu_T_lidar: Optional[SE3] = None self.pn: Optional[str] = None self.colors: Optional[list[tuple[float, float, float]]] = None + # To be set during log + self.gt_o_T_imu_o: Optional[SE3] = None + directions = np.array( [ [1, 0, 0], @@ -127,7 +100,7 @@ def _blueprint(self) -> rrb.BlueprintLike: for n in self.pipeline_names } - if self.args.image: + if self.args is not None and VisOption.IMAGE in self.args: return rrb.Blueprint( rrb.Vertical( rrb.Spatial2DView(), # image @@ -139,7 +112,7 @@ def _blueprint(self) -> rrb.BlueprintLike: return rrb.Blueprint(rrb.Spatial3DView(overrides=overrides)) def new_dataset(self, dataset: Dataset): - if not self.args.show: + if self.args is None: return self.recording_params: RerunArgs = { @@ -157,63 +130,86 @@ def new_dataset(self, dataset: Dataset): self.rec.log("gt", convert(self.gt, color=GT_COLOR), static=True) - def new_pipe(self, pipe_name: str): - if not self.args.show: + # reset other variables + self.pn = None + self.gt_o_T_imu_o = None + self.trajectory = None + self.colors = None + + def new_pipe(self, pipe_name: str, feat_num: int): + if self.args is None: return if self.imu_T_lidar is None: - raise ValueError( - "You needed to initialize the recording before adding a pipeline!" - ) + raise ValueError("You needed to add a dataset before a pipeline!") + + # Define colors for this pipeline + # features/map colors + trajectory + scan + self.colors = distinctipy.get_colors( + feat_num + 2, + exclude_colors=self.used_colors, + rng=0, + ) + self.used_colors.extend(self.colors) # First reconnect to make sure we're connected (happens b/c of multithread passing) self.rec = rr.RecordingStream(**self.recording_params) self.rec.connect_grpc() self.pn = pipe_name - self.gt_o_T_imu_o = None - self.colors = None self.trajectory = Trajectory(stamps=[], poses=[]) self.rec.log(f"{self.pn}/imu/lidar", convert(self.imu_T_lidar), static=True) - def log( - self, - data: LidarMeasurement, - features: dict[str, list[Point]], - pose: SE3, - pipe: Pipeline, - ): - if not self.args.show: + self.gt_o_T_imu_o = None + + def log_scan(self, data: LidarMeasurement): + if self.args is None: return - if self.colors is None: - # features/map colors + trajectory + scan - self.colors = distinctipy.get_colors( - len(features) + 2, - exclude_colors=self.used_colors, - rng=0, + if self.lidar_params is None or self.gt is None: + raise ValueError("You needed to add a dataset before stepping!") + if self.pn is None or self.trajectory is None or self.colors is None: + raise ValueError("You needed to add a pipeline before stepping!") + + self.rec.set_time("evalio_time", timestamp=data.stamp.to_sec()) + + # Include the original point cloud + if VisOption.SCAN in self.args: + self.rec.log( + f"{self.pn}/imu/lidar/scan", + convert(data, color=self.colors[-2], radii=0.08), ) - self.used_colors.extend(self.colors) - if self.lidar_params is None or self.gt is None: - raise ValueError( - "You needed to initialize the recording before stepping!" + # Include the intensity image + if VisOption.IMAGE in self.args: + intensity = np.array([d.intensity for d in data.points]) + # row major order + image = intensity.reshape( + (self.lidar_params.num_rows, self.lidar_params.num_columns) ) - if self.pn is None or self.trajectory is None: + self.rec.log("image", rr.Image(image)) + + def log_pose(self, stamp: Stamp, pose: SE3): + if self.args is None: + return + + if self.lidar_params is None or self.gt is None: + raise ValueError("You needed to add a dataset before stepping!") + if self.pn is None or self.trajectory is None or self.colors is None: raise ValueError("You needed to add a pipeline before stepping!") # Find transform between ground truth and imu origins if self.gt_o_T_imu_o is None: - if data.stamp < self.gt.stamps[0]: + if stamp < self.gt.stamps[0]: pass else: imu_o_T_imu_0 = pose # find the ground truth pose that is temporally closest to the imu pose gt_index = 0 - while self.gt.stamps[gt_index] < data.stamp: + while self.gt.stamps[gt_index] < stamp: gt_index += 1 if not closest( - data.stamp, + stamp, self.gt.stamps[gt_index - 1], self.gt.stamps[gt_index], ): @@ -223,44 +219,50 @@ def log( self.rec.log(self.pn, convert(self.gt_o_T_imu_o), static=True) # Always include the pose - self.rec.set_time("evalio_time", timestamp=data.stamp.to_sec()) + self.rec.set_time("evalio_time", timestamp=stamp.to_sec()) self.rec.log(f"{self.pn}/imu", convert(pose)) - self.trajectory.append(data.stamp, pose) + self.trajectory.append(stamp, pose) self.rec.log( f"{self.pn}/trajectory", convert(self.trajectory, color=self.colors[-1]), ) + def log_map(self, stamp: Stamp, map: dict[str, NDArray[np.float64]]): + if self.args is None: + return + + if self.lidar_params is None or self.gt is None: + raise ValueError("You needed to add a pipeline before stepping!") + if self.pn is None or self.trajectory is None or self.colors is None: + raise ValueError("You needed to add a pipeline before stepping!") + + self.rec.set_time("evalio_time", timestamp=stamp.to_sec()) + + # Include the current map + if VisOption.MAP in self.args: + for (k, p), c in zip(map.items(), self.colors): + self.rec.log(f"{self.pn}/map/{k}", convert(p, color=c, radii=0.03)) + + def log_features(self, stamp: Stamp, features: dict[str, NDArray[np.float64]]): + if self.args is None: + return + + if self.lidar_params is None or self.gt is None: + raise ValueError("You needed to add a pipeline before stepping!") + if self.pn is None or self.trajectory is None or self.colors is None: + raise ValueError("You needed to add a pipeline before stepping!") + + self.rec.set_time("evalio_time", timestamp=stamp.to_sec()) + # Features from the scan - if self.args.features: + if VisOption.FEATURES in self.args: if len(features) > 0: for (k, p), c in zip(features.items(), self.colors): self.rec.log( f"{self.pn}/imu/lidar/{k}", - convert(list(p), color=c, radii=0.12), + convert(p, color=c, radii=0.12), ) - # Include the current map - if self.args.map: - for (k, p), c in zip(pipe.map().items(), self.colors): - self.rec.log(f"{self.pn}/map/{k}", convert(p, color=c, radii=0.03)) - - # Include the original point cloud - if self.args.scan: - self.rec.log( - f"{self.pn}/imu/lidar/scan", - convert(data, color=self.colors[-2], radii=0.08), - ) - - # Include the intensity image - if self.args.image: - intensity = np.array([d.intensity for d in data.points]) - # row major order - image = intensity.reshape( - (self.lidar_params.num_rows, self.lidar_params.num_columns) - ) - self.rec.log("image", rr.Image(image)) - # ------------------------- For converting to rerun types ------------------------- # # point clouds @overload @@ -309,7 +311,12 @@ def convert( @overload def convert( obj: NDArray[np.float64], - color: Optional[Literal["z"] | NDArray[np.float64]] = None, + color: Optional[ + Literal["z"] + | NDArray[np.float64] + | tuple[int, int, int] + | tuple[float, float, float] + ] = None, radii: Optional[float] = None, ) -> rr.Points3D: """Convert an (n, 3) numpy array to a rerun Points3D. @@ -467,21 +474,26 @@ def convert( except Exception: class RerunVis: - def __init__(self, args: VisArgs, pipeline_names: list[str]) -> None: - if args.show: + def __init__( + self, args: Optional[set[VisOption]], pipeline_names: list[str] + ) -> None: + if args is not None: print_warning("Rerun not found, visualization disabled") def new_dataset(self, dataset: Dataset): pass - def log( - self, - data: LidarMeasurement, - features: Sequence[Point], - pose: SE3, - pipe: Pipeline, - ): + def new_pipe(self, pipe_name: str, feat_num: int): + pass + + def log_scan(self, data: LidarMeasurement): + pass + + def log_pose(self, stamp: Stamp, pose: SE3): + pass + + def log_map(self, stamp: Stamp, map: dict[str, NDArray[np.float64]]): pass - def new_pipe(self, pipe_name: str): + def log_features(self, stamp: Stamp, features: dict[str, NDArray[np.float64]]): pass