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

Remove vector.reserve() from hot path in cloud concatenation #3361

Merged
merged 2 commits into from
Sep 24, 2019
Merged

Remove vector.reserve() from hot path in cloud concatenation #3361

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
3401fb4
Fix error and documentation in PCLPointCloud2
kunaltyagi Sep 24, 2019
9686ffe
Fix issue caused by std::vector<T, A>{}.reserve on libstdc++
kunaltyagi Sep 24, 2019
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
4 changes: 2 additions & 2 deletions common/include/pcl/point_cloud.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -271,8 +271,8 @@ namespace pcl
// Make the resultant point cloud take the newest stamp
cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);

size_t nr_points = cloud1.points.size ();
cloud1.points.reserve (nr_points + cloud2.points.size ());
// libstdc++ (GCC) on calling reserve allocates new memory, copies and deallocates old vector
// This causes a drastic performance hit. Prefer not to use reserve with libstdc++ (default on clang)
cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ());

cloud1.width = static_cast<uint32_t>(cloud1.points.size ());
Expand Down
25 changes: 14 additions & 11 deletions common/src/PCLPointCloud2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,16 +74,16 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPoi
}

// Ideally this should be in PCLPointField class since this is global behavior
// We're fine with the special RGB vs RGBA use case
auto field_eq = [](const auto& field1, const auto& field2)
{
// We're fine with the special RGB vs RGBA use case
return ((field1.name == field2.name) ||
(field1.name == "rgb" && field2.name == "rgba") ||
(field1.name == "rgba" && field2.name == "rgb"));
};

// A simple memcpy is possible if layout (name and order of fields) is same
bool memcpy_possible = std::equal(cloud1.fields.begin (),
// A simple memcpy is possible if layout (name and order of fields) is same for both clouds
bool simple_layout = std::equal(cloud1.fields.begin (),
cloud1.fields.end (),
cloud2.fields.begin (),
cloud2.fields.end (),
Expand All @@ -100,8 +100,9 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPoi
const auto max_field_size = std::max (cloud1.fields.size (), cloud2.fields.size ());
valid_fields.reserve (max_field_size);

// refactor to return std::optional<std::vector<FieldDetails>>
if (!memcpy_possible)
// @TODO: Refactor to return std::optional<std::vector<FieldDetails>>
// Store the details of fields with common data in both cloud, exit early if any errors are found
if (!simple_layout)
{
std::size_t i = 0, j = 0;
while (i < cloud1.fields.size () && j < cloud2.fields.size ())
Expand All @@ -119,6 +120,7 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPoi

if (field_eq(cloud1.fields[i], cloud2.fields[j]))
{
// Assumption: cloud1.fields[i].datatype == cloud2.fields[j].datatype
valid_fields.emplace_back(i, j, pcl::getFieldSize (cloud2.fields[j].datatype));
++i;
++j;
Expand All @@ -127,36 +129,37 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPoi
PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
return (false);
}
// Both i and j should have exhausted their respective cloud.fields
if (i != cloud1.fields.size () || j != cloud2.fields.size ())
{
PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Number of fields to copy in cloud1 (%u) != Number of fields to copy in cloud2 (%u)\n", i, j);
return (false);
}
}

// It's safe to modify the cloud1 inplace
// Save the latest timestamp in the destination cloud
cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);

cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
cloud1.height = 1;
cloud1.width = size1 + size2;

const auto data1_size = cloud1.data.size ();

// conservative allocation
cloud1.data.reserve (data1_size + cloud2.data.size ());
if (memcpy_possible)
if (simple_layout)
{
cloud1.data.insert (cloud1.data.end (), cloud2.data.begin (), cloud2.data.end ());
return (true);
}
const auto data1_size = cloud1.data.size ();
cloud1.data.resize(data1_size + cloud2.data.size ());
for (std::size_t cp = 0; cp < size2; ++cp)
{
for (const auto field_data: valid_fields)
{
const auto& i = field_data.idx1;
const auto& j = field_data.idx2;
const auto& size = field_data.size;
// Leave the data for the skip fields untouched in cloud1
// Copy only the required data from cloud2 to the correct location for cloud1
memcpy (reinterpret_cast<char*> (&cloud1.data[data1_size + cp * cloud1.point_step + cloud1.fields[i].offset]),
reinterpret_cast<const char*> (&cloud2.data[cp * cloud2.point_step + cloud2.fields[j].offset]),
cloud2.fields[j].count * size);
Expand Down