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 7 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
164 changes: 49 additions & 115 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,12 @@
#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/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 +151,66 @@ 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;
bounding_boxes_[i] = computeBoundingBoxAndCenterTemplatePointCloud (template_point_clouds_[i]);
}

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.points[j];
return (true);
}

if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
continue;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
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

bb.width = bb.height = bb.depth = 0.0f;

Eigen::Vector3f geometric_center = Eigen::Vector3f::Zero ();
Eigen::Vector3f min_pos, max_pos;
min_pos.fill (std::numeric_limits<float>::max ());
max_pos.fill (std::numeric_limits<float>::lowest ());
std::size_t counter = 0;
for (std::size_t j = 0; j < template_point_cloud.size (); ++j)
{
const PointXYZRGBA & p = template_point_cloud.points[j];

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);
if (!isXYZFinite(p))
continue;

center_x += p.x;
center_y += p.y;
center_z += p.z;
min_pos = min_pos.cwiseMin (p.getVector3fMap ());
max_pos = max_pos.cwiseMax (p.getVector3fMap ());

++counter;
}
geometric_center += p.getVector3fMap ();

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

bb.width = max_x - min_x;
bb.height = max_y - min_y;
bb.depth = max_z - min_z;
Eigen::Vector3f bb_dim = max_pos - min_pos;
bb.width = bb_dim[0];
bb.height = bb_dim[1];
bb.depth = bb_dim[2];

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.

Eigen::Vector3f diff_pos = min_pos - geometric_center;
bb.x = diff_pos[0];
bb.y = diff_pos[1];
bb.z = diff_pos[2];

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

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

p.x -= center_x;
p.y -= center_y;
p.z -= center_z;
p.getVector3fMap () -= geometric_center;

template_point_cloud.points[j] = p;
}
template_point_cloud.points[j] = p;
}

return (true);
return bb;
}


Expand All @@ -237,69 +232,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.points[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.points[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.points[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