TEST(sensor_msgs, PointCloud2Iterator) { // Create a dummy PointCloud2 size_t n_points = 4; sensor_msgs::msg::PointCloud2 cloud_msg_1, cloud_msg_2; cloud_msg_1.height = static_cast<uint32_t>(n_points); cloud_msg_1.width = 1; sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1); modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); cloud_msg_2 = cloud_msg_1; // Fill 1 by hand float point_data_raw[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0}; std::vector<float> point_data(point_data_raw, point_data_raw + 3 * n_points); // colors in RGB order uint8_t color_data_raw[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120}; std::vector<uint8_t> color_data(color_data_raw, color_data_raw + 3 * n_points); float * data = reinterpret_cast<float *>(&cloud_msg_1.data.front()); for (size_t n = 0, i = 0; n < n_points; ++n) { for (; i < 3 * (n + 1); ++i) { *(data++) = point_data[i]; } // Add an extra float of padding ++data; uint8_t * bgr = reinterpret_cast<uint8_t *>(data++); // add the colors in order BGRA like PCL size_t j_max = 2; for (size_t j = 0; j <= j_max; ++j) { *(bgr++) = color_data[3 * n + (j_max - j)]; } // Add 3 extra floats of padding data += 3; } // Fill 2 using an iterator sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_2, "x"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg_2, "r"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg_2, "g"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg_2, "b"); for (size_t i = 0; i < n_points; ++i, ++iter_x, ++iter_r, ++iter_g, ++iter_b) { for (size_t j = 0; j < 3; ++j) { iter_x[j] = point_data[j + 3 * i]; } *iter_r = color_data[3 * i]; *iter_g = color_data[3 * i + 1]; *iter_b = color_data[3 * i + 2]; } // Check the values using an iterator sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_x(cloud_msg_1, "x"), iter_const_2_x( cloud_msg_2, "x"); sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_y(cloud_msg_1, "y"), iter_const_2_y( cloud_msg_2, "y"); sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_z(cloud_msg_1, "z"), iter_const_2_z( cloud_msg_2, "z"); sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_r(cloud_msg_1, "r"), iter_const_2_r( cloud_msg_2, "r"); sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_g(cloud_msg_1, "g"), iter_const_2_g( cloud_msg_2, "g"); sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_b(cloud_msg_1, "b"), iter_const_2_b( cloud_msg_2, "b"); size_t i = 0; for (; iter_const_1_x != iter_const_1_x.end(); ++i, ++iter_const_1_x, ++iter_const_2_x, ++iter_const_1_y, ++iter_const_2_y, ++iter_const_1_z, ++iter_const_2_z, ++iter_const_1_r, ++iter_const_1_g, ++iter_const_1_b) { EXPECT_EQ(*iter_const_1_x, *iter_const_2_x); EXPECT_EQ(*iter_const_1_x, point_data[0 + 3 * i]); EXPECT_EQ(*iter_const_1_y, *iter_const_2_y); EXPECT_EQ(*iter_const_1_y, point_data[1 + 3 * i]); EXPECT_EQ(*iter_const_1_z, *iter_const_2_z); EXPECT_EQ(*iter_const_1_z, point_data[2 + 3 * i]); EXPECT_EQ(*iter_const_1_r, *iter_const_2_r); EXPECT_EQ(*iter_const_1_r, color_data[3 * i + 0]); EXPECT_EQ(*iter_const_1_g, *iter_const_2_g); EXPECT_EQ(*iter_const_1_g, color_data[3 * i + 1]); EXPECT_EQ(*iter_const_1_b, *iter_const_2_b); EXPECT_EQ(*iter_const_1_b, color_data[3 * i + 2]); // This is to test the different operators ++iter_const_2_r; iter_const_2_g += 1; iter_const_2_b = iter_const_2_b + 1; } EXPECT_EQ(i, n_points); }
/** fill the point cloud with the 3D points */ void LinemodPointcloud::fill(const std::vector<cv::Vec3f> & pts, const cv::Vec3b &color) { int size_old = modifier->size(); modifier->resize(size_old + pts.size()); sensor_msgs::PointCloud2Iterator<float> iter_x(pc_msg, "x"); sensor_msgs::PointCloud2Iterator<float> iter_y(pc_msg, "y"); sensor_msgs::PointCloud2Iterator<float> iter_z(pc_msg, "z"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(pc_msg, "r"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(pc_msg, "g"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(pc_msg, "b"); iter_x += size_old; iter_y += size_old; iter_z += size_old; iter_r += size_old; iter_g += size_old; iter_b += size_old; std::vector<cv::Vec3f>::const_iterator it_data = pts.begin(); for(; it_data != pts.end(); ++it_data, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) { *iter_x = (*it_data)(0); *iter_y = (*it_data)(1); *iter_z = (*it_data)(2); *iter_r = color(0); *iter_g = color(1); *iter_b = color(2); } }
void convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg) { // Use correct principal point from calibration float center_x = cameraModel.cx(); float center_y = cameraModel.cy(); // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) double unit_scaling = DepthTraits<T>::toMeters( T(1) ); float constant_x = unit_scaling / cameraModel.fx(); float constant_y = unit_scaling / cameraModel.fy(); float bad_point = std::numeric_limits<float>::quiet_NaN(); sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x"); sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y"); sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z"); const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]); int row_step = depth_msg->step / sizeof(T); for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step) { for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) { T depth = depth_row[u]; // Missing points denoted by NaNs if (!DepthTraits<T>::valid(depth)) { *iter_x = *iter_y = *iter_z = bad_point; continue; } // 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); } } }
void point_containment_filter::ShapeMask::maskContainment(const sensor_msgs::PointCloud2& data_in, const Eigen::Vector3d& sensor_origin, const double min_sensor_dist, const double max_sensor_dist, std::vector<int>& mask) { boost::mutex::scoped_lock _(shapes_lock_); const unsigned int np = data_in.data.size() / data_in.point_step; mask.resize(np); if (bodies_.empty()) std::fill(mask.begin(), mask.end(), (int)OUTSIDE); else { Eigen::Isometry3d tmp; bspheres_.resize(bodies_.size()); std::size_t j = 0; for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end(); ++it) { if (!transform_callback_(it->handle, tmp)) { if (!it->body) ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape with handle " << it->handle << " without a body"); else ROS_ERROR_STREAM_NAMED("shape_mask", "Missing transform for shape " << it->body->getType() << " with handle " << it->handle); } else { it->body->setPose(tmp); it->body->computeBoundingSphere(bspheres_[j++]); } } // compute a sphere that bounds the entire robot bodies::BoundingSphere bound; bodies::mergeBoundingSpheres(bspheres_, bound); const double radius_squared = bound.radius * bound.radius; // we now decide which points we keep sensor_msgs::PointCloud2ConstIterator<float> iter_x(data_in, "x"); sensor_msgs::PointCloud2ConstIterator<float> iter_y(data_in, "y"); sensor_msgs::PointCloud2ConstIterator<float> iter_z(data_in, "z"); // Cloud iterators are not incremented in the for loop, because of the pragma // Comment out below parallelization as it can result in very high CPU consumption //#pragma omp parallel for schedule(dynamic) for (int i = 0; i < (int)np; ++i) { Eigen::Vector3d pt = Eigen::Vector3d(*(iter_x + i), *(iter_y + i), *(iter_z + i)); double d = pt.norm(); int out = OUTSIDE; if (d < min_sensor_dist || d > max_sensor_dist) out = CLIP; else if ((bound.center - pt).squaredNorm() < radius_squared) for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it) if (it->body->containsPoint(pt)) out = INSIDE; mask[i] = out; } } }
// Fill depth information bool GazeboRosOpenniKinect::FillPointCloudHelper( sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void* data_arg) { sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); // convert to flat array shape, we need to reconvert later pcd_modifier.resize(rows_arg*cols_arg); point_cloud_msg.is_dense = true; sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x"); sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y"); sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z"); sensor_msgs::PointCloud2Iterator<uint8_t> iter_rgb(point_cloud_msg_, "rgb"); float* toCopyFrom = (float*)data_arg; int index = 0; double hfov = this->parentSensor->GetDepthCamera()->GetHFOV().Radian(); double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); // convert depth to point cloud for (uint32_t j=0; j<rows_arg; j++) { double pAngle; if (rows_arg>1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); else pAngle = 0.0; for (uint32_t i=0; i<cols_arg; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) { double yAngle; if (cols_arg>1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); else yAngle = 0.0; double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip(); if(depth > this->point_cloud_cutoff_ && depth < this->point_cloud_cutoff_max_) { // in optical frame // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in // to urdf, where the *_optical_frame should have above relative // rotation from the physical camera *_frame *iter_x = depth * tan(yAngle); *iter_y = depth * tan(pAngle); *iter_z = depth; } else //point in the unseeable range { *iter_x = *iter_y = *iter_z = std::numeric_limits<float>::quiet_NaN (); point_cloud_msg.is_dense = false; } // put image color data for each point uint8_t* image_src = (uint8_t*)(&(this->image_msg_.data[0])); if (this->image_msg_.data.size() == rows_arg*cols_arg*3) { // color iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; } else if (this->image_msg_.data.size() == rows_arg*cols_arg) { // mono (or bayer? @todo; fix for bayer) iter_rgb[0] = image_src[i+j*cols_arg]; iter_rgb[1] = image_src[i+j*cols_arg]; iter_rgb[2] = image_src[i+j*cols_arg]; } else { // no image iter_rgb[0] = 0; iter_rgb[1] = 0; iter_rgb[2] = 0; } } } // reconvert to original height and width after the flat reshape point_cloud_msg.height = rows_arg; point_cloud_msg.width = cols_arg; point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width; return true; }