Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

use xyz instead of xyzrgb in ClusterPointIndicesDecomposer #2839

Merged
merged 2 commits into from
May 14, 2024
Merged

use xyz instead of xyzrgb in ClusterPointIndicesDecomposer #2839

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7dc9633
use xyz instead of xyzrgb
knorth55 May 11, 2024
fb8bcca
Merge branch 'master' into fix-decomposer
iory May 14, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions jsk_pcl_ros/include/jsk_pcl_ros/cluster_point_indices_decomposer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,12 +111,12 @@ namespace jsk_pcl_ros
boost::mutex mutex_;

void addToDebugPointCloud
(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
(const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
size_t i,
pcl::PointCloud<pcl::PointXYZRGB>& debug_output);

virtual bool computeCenterAndBoundingBox
(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
(const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
const std_msgs::Header header,
const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients,
Expand All @@ -125,8 +125,8 @@ namespace jsk_pcl_ros
bool& publish_tf);

virtual bool transformPointCloudToAlignWithPlane(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud_transformed,
const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud_transformed,
const Eigen::Vector4f center,
const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients,
Expand Down
34 changes: 16 additions & 18 deletions jsk_pcl_ros/src/cluster_point_indices_decomposer_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -341,8 +341,8 @@ namespace jsk_pcl_ros
}

bool ClusterPointIndicesDecomposer::transformPointCloudToAlignWithPlane(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud_transformed,
const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud_transformed,
const Eigen::Vector4f center,
const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients,
Expand Down Expand Up @@ -386,9 +386,9 @@ namespace jsk_pcl_ros
Eigen::Matrix3f m = Eigen::Matrix3f::Identity() * rot;
if (use_pca_) {
// first project points to the plane
pcl::PointCloud<pcl::PointXYZRGB>::Ptr projected_cloud
(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ProjectInliers<pcl::PointXYZRGB> proj;
pcl::PointCloud<pcl::PointXYZ>::Ptr projected_cloud
(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
pcl::ModelCoefficients::Ptr
plane_coefficients (new pcl::ModelCoefficients);
Expand All @@ -398,7 +398,7 @@ namespace jsk_pcl_ros
proj.setInputCloud(segmented_cloud);
proj.filter(*projected_cloud);
if (projected_cloud->points.size() >= 3) {
pcl::PCA<pcl::PointXYZRGB> pca;
pcl::PCA<pcl::PointXYZ> pca;
pca.setInputCloud(projected_cloud);
Eigen::Matrix3f eigen = pca.getEigenVectors();
m.col(0) = eigen.col(0);
Expand Down Expand Up @@ -433,7 +433,7 @@ namespace jsk_pcl_ros
}

bool ClusterPointIndicesDecomposer::computeCenterAndBoundingBox
(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
(const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
const std_msgs::Header header,
const jsk_recognition_msgs::PolygonArrayConstPtr& planes,
const jsk_recognition_msgs::ModelCoefficientsArrayConstPtr& coefficients,
Expand All @@ -450,8 +450,8 @@ namespace jsk_pcl_ros

bool is_center_valid = false;
Eigen::Vector4f center;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
segmented_cloud_transformed (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr
segmented_cloud_transformed (new pcl::PointCloud<pcl::PointXYZ>);
segmented_cloud_transformed->is_dense = segmented_cloud->is_dense;
// align boxes if possible
Eigen::Matrix4f m4 = Eigen::Matrix4f::Identity();
Expand Down Expand Up @@ -489,7 +489,7 @@ namespace jsk_pcl_ros
is_center_valid = pcl::compute3DCentroid(*segmented_cloud, center) != 0;

// compute planes from target frame
pcl::PointXYZRGB min_pt, max_pt;
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(*segmented_cloud, min_pt, max_pt);
//
pcl_msgs::ModelCoefficients coef_by_frame;
Expand Down Expand Up @@ -570,7 +570,7 @@ namespace jsk_pcl_ros

// create a bounding box
Eigen::Vector4f minpt, maxpt;
pcl::getMinMax3D<pcl::PointXYZRGB>(*segmented_cloud_transformed, minpt, maxpt);
pcl::getMinMax3D<pcl::PointXYZ>(*segmented_cloud_transformed, minpt, maxpt);

double xwidth = maxpt[0] - minpt[0];
double ywidth = maxpt[1] - minpt[1];
Expand Down Expand Up @@ -619,7 +619,7 @@ namespace jsk_pcl_ros
}

void ClusterPointIndicesDecomposer::addToDebugPointCloud
(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud,
(const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
size_t i,
pcl::PointCloud<pcl::PointXYZRGB>& debug_output)
{
Expand Down Expand Up @@ -672,11 +672,9 @@ namespace jsk_pcl_ros
allocatePublishers(indices_input->cluster_indices.size());
}
publishNegativeIndices(input, indices_input);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extract;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *cloud);
pcl::fromROSMsg(*input, *cloud_xyz);
cluster_counter_.add(indices_input->cluster_indices.size());

std::vector<pcl::IndicesPtr> converted_indices;
Expand All @@ -702,7 +700,7 @@ namespace jsk_pcl_ros
}

std::vector<size_t> argsort;
sortIndicesOrder(cloud_xyz, converted_indices, &argsort);
sortIndicesOrder(cloud, converted_indices, &argsort);
extract.setInputCloud(cloud);

// point cloud from camera not laser
Expand All @@ -719,7 +717,7 @@ namespace jsk_pcl_ros
out_cluster_indices.header = input->header;
for (size_t i = 0; i < argsort.size(); i++)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud (new pcl::PointCloud<pcl::PointXYZ>);
segmented_cloud->is_dense = cloud->is_dense;

pcl_msgs::PointIndices out_indices_msg;
Expand Down
Loading