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

Jump to bottom

Cropping pointcloud before publishing using min/max XYZ values #766

Open
wants to merge 3 commits into
base: noetic
Choose a base branch
from
Open

Cropping pointcloud before publishing using min/max XYZ values #766

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9b8100c
added a min and max distance threshold for pointcloud
Jun 24, 2022
53e44a6
pointcloud_xyzrbg takes max/min x,y,z points for cropping the pointcloud
Jul 30, 2022
0a648fe
added xyz crop test on all pointcloud classes
Dec 6, 2022
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
42 changes: 37 additions & 5 deletions depth_image_proc/src/nodelets/point_cloud_xyz_radial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,15 @@ namespace depth_image_proc {

std::vector<double> D_;
boost::array<double, 9> K_;


// range crop
double max_x ;
double max_y ;
double max_z ;
double min_x ;
double min_y ;
double min_z ;

int width_;
int height_;

Expand Down Expand Up @@ -130,6 +138,14 @@ namespace depth_image_proc {
// Read parameters
private_nh.param("queue_size", queue_size_, 5);

// min/max ranges for crop
private_nh.param("max_x", max_x, std::numeric_limits<double>::infinity());
private_nh.param("max_y", max_y, std::numeric_limits<double>::infinity());
private_nh.param("max_z", max_z, std::numeric_limits<double>::infinity());
private_nh.param("min_x", min_x, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_y", min_y, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_z", min_z, -1*std::numeric_limits<double>::infinity());

// Monitor whether anyone is subscribed to the output
ros::SubscriberStatusCallback connect_cb =
boost::bind(&PointCloudXyzRadialNodelet::connectCb, this);
Expand Down Expand Up @@ -221,10 +237,26 @@ namespace depth_image_proc {
continue;
}
const cv::Vec3f &cvPoint = binned.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
// Fill in XYZ
*iter_x = cvPoint(0);
*iter_y = cvPoint(1);
*iter_z = cvPoint(2);
// test if the point is in the XYZ range
if (
( cvPoint(0) ) < max_x &&
( cvPoint(1) ) < max_y &&
( cvPoint(2) ) < max_z &&
( cvPoint(0) ) > min_x &&
( cvPoint(1) ) > min_y &&
( cvPoint(2) ) > min_z
)
{
// Fill in XYZ
*iter_x = cvPoint(0);
*iter_y = cvPoint(1);
*iter_z = cvPoint(2);
}
else
{
*iter_x = *iter_y = *iter_z = bad_point;
continue;
}
}
}
}
Expand Down
39 changes: 35 additions & 4 deletions depth_image_proc/src/nodelets/point_cloud_xyzi.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,14 @@ class PointCloudXyziNodelet : public nodelet::Nodelet

image_geometry::PinholeCameraModel model_;

// range crop
double max_x ;
double max_y ;
double max_z ;
double min_x ;
double min_y ;
double min_z ;

virtual void onInit();

void connectCb();
Expand Down Expand Up @@ -102,6 +110,14 @@ void PointCloudXyziNodelet::onInit()
int queue_size;
private_nh.param("queue_size", queue_size, 5);

// min/max ranges for crop
private_nh.param("max_x", max_x, std::numeric_limits<double>::infinity());
private_nh.param("max_y", max_y, std::numeric_limits<double>::infinity());
private_nh.param("max_z", max_z, std::numeric_limits<double>::infinity());
private_nh.param("min_x", min_x, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_y", min_y, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_z", min_z, -1*std::numeric_limits<double>::infinity());

// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_intensity_, sub_info_) );
sync_->registerCallback(boost::bind(&PointCloudXyziNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
Expand Down Expand Up @@ -307,10 +323,25 @@ void PointCloudXyziNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
}
else
{
// Fill in XYZ
*iter_x = (u - center_x) * depth * constant_x;
*iter_y = (v - center_y) * depth * constant_y;
*iter_z = DepthTraits<T>::toMeters(depth);
// test if the point is in the XYZ range
if (
( (u - center_x) * depth * constant_x ) < max_x &&
( (v - center_y) * depth * constant_y ) < max_y &&
( DepthTraits<T>::toMeters(depth) ) < max_z &&
( (u - center_x) * depth * constant_x ) > min_x &&
( (v - center_y) * depth * constant_y ) > min_y &&
( DepthTraits<T>::toMeters(depth) ) > min_z
)
{
// Fill in XYZ
*iter_x = (u - center_x) * depth * constant_x;
*iter_y = (v - center_y) * depth * constant_y;
*iter_z = DepthTraits<T>::toMeters(depth);
}
else
{
*iter_x = *iter_y = *iter_z = bad_point;
}
}

// Fill in intensity
Expand Down
40 changes: 36 additions & 4 deletions depth_image_proc/src/nodelets/point_cloud_xyzi_radial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,14 @@ namespace depth_image_proc {
int width_;
int height_;

// range crop
double max_x ;
double max_y ;
double max_z ;
double min_x ;
double min_y ;
double min_z ;

cv::Mat transform_;

virtual void onInit();
Expand Down Expand Up @@ -154,6 +162,14 @@ namespace depth_image_proc {
// Read parameters
private_nh.param("queue_size", queue_size_, 5);

// min/max ranges for crop
private_nh.param("max_x", max_x, std::numeric_limits<double>::infinity());
private_nh.param("max_y", max_y, std::numeric_limits<double>::infinity());
private_nh.param("max_z", max_z, std::numeric_limits<double>::infinity());
private_nh.param("min_x", min_x, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_y", min_y, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_z", min_z, -1*std::numeric_limits<double>::infinity());

// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_intensity_, sub_info_) );
sync_->registerCallback(boost::bind(&PointCloudXyziRadialNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
Expand Down Expand Up @@ -286,10 +302,26 @@ namespace depth_image_proc {
continue;
}
const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
// Fill in XYZ
*iter_x = cvPoint(0);
*iter_y = cvPoint(1);
*iter_z = cvPoint(2);
// test if the point is in the XYZ range
if (
( cvPoint(0) ) < max_x &&
( cvPoint(1) ) < max_y &&
( cvPoint(2) ) < max_z &&
( cvPoint(0) ) > min_x &&
( cvPoint(1) ) > min_y &&
( cvPoint(2) ) > min_z
)
{
// Fill in XYZ
*iter_x = cvPoint(0);
*iter_y = cvPoint(1);
*iter_z = cvPoint(2);
}
else
{
*iter_x = *iter_y = *iter_z = bad_point;
continue;
}
}
}
}
Expand Down
49 changes: 42 additions & 7 deletions depth_image_proc/src/nodelets/point_cloud_xyzrgb.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,15 @@ class PointCloudXyzrgbNodelet : public nodelet::Nodelet

image_geometry::PinholeCameraModel model_;

// range crop
double max_x ;
double max_y ;
double max_z ;
double min_x ;
double min_y ;
double min_z ;


virtual void onInit();

void connectCb();
Expand All @@ -102,7 +111,15 @@ void PointCloudXyzrgbNodelet::onInit()
ros::NodeHandle depth_nh(nh, "depth_registered");
rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) );
depth_it_.reset( new image_transport::ImageTransport(depth_nh) );


// min/max ranges for crop
private_nh.param("max_x", max_x, std::numeric_limits<double>::infinity());
private_nh.param("max_y", max_y, std::numeric_limits<double>::infinity());
private_nh.param("max_z", max_z, std::numeric_limits<double>::infinity());
private_nh.param("min_x", min_x, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_y", min_y, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_z", min_z, -1*std::numeric_limits<double>::infinity());

// Read parameters
int queue_size;
private_nh.param("queue_size", queue_size, 5);
Expand All @@ -116,7 +133,7 @@ void PointCloudXyzrgbNodelet::onInit()
exact_sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
else
{
{
sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
Expand Down Expand Up @@ -324,25 +341,43 @@ void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_ms
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud_msg, "b");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_a(*cloud_msg, "a");



for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, rgb += rgb_skip)
{
for (int u = 0; u < int(cloud_msg->width); ++u, rgb += color_step, ++iter_x, ++iter_y, ++iter_z, ++iter_a, ++iter_r, ++iter_g, ++iter_b)
{
T depth = depth_row[u];

// Check for invalid measurements
if (!DepthTraits<T>::valid(depth))
{
*iter_x = *iter_y = *iter_z = bad_point;
}
else
{
// Fill in XYZ
*iter_x = (u - center_x) * depth * constant_x;
*iter_y = (v - center_y) * depth * constant_y;
*iter_z = DepthTraits<T>::toMeters(depth);
// test if the point is in the XYZ range
if (
( (u - center_x) * depth * constant_x ) < max_x &&
( (v - center_y) * depth * constant_y ) < max_y &&
( DepthTraits<T>::toMeters(depth) ) < max_z &&
( (u - center_x) * depth * constant_x ) > min_x &&
( (v - center_y) * depth * constant_y ) > min_y &&
( DepthTraits<T>::toMeters(depth) ) > min_z
)
{
// Fill in XYZ
*iter_x = (u - center_x) * depth * constant_x;
*iter_y = (v - center_y) * depth * constant_y;
*iter_z = DepthTraits<T>::toMeters(depth);
}
else
{
*iter_x = *iter_y = *iter_z = bad_point;
}
}


// Fill in color
*iter_a = 255;
*iter_r = rgb[red_offset];
Expand Down
43 changes: 39 additions & 4 deletions depth_image_proc/src/nodelets/point_cloud_xyzrgb_radial.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,14 @@ namespace depth_image_proc {
int width_;
int height_;

// range crop
double max_x ;
double max_y ;
double max_z ;
double min_x ;
double min_y ;
double min_z ;

cv::Mat transform_;
image_geometry::PinholeCameraModel model_;

Expand Down Expand Up @@ -163,6 +171,15 @@ namespace depth_image_proc {
bool use_exact_sync;
private_nh.param("exact_sync", use_exact_sync, false);

// min/max ranges for crop
private_nh.param("max_x", max_x, std::numeric_limits<double>::infinity());
private_nh.param("max_y", max_y, std::numeric_limits<double>::infinity());
private_nh.param("max_z", max_z, std::numeric_limits<double>::infinity());
private_nh.param("min_x", min_x, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_y", min_y, -1*std::numeric_limits<double>::infinity());
private_nh.param("min_z", min_z, -1*std::numeric_limits<double>::infinity());


// Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
if(use_exact_sync) {
exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size_), sub_depth_, sub_rgb_, sub_info_) );
Expand Down Expand Up @@ -385,17 +402,35 @@ namespace depth_image_proc {
{
T depth = depth_row[u];


// Missing points denoted by NaNs
if (!DepthTraits<T>::valid(depth))
{
*iter_x = *iter_y = *iter_z = bad_point;
continue;
}
const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
// Fill in XYZ
*iter_x = cvPoint(0);
*iter_y = cvPoint(1);
*iter_z = cvPoint(2);
// test if the point is in the XYZ range
if (
( cvPoint(0) ) < max_x &&
( cvPoint(1) ) < max_y &&
( cvPoint(2) ) < max_z &&
( cvPoint(0) ) > min_x &&
( cvPoint(1) ) > min_y &&
( cvPoint(2) ) > min_z
)
{
// Fill in XYZ
*iter_x = cvPoint(0);
*iter_y = cvPoint(1);
*iter_z = cvPoint(2);
}
else
{
*iter_x = *iter_y = *iter_z = bad_point;
continue;
}

}
}
}
Expand Down