diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index b4653494..c393cf39 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -7,11 +7,11 @@ jobs: runs-on: ubuntu-latest container: ros:jazzy-perception steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v6 with: {path: src/hydra} - name: CCache id: cache-ccache - uses: actions/cache@v4 + uses: actions/cache@v6 with: path: ~/.cache/ccache key: ${{ runner.os }}-ccache-${{ github.sha }} diff --git a/.github/workflows/linting.yaml b/.github/workflows/linting.yaml index f7a7018c..7eaeb11a 100644 --- a/.github/workflows/linting.yaml +++ b/.github/workflows/linting.yaml @@ -6,7 +6,7 @@ jobs: lint: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v4 + - uses: actions/checkout@v6 - name: Dependencies run: sudo apt-get update && sudo apt install pipx - name: Lint diff --git a/config/label_spaces/gdsam2_default_label_space.yaml b/config/label_spaces/gdsam2_default_label_space.yaml deleted file mode 100644 index 64dbeb8b..00000000 --- a/config/label_spaces/gdsam2_default_label_space.yaml +++ /dev/null @@ -1,15 +0,0 @@ ---- -# YOLOv11 Label Space (COCO 80 classes) -# Based on COCO dataset for instance segmentation -# Dynamic: humans, animals, airplanes -# Objects: vehicles (including bicycles), static objects, furniture, etc. -total_semantic_labels: 4 -dynamic_labels: [] -invalid_labels: [] -object_labels: [0, 1, 2, 3] -surface_places_labels: [] -label_names: - - {label: 0, name: bench} - - {label: 1, name: car} - - {label: 2, name: trash bin} - - {label: 3, name: fire hydrant} diff --git a/config/label_spaces/gdsam2_mit_label_space.yaml b/config/label_spaces/gdsam2_mit_label_space.yaml deleted file mode 100644 index 5c327103..00000000 --- a/config/label_spaces/gdsam2_mit_label_space.yaml +++ /dev/null @@ -1,55 +0,0 @@ ---- -total_semantic_labels: 48 -dynamic_labels: [] -invalid_labels: [11, 39] -object_labels: [1, 2, 3, 17, 18, 24, 26, 27, 29, 33, 34, 35, 36, 37, 38, 40, 41, 42, 43, 44, 45, 46, 47] -surface_places_labels: [0, 4, 5, 6, 7, 8, 9, 10, 12, 13, 14, 15, 16, 19, 20, 21, 22, 23, 25, 28, 30, 31, 32] -label_names: - - {label: 0, name: unknown} - - {label: 1, name: green chair} - - {label: 2, name: van} - - {label: 3, name: fire hydrant} - - {label: 4, name: ground} - - {label: 5, name: grass} - - {label: 6, name: sand} - - {label: 7, name: sidewalk} - - {label: 8, name: dock} - - {label: 9, name: road} - - {label: 10, name: path} - - {label: 11, name: ignore} - - {label: 12, name: building} - - {label: 13, name: shelter} - - {label: 14, name: signal} - - {label: 15, name: rock} - - {label: 16, name: fence} - - {label: 17, name: boat} - - {label: 18, name: sign} - - {label: 19, name: hill} - - {label: 20, name: bridge} - - {label: 21, name: wall} - - {label: 22, name: floor} - - {label: 23, name: ceiling} - - {label: 24, name: door} - - {label: 25, name: stairs} - - {label: 26, name: pole} - - {label: 27, name: rail} - - {label: 28, name: structure} - - {label: 29, name: window} - - {label: 30, name: surface} - - {label: 31, name: flora} - - {label: 32, name: flower} - - {label: 33, name: bed} - - {label: 34, name: box} - - {label: 35, name: storage} - - {label: 36, name: barrel} - - {label: 37, name: bag} - - {label: 38, name: basket} - - {label: 39, name: ignore} - - {label: 40, name: flag} - - {label: 41, name: decor} - - {label: 42, name: light} - - {label: 43, name: appliance} - - {label: 44, name: trash} - - {label: 45, name: bicycle} - - {label: 46, name: food} - - {label: 47, name: clothes} diff --git a/config/label_spaces/kitti_360_label_space.yaml b/config/label_spaces/kitti_360_label_space.yaml deleted file mode 100644 index c6ab5dc3..00000000 --- a/config/label_spaces/kitti_360_label_space.yaml +++ /dev/null @@ -1,53 +0,0 @@ ---- -total_semantic_labels: 46 -dynamic_labels: [1, 5, 24, 25, 26, 27, 28, 30, 31, 32, 33, 43] -invalid_labels: [0, 1, 2, 3, 4, 5, 23, 45] -object_labels: [17, 19, 20, 26, 27, 28, 29, 30, 31, 32, 33, 36, 37, 38, 39, 40, 41] -surface_places_labels: [6, 7, 8, 9, 10, 15, 21, 22] -label_names: - - {label: 0, name: unlabeled, name_descriptive: unknown} - - {label: 1, name: ego_vehicle, name_descriptive: an ego vehicle} - - {label: 2, name: rectification, name_descriptive: a rectification border} - - {label: 3, name: out, name_descriptive: out of region of interest} - - {label: 4, name: static, name_descriptive: a static object} - - {label: 5, name: dynamic, name_descriptive: a dynamic object} - - {label: 6, name: ground, name_descriptive: the ground} - - {label: 7, name: road, name_descriptive: a part of a road} - - {label: 8, name: sidewalk, name_descriptive: a sidewalk} - - {label: 9, name: parking, name_descriptive: a parking space} - - {label: 10, name: rail, name_descriptive: a railroad track} - - {label: 11, name: building, name_descriptive: a building} - - {label: 12, name: wall, name_descriptive: a wall} - - {label: 13, name: fence, name_descriptive: a fence} - - {label: 14, name: guard, name_descriptive: a guard rail} - - {label: 15, name: bridge, name_descriptive: a bridge} - - {label: 16, name: tunnel, name_descriptive: a tunnel} - - {label: 17, name: pole, name_descriptive: a pole} - - {label: 18, name: pole_group, name_descriptive: a group of poles} - - {label: 19, name: traffic_light, name_descriptive: a traffic light} - - {label: 20, name: traffic_sign, name_descriptive: a traffic sign} - - {label: 21, name: vegetation, name_descriptive: vegetation} - - {label: 22, name: terrain, name_descriptive: terrain} - - {label: 23, name: sky, name_descriptive: the sky} - - {label: 24, name: person, name_descriptive: a person} - - {label: 25, name: rider, name_descriptive: a rider} - - {label: 26, name: car, name_descriptive: a car} - - {label: 27, name: truck, name_descriptive: a truck} - - {label: 28, name: bus, name_descriptive: a bus} - - {label: 29, name: caravan, name_descriptive: a caravan} - - {label: 30, name: trailer, name_descriptive: a trailer} - - {label: 31, name: train, name_descriptive: a train} - - {label: 32, name: motorcycle, name_descriptive: a motorcycle} - - {label: 33, name: bicycle, name_descriptive: a bicycle} - - {label: 34, name: garage, name_descriptive: a garage} - - {label: 35, name: gate, name_descriptive: a gate} - - {label: 36, name: stop, name_descriptive: a stop} - - {label: 37, name: small_pole, name_descriptive: a small pole} - - {label: 38, name: lamp, name_descriptive: a lamp} - - {label: 39, name: trash, name_descriptive: a trash can} - - {label: 40, name: vending, name_descriptive: a vending machine} - - {label: 41, name: box, name_descriptive: a box} - - {label: 42, name: construction, name_descriptive: construction} - - {label: 43, name: vehicle, name_descriptive: an unknown vehicle} - - {label: 44, name: object, name_descriptive: an unknown object} - - {label: 45, name: license, name_descriptive: a license plate} diff --git a/config/label_spaces/phoenix_label_space.yaml b/config/label_spaces/phoenix_label_space.yaml deleted file mode 100644 index 467729b3..00000000 --- a/config/label_spaces/phoenix_label_space.yaml +++ /dev/null @@ -1,31 +0,0 @@ ---- -total_semantic_labels: 24 -dynamic_labels: [16] -invalid_labels: [] -obejct_labels: [3, 4, 7, 8, 14, 15, 18, 19, 20, 23] -surface_places_labels: [0, 1, 2, 5, 9, 10, 12, 13, 21, 22] -label_names: - - {label: 0, name: dirt} - - {label: 1, name: sand} - - {label: 2, name: grass} - - {label: 3, name: tree} - - {label: 4, name: pole} - - {label: 5, name: water} - - {label: 6, name: sky} - - {label: 7, name: vehicle} - - {label: 8, name: container} - - {label: 9, name: asphalt} - - {label: 10, name: gravel} - - {label: 11, name: building} - - {label: 12, name: mulch} - - {label: 13, name: rock-bed} - - {label: 14, name: log} - - {label: 15, name: bicycle} - - {label: 16, name: person} - - {label: 17, name: fence} - - {label: 18, name: bush} - - {label: 19, name: sign} - - {label: 20, name: rock} - - {label: 21, name: bridge} - - {label: 22, name: concrete} - - {label: 23, name: picnic-table} diff --git a/config/label_spaces/yolov11_coco_label_space.yaml b/config/label_spaces/yolov11_coco_label_space.yaml deleted file mode 100644 index 2e3869d4..00000000 --- a/config/label_spaces/yolov11_coco_label_space.yaml +++ /dev/null @@ -1,162 +0,0 @@ ---- -# YOLOv11 Label Space (COCO 80 classes) -# Based on COCO dataset for instance segmentation -# Dynamic: humans, animals, airplanes -# Objects: vehicles (including bicycles), static objects, furniture, etc. -total_semantic_labels: 81 -dynamic_labels: [0, 4, 8, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23] -invalid_labels: [80] -object_labels: - - 1 - - 2 - - 3 - - 5 - - 6 - - 7 - - 9 - - 10 - - 11 - - 12 - - 13 - - 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 - - 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 -# dynamic_labels: [] -# invalid_labels: [1, 2, 3, 5, 6, 7, 9, 10, 11, 12, 13, 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, 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, 0, 4, 8, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23] -# object_labels: [2, 13] -surface_places_labels: [] -label_names: - - {label: 0, name: person} - - {label: 1, name: bicycle} - - {label: 2, name: car} - - {label: 3, name: motorcycle} - - {label: 4, name: airplane} - - {label: 5, name: bus} - - {label: 6, name: train} - - {label: 7, name: truck} - - {label: 8, name: boat} - - {label: 9, name: traffic light} - - {label: 10, name: fire hydrant} - - {label: 11, name: stop sign} - - {label: 12, name: parking meter} - - {label: 13, name: bench} - - {label: 14, name: bird} - - {label: 15, name: cat} - - {label: 16, name: dog} - - {label: 17, name: horse} - - {label: 18, name: sheep} - - {label: 19, name: cow} - - {label: 20, name: elephant} - - {label: 21, name: bear} - - {label: 22, name: zebra} - - {label: 23, name: giraffe} - - {label: 24, name: backpack} - - {label: 25, name: umbrella} - - {label: 26, name: handbag} - - {label: 27, name: tie} - - {label: 28, name: suitcase} - - {label: 29, name: frisbee} - - {label: 30, name: skis} - - {label: 31, name: snowboard} - - {label: 32, name: sports ball} - - {label: 33, name: kite} - - {label: 34, name: baseball bat} - - {label: 35, name: baseball glove} - - {label: 36, name: skateboard} - - {label: 37, name: surfboard} - - {label: 38, name: tennis racket} - - {label: 39, name: bottle} - - {label: 40, name: wine glass} - - {label: 41, name: cup} - - {label: 42, name: fork} - - {label: 43, name: knife} - - {label: 44, name: spoon} - - {label: 45, name: bowl} - - {label: 46, name: banana} - - {label: 47, name: apple} - - {label: 48, name: sandwich} - - {label: 49, name: orange} - - {label: 50, name: broccoli} - - {label: 51, name: carrot} - - {label: 52, name: hot dog} - - {label: 53, name: pizza} - - {label: 54, name: donut} - - {label: 55, name: cake} - - {label: 56, name: chair} - - {label: 57, name: couch} - - {label: 58, name: potted plant} - - {label: 59, name: bed} - - {label: 60, name: dining table} - - {label: 61, name: toilet} - - {label: 62, name: tv} - - {label: 63, name: laptop} - - {label: 64, name: mouse} - - {label: 65, name: remote} - - {label: 66, name: keyboard} - - {label: 67, name: cell phone} - - {label: 68, name: microwave} - - {label: 69, name: oven} - - {label: 70, name: toaster} - - {label: 71, name: sink} - - {label: 72, name: refrigerator} - - {label: 73, name: book} - - {label: 74, name: clock} - - {label: 75, name: vase} - - {label: 76, name: scissors} - - {label: 77, name: teddy bear} - - {label: 78, name: hair drier} - - {label: 79, name: toothbrush} - - {label: 80, name: unknown} diff --git a/include/hydra/common/semantic_color_map.h b/include/hydra/common/semantic_color_map.h index 4d43958b..87b17e43 100644 --- a/include/hydra/common/semantic_color_map.h +++ b/include/hydra/common/semantic_color_map.h @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -70,6 +71,8 @@ class SemanticColorMap { std::string toString() const; + cv::Mat colorsToLabels(const cv::Mat& colors, int32_t default_label = -1) const; + public: static SemanticColorMap::Ptr randomColors(size_t num_labels, const spark_dsg::Color& unknown = {}); diff --git a/include/hydra/input/input_conversion.h b/include/hydra/input/input_conversion.h index 70a5f699..079a1db5 100644 --- a/include/hydra/input/input_conversion.h +++ b/include/hydra/input/input_conversion.h @@ -23,21 +23,13 @@ std::unique_ptr parseInputPacket(const InputPacket& input_packet, bool normalize_labels = true); /** - * @brief make sure that all the images are of the right type + * @brief Ensure that all the images are of the right type + * @param data Input data to normalize + * @param normalize_labels Whether or not to ensure labels are the correct type + * @return Whether or not the input normalization was successful */ bool normalizeData(InputData& data, bool normalize_labels = true); -bool normalizeDepth(InputData& data); - -bool colorToLabels(cv::Mat& label_image, const cv::Mat& colors); - -// TODO(nathan) check if the conversions are directly used... -bool convertLabels(InputData& data); - -bool convertDepth(InputData& data); - -bool convertColor(InputData& data); - /** * @brief Ensure that the vertex map is in the correct frame. * @param data The input data to convert. diff --git a/include/hydra/input/input_data.h b/include/hydra/input/input_data.h index 5c56f44d..f90ca5a7 100644 --- a/include/hydra/input/input_data.h +++ b/include/hydra/input/input_data.h @@ -19,52 +19,42 @@ struct InputData { using RangeType = float; using VertexType = cv::Vec3f; using LabelType = int; + using InstanceType = int16_t; explicit InputData(Sensor::ConstPtr sensor) : sensor_(std::move(sensor)) {} virtual ~InputData() = default; //! Time stamp this input data was captured. TimeStamp timestamp_ns; - //! Pose of the robot body in the world frame. Eigen::Isometry3d world_T_body; - //! Color image as RGB. cv::Mat color_image; - //! Depth image as planar depth in meters. cv::Mat depth_image; - //! Ray lengths in meters. cv::Mat range_image; - //! Label image for semantic input data. cv::Mat label_image; - + //! Instance image for semantic input data. + cv::Mat instance_image; //! 3D points of the range image in sensor or world frame. cv::Mat vertex_map; //! Whether or not the vertex map is in the world frame (or sensor frame). bool points_in_world_frame = false; - //! Min range observed in the range image. float min_range = 0.0f; //! Max range observed in the range image. float max_range = std::numeric_limits::infinity(); - //! Feature associated with current input data FeatureVector feature; - //! Features associated with each label FeatureMap label_features; - /** - * @brief Get the sensor that captured this data. - */ + //! Get the sensor that captured this data. const Sensor& getSensor() const { return *sensor_; } - /** - * @brief Get the pose of the sensor in world frame when this data was captured. - */ + //! Get the pose of the sensor in world frame when this data was captured. Eigen::Isometry3d getSensorPose() const { return world_T_body * sensor_->body_T_sensor(); } diff --git a/include/hydra/input/input_packet.h b/include/hydra/input/input_packet.h index fe465626..30e8ed35 100644 --- a/include/hydra/input/input_packet.h +++ b/include/hydra/input/input_packet.h @@ -34,7 +34,6 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include #include "hydra/input/sensor_input_packet.h" diff --git a/include/hydra/input/sensor_input_packet.h b/include/hydra/input/sensor_input_packet.h index b6e58f0a..b5b1b1a5 100644 --- a/include/hydra/input/sensor_input_packet.h +++ b/include/hydra/input/sensor_input_packet.h @@ -47,8 +47,11 @@ struct SensorInputPacket { bool fillInputData(InputData& msg) const; + //! Timestamp sensor input is acquired const uint64_t timestamp_ns; + //! Sensor that acquired the input const std::string sensor_name; + //! Frame ID for sensor std::string sensor_frame; //! Learned feature for the input data (e.g., CLIP for camera) FeatureVector input_feature; @@ -66,6 +69,8 @@ struct ImageInputPacket : public SensorInputPacket { cv::Mat depth; //! Labels for each pixel cv::Mat labels; + //! Instance IDs for each pixel + cv::Mat instances; //! Features associated with each label FeatureMap label_features; //! Whether or not the input color image is bgr order @@ -78,10 +83,16 @@ struct ImageInputPacket : public SensorInputPacket { struct CloudInputPacket : public SensorInputPacket { explicit CloudInputPacket(uint64_t stamp, const std::string& sensor_name); + //! Whether or not the pointcloud is in the world frame bool in_world_frame = false; + //! Points for pointcloud cv::Mat points; + //! Colors for each point cv::Mat colors; + //! Labels for each point cv::Mat labels; + //! Instance IDs for each point + cv::Mat instances; protected: bool fillInputDataImpl(InputData& msg) const override; diff --git a/src/common/global_info.cpp b/src/common/global_info.cpp index 1425dc80..d8d6ce10 100644 --- a/src/common/global_info.cpp +++ b/src/common/global_info.cpp @@ -80,6 +80,8 @@ void declare_config(PipelineConfig& config) { field(config.store_visualization_details, "store_visualization_details"); config.map_window.setOptional(); field(config.map_window, "map_window"); + config.labelspace.setOptional(); + field(config.labelspace, "labelspace"); field(config.label_remap_filepath, "semantic_label_remap_filepath"); field(config.mesh, "mesh"); field(config.semantic_layers, "semantic_layers"); @@ -87,8 +89,6 @@ void declare_config(PipelineConfig& config) { // the following subconfigs should not be namespaced field(config.frames, "frames", false); field(config.graph, "graph", false); - config.labelspace.setOptional(); - field(config.labelspace, "labelspace", false); } GlobalInfo::GlobalInfo() : force_shutdown_(false) {} diff --git a/src/common/semantic_color_map.cpp b/src/common/semantic_color_map.cpp index 8132188d..45194ff2 100644 --- a/src/common/semantic_color_map.cpp +++ b/src/common/semantic_color_map.cpp @@ -147,6 +147,27 @@ SemanticColorMap::Ptr SemanticColorMap::fromCsv(const std::string& filename, return std::make_unique(cmap, unknown); } +cv::Mat SemanticColorMap::colorsToLabels(const cv::Mat& colors, + int32_t default_label) const { + if (colors.empty() || colors.channels() != 3) { + LOG(ERROR) << "3-channel color image required for conversion to labels!"; + return cv::Mat(); + } + + CHECK_EQ(colors.type(), CV_8UC3); + + cv::Mat label_image(colors.size(), CV_32SC1); + for (int r = 0; r < colors.rows; ++r) { + for (int c = 0; c < colors.cols; ++c) { + const auto& pixel = colors.at(r, c); + const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]); + label_image.at(r, c) = getLabelFromColor(color).value_or(default_label); + } + } + + return label_image; +} + SemanticColorMap::Ptr SemanticColorMap::randomColors(size_t num_labels, const Color& unknown) { const std::vector defaults{Color::gray(), diff --git a/src/input/input_conversion.cpp b/src/input/input_conversion.cpp index 5740f2d3..3f995320 100644 --- a/src/input/input_conversion.cpp +++ b/src/input/input_conversion.cpp @@ -5,7 +5,6 @@ #include #include "hydra/common/global_info.h" -#include "hydra/common/semantic_color_map.h" #include "hydra/input/input_packet.h" #include "hydra/input/sensor.h" @@ -19,92 +18,6 @@ inline std::string showTypeInfo(const cv::Mat& mat) { return ss.str(); } -} // namespace - -std::unique_ptr parseInputPacket(const InputPacket& input_packet, - bool vertices_in_world_frame, - bool normalize_labels) { - if (!input_packet.sensor_input) { - LOG(ERROR) << "[Input Conversion] Input packet has no sensor input."; - return nullptr; - } - - const auto& sensor_name = input_packet.sensor_input->sensor_name; - auto sensor = GlobalInfo::instance().getSensor(sensor_name); - if (!sensor) { - LOG(ERROR) << "[Input Conversion] Missing sensor '" << sensor_name - << "' for input packet @ " << input_packet.timestamp_ns << " [ns]"; - return nullptr; - } - - auto data = std::make_unique(sensor); - if (!input_packet.fillInputData(*data)) { - LOG(ERROR) << "[Input Conversion] Unable to fill input data from input packet."; - return nullptr; - } - - if (!normalizeData(*data, normalize_labels)) { - LOG(ERROR) << "[Input Conversion] Unable to normalize data."; - return nullptr; - } - - if (!data->getSensor().finalizeRepresentations(*data)) { - LOG(ERROR) << "[Input Conversion] Unable to compute inputs for integration"; - return nullptr; - } - - convertVertexMap(*data, vertices_in_world_frame); - return data; -} - -bool normalizeDepth(InputData& data) { return convertDepth(data); } - -bool normalizeData(InputData& data, bool normalize_labels) { - if (!convertDepth(data)) { - return false; - } - - if (!convertColor(data)) { - return false; - } - - // must come after convertColor as it uses color image - if (normalize_labels && !convertLabels(data)) { - return false; - } - - if (!data.vertex_map.empty() && data.vertex_map.type() != CV_32FC3) { - LOG(ERROR) << "pointcloud must be of type CV_32FC3, not " - << showTypeInfo(data.vertex_map); - return false; - } - - return true; -} - -bool colorToLabels(const SemanticColorMap& colormap, - cv::Mat& label_image, - const cv::Mat& colors) { - if (colors.empty() || colors.channels() != 3) { - LOG(ERROR) << "color image required to decode semantic labels"; - return false; - } - - CHECK_EQ(colors.type(), CV_8UC3); - - label_image = cv::Mat(colors.size(), CV_32SC1); - for (int r = 0; r < colors.rows; ++r) { - for (int c = 0; c < colors.cols; ++c) { - const auto& pixel = colors.at(r, c); - const spark_dsg::Color color(pixel[0], pixel[1], pixel[2]); - // this is lazy, but works out to the same invalid label we normally use - label_image.at(r, c) = colormap.getLabelFromColor(color).value_or(-1); - } - } - - return true; -} - bool convertLabels(InputData& data) { if (data.label_image.empty()) { return false; @@ -188,17 +101,78 @@ bool convertColor(InputData& data) { return true; } +} // namespace + +std::unique_ptr parseInputPacket(const InputPacket& input_packet, + bool vertices_in_world_frame, + bool normalize_labels) { + if (!input_packet.sensor_input) { + LOG(ERROR) << "[Input Conversion] Input packet has no sensor input."; + return nullptr; + } + + const auto& name = input_packet.sensor_input->sensor_name; + auto sensor = GlobalInfo::instance().getSensor(name); + if (!sensor) { + LOG(ERROR) << "[Input Conversion] Missing sensor '" << name + << "' for input packet @ " << input_packet.timestamp_ns << " [ns]"; + return nullptr; + } + + auto data = std::make_unique(sensor); + if (!input_packet.fillInputData(*data)) { + LOG(ERROR) << "[Input Conversion] Unable to fill input data from input packet."; + return nullptr; + } + + if (!normalizeData(*data, normalize_labels)) { + LOG(ERROR) << "[Input Conversion] Unable to normalize data."; + return nullptr; + } + + if (!data->getSensor().finalizeRepresentations(*data)) { + LOG(ERROR) << "[Input Conversion] Unable to compute inputs for integration"; + return nullptr; + } + + convertVertexMap(*data, vertices_in_world_frame); + return data; +} + +bool normalizeData(InputData& data, bool normalize_labels) { + if (!convertDepth(data)) { + return false; + } + + if (!convertColor(data)) { + return false; + } + + if (normalize_labels && !convertLabels(data)) { + return false; + } + + if (!data.vertex_map.empty() && data.vertex_map.type() != CV_32FC3) { + LOG(ERROR) << "pointcloud must be CV_32FC3, not " << showTypeInfo(data.vertex_map); + return false; + } + + return true; +} + void convertVertexMap(InputData& data, bool in_world_frame) { if (data.points_in_world_frame == in_world_frame) { return; } + Eigen::Isometry3f transform = data.getSensorPose().cast(); // world_T_sensor if (!in_world_frame) { transform = transform.inverse(); // Instead get sensor_T_world } + for (int r = 0; r < data.vertex_map.rows; ++r) { for (int c = 0; c < data.vertex_map.cols; ++c) { - cv::Vec3f& point = data.vertex_map.at(r, c); + auto& point = data.vertex_map.at(r, c); Eigen::Vector3f point_eigen(point[0], point[1], point[2]); point_eigen = transform * point_eigen; point[0] = point_eigen.x(); @@ -206,6 +180,7 @@ void convertVertexMap(InputData& data, bool in_world_frame) { point[2] = point_eigen.z(); } } + data.points_in_world_frame = in_world_frame; } diff --git a/src/input/sensor_input_packet.cpp b/src/input/sensor_input_packet.cpp index 17fb3a43..5fc39073 100644 --- a/src/input/sensor_input_packet.cpp +++ b/src/input/sensor_input_packet.cpp @@ -38,8 +38,6 @@ #include -#include "hydra/common/global_info.h" - namespace hydra { namespace { @@ -82,6 +80,7 @@ bool ImageInputPacket::fillInputDataImpl(InputData& msg) const { msg.depth_image = depth; msg.label_image = labels; + msg.instance_image = instances; // TODO(nathan) think about better copy msg.label_features = label_features; @@ -91,6 +90,12 @@ bool ImageInputPacket::fillInputDataImpl(InputData& msg) const { return false; } + if (!msg.instance_image.empty() && !sizesMatch(msg.depth_image, msg.instance_image)) { + LOG(ERROR) << "Instance dimensions " << showImageDim(msg.label_image) + << " do not match depth dimensions " << showImageDim(msg.instance_image); + return false; + } + if (!msg.color_image.empty() && !sizesMatch(msg.depth_image, msg.color_image)) { LOG(ERROR) << "Color dimensions " << showImageDim(msg.color_image) << " do not match depth dimensions " << showImageDim(msg.depth_image); @@ -113,6 +118,7 @@ bool CloudInputPacket::fillInputDataImpl(InputData& msg) const { msg.points_in_world_frame = in_world_frame; msg.color_image = colors; msg.label_image = labels; + msg.instance_image = instances; if (!msg.label_image.empty() && !sizesMatch(msg.vertex_map, msg.label_image)) { LOG(ERROR) << "Label dimensions " << showImageDim(msg.label_image) @@ -121,6 +127,13 @@ bool CloudInputPacket::fillInputDataImpl(InputData& msg) const { return false; } + if (!msg.instance_image.empty() && !sizesMatch(msg.vertex_map, msg.instance_image)) { + LOG(ERROR) << "Instance dimensions " << showImageDim(msg.instance_image) + << " do not match pointcloud dimensions " + << showImageDim(msg.vertex_map); + return false; + } + if (!msg.color_image.empty() && !sizesMatch(msg.vertex_map, msg.color_image)) { LOG(ERROR) << "Color dimensions " << showImageDim(msg.color_image) << " do not match pointcloud dimensions " diff --git a/tests/input/test_sensor.cpp b/tests/input/test_sensor.cpp index ccf541b8..aa7c2296 100644 --- a/tests/input/test_sensor.cpp +++ b/tests/input/test_sensor.cpp @@ -35,9 +35,6 @@ #include #include -#include -#include - #include "hydra_test/resources.h" namespace hydra {