StepTurtle commented 9 months ago

Hello,

When I tried to create map with LIO-SAM, I got an error because of Point type in dynamic_object_removal. Some fields are being disregarded by the algorithm, and the required 'ring' field for LIO-SAM is not present in the output point cloud. To solve it I changed PointXYZI type with custom point type PointXYZIRT (adding ring and time field). It looks more generic now and maybe you can thing changing point type. These are the changes I made:

// Add crop_box algorithm's template header implementation
#include <pcl/filters/impl/crop_box.hpp>

// Add new Point type as a struct
struct PointXYZIRT {
    PCL_ADD_POINT4D

    PCL_ADD_INTENSITY
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

// Register to point type
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT,
                                   (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
                                           (uint16_t, ring, ring)(float, time, time)
)

// Also change PointXYZI parts with PointXYZIRT
// ...

davutcanakbas commented 9 months ago

Hi,

We understand your point, but it's worth noting that not every SLAM algorithm necessitates the use of the XYZIRT point type. The XYZI format has been a standard choice for general usage, which is why we have kept it as the default format in the repository.

thanks for your suggestion and the modifications you've made to accommodate the needs of specific algorithms. While we maintain the broader compatibility of XYZI, your proposal will still be taken into consideration for its relevance to particular use cases.

Your feedback is valuable, and we're grateful for your contribution to our collaborative work.

LinusVanPe1t commented 3 weeks ago

Hello, I also applied dynamic object removal to LIO-SAM.

I received a warning message stating: "Point cloud timestamp not available, deskew function disabled, system will drift significantly!" Upon analyzing this, I found that dynamic object removal only includes PointXYZI and does not have PointXYZIRT as you mentioned.

So, I tried to add PointXYZIRT. However, in my case, the ring and time fields are not added to the final output of the dynamic object removal, /output/pointcloud.

This is the code I modified.

this is hpp file.

ifndef DYNAMIC_OBJECT_REMOVAL_HPP

define DYNAMIC_OBJECT_REMOVAL_HPP

// ROS2 headers

include <rclcpp/rclcpp.hpp>

include "sensor_msgs/msg/point_cloud2.hpp"

include "autoware_auto_perception_msgs/msg/detected_objects.hpp"

include "geometry_msgs/msg/pose.hpp"

// Message filters for synchronizing topics

include <message_filters/subscriber.h>

include <message_filters/sync_policies/approximate_time.h>

include <message_filters/synchronizer.h>

// PCL headers for point cloud manipulation

include "pcl/filters/crop_box.h"

include "pcl/point_types.h"

include "pcl_conversions/pcl_conversions.h"

// Eigen for linear algebra

include <Eigen/Geometry>

// Add crop_box algorithm's template header implementation

include <pcl/filters/impl/crop_box.hpp>

// Add new Point type as a struct struct PointXYZIRT { PCL_ADD_POINT4D;

PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

} EIGEN_ALIGN16;

// Register to point type POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) (uint16_t, ring, ring)(float, time, time) )

// Change PointCloud type using PointCloudXYZIRT = pcl::PointCloud;

class DynamicObjectRemoval : public rclcpp::Node { public: DynamicObjectRemoval();

// Publisher for the processed point cloud rclcpp::Publisher::SharedPtr pclpublisher;

// Callback for synchronized messages void callback(const std::shared_ptr& pcl_msg, const std::shared_ptr& obj_msg);

// Function to remove objects using CropBox void objectRemoveCropBox(PointCloudXYZIRT::Ptr crop_cloud, const Eigen::Vector4f min_point, const Eigen::Vector4f max_point, const Eigen::Vector3f translation, double orientation_yaw);

private: // Subscribers for point cloud and detected objects message_filters::Subscriber pclsub; message_filters::Subscriber objsub;

// Synchronizer for the above subscribers std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime< sensor_msgs::msg::PointCloud2, autoware_auto_perceptionmsgs::msg::DetectedObjects>>> sync; };

endif // DYNAMIC_OBJECT_REMOVAL_HPP

and this is cpp file '

include "dynamic_object_removal.hpp"

// Constructor DynamicObjectRemoval::DynamicObjectRemoval() : Node("dynamic_object_removal_node") { pclpublisher = this->create_publisher("output/pointcloud", 10);

// Subscribe to topics pclsub.subscribe(this, "/sensing/lidar/concatenated/pointcloud", rmw_qos_profile_sensor_data); objsub.subscribe(this, "/perception/object_recognition/detection/centerpoint/objects", rmw_qos_profile_sensor_data);

// Synchronize the incoming messages typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::PointCloud2, autoware_auto_perceptionmsgs::msg::DetectedObjects> MySyncPolicy; sync = std::make_shared<message_filters::Synchronizer>(MySyncPolicy(10), pclsub, objsub); sync_->registerCallback(&DynamicObjectRemoval::callback, this); }

// Callback function void DynamicObjectRemoval::callback( const std::shared_ptr& pcl_msg, const std::shared_ptr& obj_msg) { // Convert ROS2 PointCloud2 to PCL PointCloud PointCloudXYZI::Ptr cloud(new PointCloudXYZI); pcl::fromROSMsg(pcl_msg, cloud);

// Loop through detected objects for (const auto& detected_obj : obj_msg->objects) { const auto& object_pose = detected_obj.kinematics.pose_with_covariance.pose; const auto& object_dimension = detected_obj.shape.dimensions;

Eigen::Quaterniond quat(object_pose.orientation.w, object_pose.orientation.x, object_pose.orientation.y, object_pose.orientation.z);
Eigen::Affine3d detected_obj_pose = Eigen::Translation3d(object_pose.position.x, object_pose.position.y, object_pose.position.z) * quat;

// Extract Euler angles
Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
double yaw = euler[0];

// Define object dimensions for cropping
Eigen::Vector4f min_point, max_point;
min_point << -abs(object_dimension.x) / 2 - 0.25, -abs(object_dimension.y) / 2 - 0.25, -abs(object_dimension.z) / 2 - 0.1, 1.0;
max_point << abs(object_dimension.x) / 2 + 0.25, abs(object_dimension.y) / 2 + 0.25, abs(object_dimension.z) / 2 + 0.25, 1.0;

// Remove object using objectRemoveCropBox function
objectRemoveCropBox(cloud, min_point, max_point, detected_obj_pose.translation().cast<float>(), yaw);

}

// Convert back to ROS2 PointCloud2 and publish sensor_msgs::msg::PointCloud2 output_msg; pcl::toROSMsg(*cloud, output_msg); pclpublisher->publish(output_msg); }

// Function to remove objects using CropBox void DynamicObjectRemoval::objectRemoveCropBox(PointCloudXYZI::Ptr crop_cloud, const Eigen::Vector4f min_point, const Eigen::Vector4f max_point, const Eigen::Vector3f translation, double orientation_yaw) { PointCloudXYZI xyz_filtered_cloud; pcl::CropBox ObjectRemoveCrop;

ObjectRemoveCrop.setNegative(true); ObjectRemoveCrop.setInputCloud(crop_cloud); ObjectRemoveCrop.setMin(min_point); ObjectRemoveCrop.setMax(max_point); ObjectRemoveCrop.setRotation(Eigen::Vector3f(0.0, 0.0, orientation_yaw)); ObjectRemoveCrop.setTranslation(translation); ObjectRemoveCrop.filter(*crop_cloud); }

// Main function int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; }'

Do you have any other information that could help? If there is a problem, I will delete it. Thank u

davutcanakbas commented 3 weeks ago

I have pushed a fix for the issue. Please pull the latest changes and test if the issue is resolved.