37 #ifndef JSK_PCL_ROS_PARTICLE_CUBOID_H_ 38 #define JSK_PCL_ROS_PARTICLE_CUBOID_H_ 40 #include <pcl/point_cloud.h> 95 inline ParticleCuboid (
float _x,
float _y,
float _z,
float _roll,
float _pitch,
float _yaw)
97 x = _x;
y = _y;
z = _z;
110 Eigen::Vector3f
pos(pose.translation());
112 getVector3fMap() =
pos;
133 float trans_x, trans_y, trans_z, trans_roll, trans_pitch, trans_yaw;
134 getTranslationAndEulerAngles (trans,
135 trans_x, trans_y, trans_z,
136 trans_roll, trans_pitch, trans_yaw);
141 inline float operator [] (
unsigned int i)
const 149 case 4:
return pitch;
160 const std::vector<double>&
mean,
const std::vector<double>& cov)
172 jsk_recognition_msgs::BoundingBox
box;
173 Eigen::Affine3f affine = toEigenMatrix();
175 box.dimensions.x =
dx;
176 box.dimensions.y =
dy;
177 box.dimensions.z =
dz;
183 std::vector<int> visible_faces;
184 visible_faces.reserve(3);
185 if (local_view_point[0] > 0) {
186 visible_faces.push_back(0);
188 else if (local_view_point[0] < 0) {
189 visible_faces.push_back(2);
191 if (local_view_point[1] > 0) {
192 visible_faces.push_back(1);
194 else if (local_view_point[1] < 0) {
195 visible_faces.push_back(3);
197 if (local_view_point[2] > 0) {
198 visible_faces.push_back(4);
200 else if (local_view_point[2] < 0) {
201 visible_faces.push_back(5);
203 return visible_faces;
207 const Eigen::Vector3f& v,
208 const std::vector<int>& visible_faces,
209 std::vector<jsk_pcl_ros::Polygon::Ptr>& faces)
const 211 double min_distance = DBL_MAX;
212 for (
size_t i = 0; i < visible_faces.size(); i++) {
215 face->nearestPoint(v, d);
216 if (min_distance > d) {
227 const Eigen::Vector3f& v,
const int plane_index)
const 234 n = Eigen::Vector3f::UnitX();
239 n = Eigen::Vector3f::UnitY();
244 n = - (Eigen::Vector3f::UnitX());
249 n = - (Eigen::Vector3f::UnitY());
254 n = Eigen::Vector3f::UnitZ();
259 n = - (Eigen::Vector3f::UnitZ());
265 return (n.dot(v) + d) /
sqrt(n.dot(n) + d * d);
269 const Eigen::Vector3f& v,
const int plane_index)
const 271 return std::abs(signedDistanceToPlane(v, plane_index));
278 const float x = local_v[0];
279 const float y = local_v[1];
280 const float z = local_v[2];
281 const float abs_x = std::abs(x);
282 const float abs_y = std::abs(y);
283 const float abs_z = std::abs(z);
284 if (x > 0 && abs_x >= abs_y && abs_x >= abs_z) {
287 else if (x < 0 && abs_x >= abs_y && abs_x >= abs_z) {
290 else if (y > 0 && abs_y >= abs_x && abs_y >= abs_z) {
293 else if (y < 0 && abs_y >= abs_x && abs_y >= abs_z) {
296 else if (z > 0 && abs_z >= abs_x && abs_z >= abs_y) {
299 else if (z < 0 && abs_z >= abs_x && abs_z >= abs_y) {
306 Eigen::Affine3f
pose = toEigenMatrix();
307 Eigen::Vector3f dimensions(
dx,
dy,
dz);
309 Eigen::Quaternionf(pose.rotation()),
321 Eigen::Affine3f transformed_affine = transform * particle_affine;
323 new_p.x = transformed_affine.translation()[0];
324 new_p.y = transformed_affine.translation()[1];
325 new_p.z = transformed_affine.translation()[2];
326 getEulerAngles(transformed_affine, new_p.
roll, new_p.
pitch, new_p.
yaw);
352 newp.x =
static_cast<float> (p.x * val);
353 newp.y =
static_cast<float> (p.y * val);
354 newp.z =
static_cast<float> (p.z * val);
355 newp.
roll =
static_cast<float> (p.
roll * val);
356 newp.
pitch =
static_cast<float> (p.
pitch * val);
357 newp.
yaw =
static_cast<float> (p.
yaw * val);
358 newp.
dx =
static_cast<float> (p.
dx * val);
359 newp.
dy =
static_cast<float> (p.
dy * val);
360 newp.
dz =
static_cast<float> (p.
dz * val);
371 return a + (-1.0) * b;
389 for (
int i = 0; i < nr_dimensions_; ++i)
struct pcl::PointXYZHSV EIGEN_ALIGN16
pcl::tracking::ParticleCuboid operator+(const ParticleCuboid &a, const ParticleCuboid &b)
DefaultPointRepresentation()
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
ParticleCuboid(const ParticleCuboid &obj)
boost::shared_ptr< Polygon > Ptr
Eigen::Affine3f toEigenMatrix() const
jsk_pcl_ros::Cube::Ptr toCube() const
pcl::tracking::ParticleCuboid operator-(const ParticleCuboid &a, const ParticleCuboid &b)
ParticleCuboid(float _x, float _y, float _z, float _roll, float _pitch, float _yaw)
std::vector< int > visibleFaceIndices(const Eigen::Vector3f local_view_point) const
virtual void copyToFloatArray(const pcl::tracking::ParticleCuboid &p, float *out) const
jsk_recognition_msgs::BoundingBox toBoundingBox()
double signedDistanceToPlane(const Eigen::Vector3f &v, const int plane_index) const
static int stateDimension()
ParticleCuboid operator*(const ParticleCuboid &p, const Eigen::Affine3f &transform)
face(linetab, int mi, int m, int r, int n)
void fromEigen(const Eigen::Affine3f &pose)
void sample(const std::vector< double > &mean, const std::vector< double > &cov)
int nearestPlaneIndex(const Eigen::Vector3f &local_v) const
double distanceToPlane(const Eigen::Vector3f &v, const int plane_index) const
double distanceNearestToPlaneWithOcclusion(const Eigen::Vector3f &v, const std::vector< int > &visible_faces, std::vector< jsk_pcl_ros::Polygon::Ptr > &faces) const
static pcl::tracking::ParticleCuboid toState(const Eigen::Affine3f &trans)
Cube cube(Eigen::Vector3f(1, 0, 0), Eigen::Quaternionf(0.108755, 0.088921, 0.108755, 0.984092), Eigen::Vector3f(0.3, 0.3, 0.3))
POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::PointRGB,(float, rgb, rgb))
boost::shared_ptr< Cube > Ptr