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

Fix several issues #6193

Merged
merged 5 commits into from
Dec 9, 2024
Merged

Fix several issues #6193

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
711d911
Add checks in point cloud handler for correct types
mvieth Dec 7, 2024
2287402
Fix for clang-cl (clang used within Visual Studio)
mvieth Dec 7, 2024
547df3c
estimateProjectionMatrix: consistently use double
mvieth Dec 7, 2024
b54f2bb
Improve ICP_translated test
mvieth Dec 7, 2024
b8c9908
Temporary workaround for high memory consumption
mvieth Dec 8, 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
16 changes: 8 additions & 8 deletions common/include/pcl/common/impl/projection_matrix.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,18 +103,18 @@ estimateProjectionMatrix (

while (pointIt)
{
unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width;
unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width;
Scalar yIdx = pointIt.getCurrentPointIndex () / cloud->width;
Scalar xIdx = pointIt.getCurrentPointIndex () % cloud->width;

const PointT& point = *pointIt;
if (std::isfinite (point.x))
{
Scalar xx = point.x * point.x;
Scalar xy = point.x * point.y;
Scalar xz = point.x * point.z;
Scalar yy = point.y * point.y;
Scalar yz = point.y * point.z;
Scalar zz = point.z * point.z;
Scalar xx = static_cast<Scalar>(point.x) * static_cast<Scalar>(point.x);
Scalar xy = static_cast<Scalar>(point.x) * static_cast<Scalar>(point.y);
Scalar xz = static_cast<Scalar>(point.x) * static_cast<Scalar>(point.z);
Scalar yy = static_cast<Scalar>(point.y) * static_cast<Scalar>(point.y);
Scalar yz = static_cast<Scalar>(point.y) * static_cast<Scalar>(point.z);
Scalar zz = static_cast<Scalar>(point.z) * static_cast<Scalar>(point.z);
Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

A.coeffRef (0) += xx;
Expand Down
2 changes: 1 addition & 1 deletion common/include/pcl/impl/instantiate.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,7 +95,7 @@
//
// ((x)(y)(z))((1)(2)(3))((dracula)(radu))
//
#ifdef _MSC_VER
#if defined(_MSC_VER) && !defined(__clang__)
#define PCL_INSTANTIATE_PRODUCT_IMPL(r, product) \
BOOST_PP_CAT(PCL_INSTANTIATE_, BOOST_PP_SEQ_HEAD(product)) \
BOOST_PP_EXPAND(BOOST_PP_SEQ_TO_TUPLE(BOOST_PP_SEQ_TAIL(product)))
Expand Down
2 changes: 1 addition & 1 deletion sample_consensus/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ endif()

set(srcs
src/sac.cpp
src/sac_model_ellipse3d.cpp
src/sac_model_circle.cpp
src/sac_model_circle3d.cpp
src/sac_model_cylinder.cpp
Expand All @@ -26,7 +27,6 @@ set(srcs
src/sac_model_plane.cpp
src/sac_model_registration.cpp
src/sac_model_sphere.cpp
src/sac_model_ellipse3d.cpp
src/sac_model_torus.cpp
)

Expand Down
21 changes: 10 additions & 11 deletions test/registration/test_registration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,30 +155,26 @@ TEST (PCL, findFeatureCorrespondences)

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// This test if the ICP algorithm can successfully find the transformation of a cloud that has been
// moved 0.7 in x direction.
// moved 0.2 in z direction.
// It indirectly test the KDTree doesn't get an empty input cloud, see #3624
// It is more or less a copy of https://github.com/PointCloudLibrary/pcl/blob/cc7fe363c6463a0abc617b1e17e94ab4bd4169ef/doc/tutorials/content/sources/iterative_closest_point/iterative_closest_point.cpp
TEST(PCL, ICP_translated)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5,1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

// Fill in the CloudIn data
for (auto& point : *cloud_in)
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
*cloud_in = cloud_source;

*cloud_out = *cloud_in;

for (auto& point : *cloud_out)
point.x += 0.7f;
point.z += 0.2f;

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
icp.setMaximumIterations (50);

pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
Expand All @@ -190,9 +186,12 @@ TEST(PCL, ICP_translated)
EXPECT_LT(icp.getFitnessScore(), 1e-6);

// Ensure that the translation found is within acceptable threshold.
EXPECT_NEAR(icp.getFinalTransformation()(0, 3), 0.7, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(0, 0), 1.0, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(1, 1), 1.0, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(2, 2), 1.0, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(0, 3), 0.0, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(1, 3), 0.0, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(2, 3), 0.0, 2e-3);
EXPECT_NEAR(icp.getFinalTransformation()(2, 3), 0.2, 2e-3);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Expand Down
12 changes: 9 additions & 3 deletions visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -368,10 +368,16 @@ PointCloudColorHandlerGenericField<PointT>::setInputCloud (
{
PointCloudColorHandler<PointT>::setInputCloud (cloud);
field_idx_ = pcl::getFieldIndex<PointT> (field_name_, fields_);
if (field_idx_ != -1)
capable_ = true;
else
if (field_idx_ == -1) {
capable_ = false;
return;
}
if (fields_[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32) {
capable_ = false;
PCL_ERROR("[pcl::PointCloudColorHandlerGenericField::setInputCloud] This currently only works with float32 fields, but field %s has a different type.\n", field_name_.c_str());
return;
}
capable_ = true;
}


Expand Down
8 changes: 8 additions & 0 deletions visualization/src/point_cloud_handlers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -420,6 +420,10 @@ pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2>::Poi
{
field_idx_ = pcl::getFieldIndex (*cloud, field_name);
capable_ = field_idx_ != -1;
if (field_idx_ != -1 && cloud_->fields[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32) {
capable_ = false;
PCL_ERROR("[pcl::PointCloudColorHandlerGenericField] This currently only works with float32 fields, but field %s has a different type.\n", field_name.c_str());
}
}

///////////////////////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -575,6 +579,10 @@ pcl::visualization::PointCloudColorHandlerLabelField<pcl::PCLPointCloud2>::Point
field_idx_ = pcl::getFieldIndex (*cloud, "label");
capable_ = field_idx_ != -1;
static_mapping_ = static_mapping;
if (field_idx_ != -1 && cloud_->fields[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::UINT32) {
capable_ = false;
PCL_ERROR("[pcl::PointCloudColorHandlerLabelField] This currently only works with uint32 fields, but label field has a different type.\n");
}
}

///////////////////////////////////////////////////////////////////////////////////////////
Expand Down
Loading