Skip to content

Add compute bounding box member method #3970

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

Open
wants to merge 13 commits into
base: master
Choose a base branch
from
Open

Add compute bounding box member method #3970

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
ab728a1
Add compute bounding box member method
truhoang Apr 18, 2020
0dc36ce
Refactor position coordinates with Eigen vectors
truhoang Apr 25, 2020
54452ab
Use eigen interface to access point's coordinates
truhoang Apr 26, 2020
ec1adc6
Change function interface and remove auto
truhoang Apr 27, 2020
7c9940e
Change function name
truhoang Apr 27, 2020
999d30f
Use eigen min/max functions
truhoang May 4, 2020
593be52
Fix Eigen min/ma assignment
truhoang May 7, 2020
7db6764
Clean up for-loops and remove redundancies
truhoang May 21, 2020
614a1dd
Replace vector3f with vector4f
truhoang Jun 4, 2020
9a53321
Merge branch 'master' into refactor-eigen
truhoang Jun 29, 2020
e5d5533
Empty commit to start CI
truhoang Jun 30, 2020
18c4f75
Merge branch 'refactor-eigen' of ssh://github.com/truhoang/pcl into r…
truhoang Jun 30, 2020
fd9b041
Fix eigen vector type bug
truhoang Jun 30, 2020
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
162 changes: 44 additions & 118 deletions recognition/include/pcl/recognition/impl/linemod/line_rgbd.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,11 @@
#ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
#define PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_

//#include <pcl/recognition/linemod/line_rgbd.h>
#include <pcl/io/pcd_io.h>
#include <fcntl.h>
#include <pcl/point_cloud.h>
#include <limits>
#include <Eigen/Dense>


namespace pcl
Expand Down Expand Up @@ -150,72 +150,59 @@ LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, con

// Compute 3D bounding boxes from the template point clouds
bounding_boxes_.resize (template_point_clouds_.size ());
for (std::size_t i = 0; i < template_point_clouds_.size (); ++i)
for (std::size_t i = 0; i < template_point_clouds_.size (); ++i)
{
PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
BoundingBoxXYZ & bb = bounding_boxes_[i];
bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
bb.width = bb.height = bb.depth = 0.0f;

float center_x = 0.0f;
float center_y = 0.0f;
float center_z = 0.0f;
float min_x = std::numeric_limits<float>::max ();
float min_y = std::numeric_limits<float>::max ();
float min_z = std::numeric_limits<float>::max ();
float max_x = -std::numeric_limits<float>::max ();
float max_y = -std::numeric_limits<float>::max ();
float max_z = -std::numeric_limits<float>::max ();
std::size_t counter = 0;
for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
{
const PointXYZRGBA & p = template_point_cloud[j];

if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
continue;

min_x = std::min (min_x, p.x);
min_y = std::min (min_y, p.y);
min_z = std::min (min_z, p.z);
max_x = std::max (max_x, p.x);
max_y = std::max (max_y, p.y);
max_z = std::max (max_z, p.z);
bounding_boxes_[i] = computeBoundingBoxAndCenterTemplatePointCloud (template_point_clouds_[i]);
}

center_x += p.x;
center_y += p.y;
center_z += p.z;
return (true);
}

++counter;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointXYZT, typename PointRGBT> BoundingBoxXYZ
pcl::LineRGBD<PointXYZT, PointRGBT>::computeBoundingBoxAndCenterTemplatePointCloud (PointCloud<PointXYZRGBA> & template_point_cloud)
{
BoundingBoxXYZ bb;
bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
Comment on lines +165 to +166
Copy link
Member

Choose a reason for hiding this comment

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

Move this definition to where you actually use it. Line 187. No need to initialize it's members x, y, and z


Eigen::Vector4f geometric_center = Eigen::Vector4f::Zero ();
Eigen::Vector4f min_pos, max_pos;
Comment on lines +168 to +169
Copy link
Member

Choose a reason for hiding this comment

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

Use Eigen::Arrays and instead of matrices to express better your intent for coefficient-wise operations.

min_pos.fill (std::numeric_limits<float>::max ());
max_pos.fill (std::numeric_limits<float>::lowest ());
std::size_t counter = 0;
for (const PointXYZRGBA & p : template_point_cloud.points)
{
if (!isXYZFinite(p))
continue;

center_x /= static_cast<float> (counter);
center_y /= static_cast<float> (counter);
center_z /= static_cast<float> (counter);
min_pos = min_pos.cwiseMin (p.getVector4fMap ());
max_pos = max_pos.cwiseMax (p.getVector4fMap ());

bb.width = max_x - min_x;
bb.height = max_y - min_y;
bb.depth = max_z - min_z;
geometric_center += p.getVector4fMap ();

bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
Comment on lines -199 to -201
Copy link
Member

Choose a reason for hiding this comment

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

🤦‍♂️ I'm embarrassed this exists in our code base.

++counter;
}
geometric_center /= static_cast<float> (counter);
Comment on lines +173 to +185
Copy link
Member

Choose a reason for hiding this comment

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

Have a look at this to prevent the need for having a foor loop, in case the point cloud is dense.

dim=4
stride=sizeof(PointXYZRGBA)/sizeof(float) (confirm this is an actual integer even if you did floating point division. it should be 5)
offset=0

Then use eigen function for mean and extracting min and max element.

Comment on lines +181 to +185
Copy link
Member

Choose a reason for hiding this comment

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

This is not a geometric center, this is the center of mass of all points. Did you expect this to be the center of the bounding box? If so, this is a bug.


for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
{
PointXYZRGBA p = template_point_cloud[j];
Eigen::Vector4f bb_dim = max_pos - min_pos;
bb.width = bb_dim[0];
bb.height = bb_dim[1];
bb.depth = bb_dim[2];
Comment on lines +187 to +190
Copy link
Member

@SergioRAgostinho SergioRAgostinho Jul 1, 2020 •
edited
Loading

Choose a reason for hiding this comment

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

To compute the geometric center

 Eigen::Vector4f geometric_center = 0.5*(max_pos + min_pos);


if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
continue;
Eigen::Vector4f diff_pos = min_pos - geometric_center;
bb.x = diff_pos[0];
bb.y = diff_pos[1];
bb.z = diff_pos[2];

p.x -= center_x;
p.y -= center_y;
p.z -= center_z;
for (PointXYZRGBA & p : template_point_cloud.points)
Copy link
Member

Choose a reason for hiding this comment

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

  for (PointXYZRGBA & p : template_point_cloud)

{
if (!isXYZFinite(p))
continue;

template_point_cloud[j] = p;
}
p.getVector4fMap () -= geometric_center;
Comment on lines +197 to +202
Copy link
Member

@SergioRAgostinho SergioRAgostinho Jul 1, 2020 •
edited
Loading

Choose a reason for hiding this comment

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

No need to check if the point is finite. if it isn't the subtraction operation will generate more nans on the same points. Meaning you can use a matrix wise row subtraction with the Mapping I mentioned earlier.

}

return (true);
return bb;
}


Expand All @@ -237,69 +224,8 @@ LineRGBD<PointXYZT, PointRGBT>::createAndAddTemplate (
// Compute 3D bounding boxes from the template point clouds
bounding_boxes_.resize (template_point_clouds_.size ());
{
const std::size_t i = template_point_clouds_.size () - 1;

PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
BoundingBoxXYZ & bb = bounding_boxes_[i];
bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
bb.width = bb.height = bb.depth = 0.0f;

float center_x = 0.0f;
float center_y = 0.0f;
float center_z = 0.0f;
float min_x = std::numeric_limits<float>::max ();
float min_y = std::numeric_limits<float>::max ();
float min_z = std::numeric_limits<float>::max ();
float max_x = -std::numeric_limits<float>::max ();
float max_y = -std::numeric_limits<float>::max ();
float max_z = -std::numeric_limits<float>::max ();
std::size_t counter = 0;
for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
{
const PointXYZRGBA & p = template_point_cloud[j];

if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
continue;

min_x = std::min (min_x, p.x);
min_y = std::min (min_y, p.y);
min_z = std::min (min_z, p.z);
max_x = std::max (max_x, p.x);
max_y = std::max (max_y, p.y);
max_z = std::max (max_z, p.z);

center_x += p.x;
center_y += p.y;
center_z += p.z;

++counter;
}

center_x /= static_cast<float> (counter);
center_y /= static_cast<float> (counter);
center_z /= static_cast<float> (counter);

bb.width = max_x - min_x;
bb.height = max_y - min_y;
bb.depth = max_z - min_z;

bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;

for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
{
PointXYZRGBA p = template_point_cloud[j];

if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
continue;

p.x -= center_x;
p.y -= center_y;
p.z -= center_z;

template_point_cloud[j] = p;
}
const std::size_t new_idx = template_point_clouds_.size () - 1;
bounding_boxes_[new_idx] = computeBoundingBoxAndCenterTemplatePointCloud (template_point_clouds_[new_idx]);
}

std::vector<pcl::QuantizableModality*> modalities;
Expand Down
4 changes: 4 additions & 0 deletions recognition/include/pcl/recognition/linemod/line_rgbd.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -282,6 +282,10 @@ namespace pcl
bool
readLTMHeader (int fd, pcl::io::TARHeader &header);

/** \brief Compute bounding box from PCD masks **/
Copy link
Member

Choose a reason for hiding this comment

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

Missing parameter description

BoundingBoxXYZ
computeBoundingBoxAndCenterTemplatePointCloud (PointCloud<PointXYZRGBA> & template_point_cloud);

/** \brief Intersection volume threshold. */
float intersection_volume_threshold_;

Expand Down