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.

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

TODO: <Modify index type for pointCloud> #4198

Draft
wants to merge 14 commits into
base: master
Choose a base branch
from
8 changes: 4 additions & 4 deletions 2d/include/pcl/2d/impl/edge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ Edge<PointInT, PointOutT>::detectEdgeSobel(pcl::PointCloud<PointOutT>& output)
output.height = height;
output.width = width;

for (std::size_t i = 0; i < output.size(); ++i) {
for (index_t i = 0; i < output.size(); ++i) {
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
Expand Down Expand Up @@ -110,7 +110,7 @@ Edge<PointInT, PointOutT>::sobelMagnitudeDirection(
output.height = height;
output.width = width;

for (std::size_t i = 0; i < output.size(); ++i) {
for (index_t i = 0; i < output.size(); ++i) {
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
Expand Down Expand Up @@ -148,7 +148,7 @@ Edge<PointInT, PointOutT>::detectEdgePrewitt(pcl::PointCloud<PointOutT>& output)
output.height = height;
output.width = width;

for (std::size_t i = 0; i < output.size(); ++i) {
for (index_t i = 0; i < output.size(); ++i) {
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
Expand Down Expand Up @@ -363,7 +363,7 @@ Edge<PointInT, PointOutT>::detectEdgeCanny(pcl::PointCloud<PointOutT>& output)
}

// Final thresholding
for (std::size_t i = 0; i < input_->size(); ++i) {
for (index_t i = 0; i < input_->size(); ++i) {
if ((*maxima)[i].intensity == std::numeric_limits<float>::max())
output[i].magnitude = 255;
else
Expand Down
8 changes: 4 additions & 4 deletions 2d/include/pcl/2d/impl/kernel.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,8 +113,8 @@ kernel<PointT>::gaussianKernel(pcl::PointCloud<PointT>& kernel)
}

// Normalizing the kernel
for (std::size_t i = 0; i < kernel.size(); ++i)
kernel[i].intensity /= sum;
for (auto& k : kernel)
k.intensity /= sum;
}

template <typename PointT>
Expand All @@ -139,8 +139,8 @@ kernel<PointT>::loGKernel(pcl::PointCloud<PointT>& kernel)
}

// Normalizing the kernel
for (std::size_t i = 0; i < kernel.size(); ++i)
kernel[i].intensity /= sum;
for (auto& k : kernel)
k.intensity /= sum;
}

template <typename PointT>
Expand Down
4 changes: 2 additions & 2 deletions 2d/include/pcl/2d/impl/morphology.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -353,8 +353,8 @@ Morphology<PointT>::structuringElementRectangle(pcl::PointCloud<PointT>& kernel,
kernel.height = height;
kernel.width = width;
kernel.resize(height * width);
for (std::size_t i = 0; i < kernel.size(); ++i)
kernel[i].intensity = 1;
for(auto& k : kernel)
k.intensity = 1;
}

template <typename PointT>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,8 +75,8 @@ class FPFHLocalEstimation : public LocalEstimator<PointInT, FeatureT> {
fpfh_estimate.setRadiusSearch(support_radius_);
fpfh_estimate.compute(*fpfhs);

signatures->resize(fpfhs->points.size());
signatures->width = static_cast<int>(fpfhs->points.size());
signatures->resize(fpfhs->size());
signatures->width = fpfhs->size();
signatures->height = 1;

int size_feat = 33;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -107,8 +107,8 @@ class SHOTLocalEstimation : public LocalEstimator<PointInT, FeatureT> {
shot_estimate.setInputNormals(normals);
shot_estimate.setRadiusSearch(support_radius_);
shot_estimate.compute(*shots);
signatures->resize(shots->points.size());
signatures->width = static_cast<int>(shots->points.size());
signatures->resize(shots->size());
signatures->width = shots->size();
signatures->height = 1;

int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,8 @@ class SHOTLocalEstimationOMP : public LocalEstimator<PointInT, FeatureT> {
shot_estimate.compute(*shots);
}

signatures->resize(shots->points.size());
signatures->width = static_cast<int>(shots->points.size());
signatures->resize(shots->size());
signatures->width = shots->size();
signatures->height = 1;

int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -208,7 +208,7 @@ pcl::rec_3d_framework::LocalRecognitionPipeline<Distance, PointInT, FeatureT>::
PointInTPtr keypoints_pointcloud;

if (signatures_ != nullptr && processed_ != nullptr &&
(signatures_->size() == keypoints_pointcloud->points.size())) {
(signatures_->size() == keypoints_pointcloud->size())) {
keypoints_pointcloud = keypoints_input_;
signatures = signatures_;
processed = processed_;
Expand Down
16 changes: 8 additions & 8 deletions apps/in_hand_scanner/src/icp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -160,9 +160,9 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model,
{
// Check the input
// TODO: Double check the minimum number of points necessary for icp
const std::size_t n_min = 4;
const index_t n_min = 4;

if(mesh_model->sizeVertices () < n_min || cloud_data->size () < n_min)
if(static_cast<index_t>(mesh_model->sizeVertices ()) < n_min || cloud_data->size () < n_min)
{
std::cerr << "ERROR in icp.cpp: Not enough input points!\n";
return (false);
Expand Down Expand Up @@ -193,8 +193,8 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model,
const CloudNormalConstPtr cloud_data_selected = this->selectDataPoints (cloud_data);
t_select = sw.getTime ();

const std::size_t n_model = cloud_model_selected->size ();
const std::size_t n_data = cloud_data_selected->size ();
const index_t n_model = cloud_model_selected->size ();
const index_t n_data = cloud_data_selected->size ();
if(n_model < n_min) {std::cerr << "ERROR in icp.cpp: Not enough model points after selection!\n"; return (false);}
if(n_data < n_min) {std::cerr << "ERROR in icp.cpp: Not enough data points after selection!\n"; return (false);}

Expand All @@ -203,7 +203,7 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model,
kd_tree_->setInputCloud (cloud_model_selected);
t_build = sw.getTime ();

std::vector <int> index (1);
Indices index (1);
std::vector <float> squared_distance (1);

// Clouds with one to one correspondences
Expand Down Expand Up @@ -243,7 +243,7 @@ pcl::ihs::ICP::findTransformation (const MeshConstPtr& mesh_model,
// Check the distance threshold
if (squared_distance [0] < squared_distance_threshold)
{
if ((std::size_t) index [0] >= cloud_model_selected->size ())
if (index [0] >= cloud_model_selected->size ())
{
std::cerr << "ERROR in icp.cpp: Segfault!\n";
std::cerr << " Trying to access index " << index [0] << " >= " << cloud_model_selected->size () << std::endl;
Expand Down Expand Up @@ -371,7 +371,7 @@ pcl::ihs::ICP::selectModelPoints (const MeshConstPtr& mesh_model,
const Eigen::Matrix4f& T_inv) const
{
const CloudNormalPtr cloud_model_out (new CloudNormal ());
cloud_model_out->reserve (mesh_model->sizeVertices ());
cloud_model_out->reserve (static_cast<index_t>(mesh_model->sizeVertices ()));

const Mesh::VertexDataCloud& cloud = mesh_model->getVertexDataCloud ();

Expand Down Expand Up @@ -424,7 +424,7 @@ pcl::ihs::ICP::minimizePointPlane (const CloudNormal& cloud_source,
{
// Check the input
// n < n_min already checked in the icp main loop
const std::size_t n = cloud_source.size ();
const index_t n = cloud_source.size ();
if (cloud_target.size () != n)
{
std::cerr << "ERROR in icp.cpp: Input must have the same size!\n";
Expand Down
9 changes: 4 additions & 5 deletions apps/in_hand_scanner/src/integration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -212,10 +212,9 @@ pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data,

// Nearest neighbor search
CloudXYZPtr xyz_model (new CloudXYZ ());
xyz_model->reserve (mesh_model->sizeVertices ());
for (std::size_t i=0; i<mesh_model->sizeVertices (); ++i)
xyz_model->reserve (static_cast<index_t>(mesh_model->sizeVertices ()));
for (const auto& pt : mesh_model->getVertexDataCloud ())
{
const PointIHS& pt = mesh_model->getVertexDataCloud () [i];
xyz_model->push_back (PointXYZ (pt.x, pt.y, pt.z));
}
kd_tree_->setInputCloud (xyz_model);
Expand Down Expand Up @@ -390,7 +389,7 @@ pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data,
void
pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const
{
for (std::size_t i=0; i<mesh->sizeVertices (); ++i)
for (index_t i=0; i < static_cast<index_t>(mesh->sizeVertices ()); ++i)
{
PointIHS& pt = mesh->getVertexDataCloud () [i];
if (pt.age < max_age_)
Expand Down Expand Up @@ -424,7 +423,7 @@ pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const
void
pcl::ihs::Integration::removeUnfitVertices (const MeshPtr& mesh, const bool cleanup) const
{
for (std::size_t i=0; i<mesh->sizeVertices (); ++i)
for (index_t i=0; i < static_cast<index_t>(mesh->sizeVertices ()); ++i)
{
if (pcl::ihs::countDirections (mesh->getVertexDataCloud () [i].directions) < min_directions_)
{
Expand Down
2 changes: 1 addition & 1 deletion apps/in_hand_scanner/src/opengl_viewer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -926,7 +926,7 @@ pcl::ihs::OpenGLViewer::drawMeshes ()
}
case COL_VISCONF:
{
for (std::size_t i=0; i<mesh.vertices.size (); ++i)
for (index_t i=0; i<mesh.vertices.size (); ++i)
{
const unsigned int n = pcl::ihs::countDirections (mesh.vertices [i].directions);
const unsigned int index = static_cast <unsigned int> (
Expand Down
10 changes: 6 additions & 4 deletions apps/point_cloud_editor/src/cloud.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,8 @@
#include <pcl/apps/point_cloud_editor/common.h>
#include <pcl/apps/point_cloud_editor/copyBuffer.h>

using pcl::index_t;

const float Cloud::DEFAULT_POINT_DISPLAY_SIZE_ = 2.0f;
const float Cloud::DEFAULT_POINT_HIGHLIGHT_SIZE_ = 4.0f;

Expand Down Expand Up @@ -416,7 +418,7 @@ Cloud::getDisplaySpacePoint (unsigned int index) const
void
Cloud::getDisplaySpacePoints (Point3DVector& pts) const
{
for(std::size_t i = 0; i < cloud_.size(); ++i)
for(index_t i = 0; i < cloud_.size(); ++i)
pts.push_back(getDisplaySpacePoint(i));
}

Expand Down Expand Up @@ -462,12 +464,12 @@ Cloud::updateCloudMembers ()
float *pt = &(cloud_.points[0].data[X]);
std::copy(pt, pt+XYZ_SIZE, max_xyz_);
std::copy(max_xyz_, max_xyz_+XYZ_SIZE, min_xyz_);
for (std::size_t i = 1; i < cloud_.size(); ++i)
for (const auto& pt : cloud_)
{
for (unsigned int j = 0; j < XYZ_SIZE; ++j)
{
min_xyz_[j] = std::min(min_xyz_[j], cloud_.points[i].data[j]);
max_xyz_[j] = std::max(max_xyz_[j], cloud_.points[i].data[j]);
min_xyz_[j] = std::min(min_xyz_[j], pt.data[j]);
max_xyz_[j] = std::max(max_xyz_[j], pt.data[j]);
}
}
float range = 0.0f;
Expand Down
8 changes: 4 additions & 4 deletions apps/src/grabcut_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -531,11 +531,11 @@ main(int argc, char** argv)
new pcl::PointCloud<pcl::PointXYZRGB>(scene->width, scene->height));

if (scene->isOrganized()) {
std::uint32_t height_1 = scene->height - 1;
for (std::size_t i = 0; i < scene->height; ++i) {
for (std::size_t j = 0; j < scene->width; ++j) {
pcl::index_t height_1 = scene->height - 1;
for (pcl::index_t i = 0; i < scene->height; ++i) {
for (pcl::index_t j = 0; j < scene->width; ++j) {
const pcl::PointXYZRGB& p = (*scene)(j, i);
std::size_t reverse_index = (height_1 - i) * scene->width + j;
pcl::index_t reverse_index = (height_1 - i) * scene->width + j;
display_image->points[reverse_index].r = static_cast<float>(p.r) / 255.0;
display_image->points[reverse_index].g = static_cast<float>(p.g) / 255.0;
display_image->points[reverse_index].b = static_cast<float>(p.b) / 255.0;
Expand Down
21 changes: 9 additions & 12 deletions apps/src/ni_agast.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -227,17 +227,16 @@ class AGASTDemo {
keypoints3d.height = keypoints->height;
keypoints3d.is_dense = true;

std::size_t j = 0;
for (std::size_t i = 0; i < keypoints->size(); ++i) {
index_t j = 0;
for (const auto& point : *keypoints) {
const PointT& pt =
(*cloud)(static_cast<long unsigned int>(keypoints->points[i].u),
static_cast<long unsigned int>(keypoints->points[i].v));
(*cloud)(point.u, point.v);
if (!std::isfinite(pt.x) || !std::isfinite(pt.y) || !std::isfinite(pt.z))
continue;

keypoints3d.points[j].x = pt.x;
keypoints3d.points[j].y = pt.y;
keypoints3d.points[j].z = pt.z;
keypoints3d[j].x = pt.x;
keypoints3d[j].y = pt.y;
keypoints3d[j].z = pt.z;
++j;
}

Expand Down Expand Up @@ -297,11 +296,9 @@ class AGASTDemo {

if (keypoints && !keypoints->empty()) {
image_viewer_.removeLayer(getStrBool(keypts));
for (std::size_t i = 0; i < keypoints->size(); ++i) {
int u = int(keypoints->points[i].u);
int v = int(keypoints->points[i].v);
image_viewer_.markPoint(u,
v,
for (const auto& pt : *keypoints) {
image_viewer_.markPoint(static_cast<int>(pt.u),
static_cast<int>(pt.v),
visualization::red_color,
visualization::blue_color,
10,
Expand Down
14 changes: 6 additions & 8 deletions apps/src/ni_brisk.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -180,10 +180,10 @@ class BRISKDemo {
keypoints3d.height = keypoints->height;
keypoints3d.is_dense = true;

std::size_t j = 0;
for (std::size_t i = 0; i < keypoints->size(); ++i) {
index_t j = 0;
for (const auto& point : keypoints->points) {
PointT pt =
bilinearInterpolation(cloud, keypoints->points[i].x, keypoints->points[i].y);
bilinearInterpolation(cloud, point.x, point.y);

keypoints3d.points[j].x = pt.x;
keypoints3d.points[j].y = pt.y;
Expand Down Expand Up @@ -244,11 +244,9 @@ class BRISKDemo {
image_viewer_.showRGBImage<PointT>(cloud);

image_viewer_.removeLayer(getStrBool(keypts));
for (std::size_t i = 0; i < keypoints->size(); ++i) {
int u = int(keypoints->points[i].x);
int v = int(keypoints->points[i].y);
image_viewer_.markPoint(u,
v,
for(const auto& pt : keypoints->points) {
image_viewer_.markPoint(int(pt.x),
int(pt.y),
Comment on lines +248 to +249
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

here

visualization::red_color,
visualization::blue_color,
10,
Expand Down
2 changes: 1 addition & 1 deletion apps/src/ni_susan.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -151,7 +151,7 @@ class SUSANDemo {

if (keypoints && !keypoints->empty()) {
image_viewer_.removeLayer(getStrBool(keypts));
for (std::size_t i = 0; i < keypoints->size(); ++i) {
for (index_t i = 0; i < keypoints->size(); ++i) {
int u = int(keypoints->points[i].label % cloud->width);
int v = cloud->height - int(keypoints->points[i].label / cloud->width);
image_viewer_.markPoint(u,
Expand Down
2 changes: 1 addition & 1 deletion apps/src/openni_klt.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -257,7 +257,7 @@ class OpenNIViewer {

std::vector<float> markers;
markers.reserve(keypoints_->size() * 2);
for (std::size_t i = 0; i < keypoints_->size(); ++i) {
for (pcl::index_t i = 0; i < keypoints_->size(); ++i) {
if (points_status_->indices[i] < 0)
continue;
const pcl::PointUV& uv = keypoints_->points[i];
Expand Down
2 changes: 1 addition & 1 deletion apps/src/pcd_select_object_plane.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -574,7 +574,7 @@ class ObjectSelection {
int poff = fields[rgba_index].offset;
// BGR to RGB
rgb_data_ = new unsigned char[cloud_->width * cloud_->height * 3];
for (std::uint32_t i = 0; i < cloud_->width * cloud_->height; ++i) {
for (index_t i = 0; i < cloud_->width * cloud_->height; ++i) {
RGB rgb;
memcpy(&rgb,
reinterpret_cast<unsigned char*>(&cloud_->points[i]) + poff,
Expand Down
8 changes: 4 additions & 4 deletions apps/src/test_search.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,10 @@ main(int argc, char** argv)
pcl::io::loadPCDFile(pcd_path, *cloud);
else {
cloud->resize(1000000);
for (std::size_t idx = 0; idx < cloud->size(); ++idx) {
(*cloud)[idx].x = static_cast<float>(rand() / RAND_MAX);
(*cloud)[idx].y = static_cast<float>(rand() / RAND_MAX);
(*cloud)[idx].z = static_cast<float>(rand() / RAND_MAX);
for (auto& pt : *cloud) {
pt.x = static_cast<float>(rand() / RAND_MAX);
pt.y = static_cast<float>(rand() / RAND_MAX);
pt.z = static_cast<float>(rand() / RAND_MAX);
}
}

Expand Down
8 changes: 4 additions & 4 deletions common/include/pcl/common/impl/centroid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -691,10 +691,10 @@ demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
cloud_out.width = static_cast<std::uint32_t> (indices.size ());
cloud_out.height = 1;
}
cloud_out.resize (indices.size ());
cloud_out.resize (static_cast<index_t>(indices.size ()));

// Subtract the centroid from cloud_in
for (std::size_t i = 0; i < indices.size (); ++i)
for (index_t i = 0; i < static_cast<index_t>(indices.size ()); ++i)
{
cloud_out[i].x = static_cast<float> (cloud_in[indices[i]].x - centroid[0]);
cloud_out[i].y = static_cast<float> (cloud_in[indices[i]].y - centroid[1]);
Expand Down Expand Up @@ -749,11 +749,11 @@ demeanPointCloud (const pcl::PointCloud<PointT> &cloud_in,
const Eigen::Matrix<Scalar, 4, 1> &centroid,
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_out)
{
std::size_t npts = cloud_in.size ();
const auto npts = cloud_in.size ();

cloud_out = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>::Zero (4, npts); // keep the data aligned

for (std::size_t i = 0; i < npts; ++i)
for (index_t i = 0; i < npts; ++i)
{
cloud_out (0, i) = cloud_in[i].x - centroid[0];
cloud_out (1, i) = cloud_in[i].y - centroid[1];
Expand Down
Loading