PrimitiveFusion.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX
19  * @author Markus Grotz ( markus dot grotz at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "PrimitiveFusion.h"
26 
27 #include <RobotAPI/libraries/core/FramedPose.h>
28 
29 
30 using namespace armarx;
31 
32 PrimitiveFusion::PrimitiveFusion()
33 {
34 }
35 
36 PrimitiveFusion::~PrimitiveFusion()
37 {
38 }
39 
40 // todo rename
41 bool
42 PrimitiveFusion::testBoxIntersection(Eigen::Matrix4f leftPose,
43  Eigen::Matrix4f rightPose,
44  Eigen::Vector3f leftExtent,
45  Eigen::Vector3f rightExtent)
46 {
47  Eigen::Matrix3Xf axes(3, 15);
48 
49  axes.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
50  axes.block<3, 3>(0, 3) = rightPose.block<3, 3>(0, 0);
51 
52 
53  for (int i = 0; i < 3; i++)
54  {
55  for (int j = 0; j < 3; j++)
56  {
57  Eigen::Vector3f a = leftPose.col(i).head<3>();
58  Eigen::Vector3f b = rightPose.col(j).head<3>();
59  int idx = 6 + (i * 3 + j);
60  Eigen::Vector3f c = a.cross(b);
61  if (a.norm() == 0)
62  {
63  axes.col(idx) = Eigen::Vector3f::Zero();
64  }
65  else
66  {
67  axes.col(idx) = c / c.norm();
68  }
69  }
70  }
71 
72  for (int i = 0; i < 15; i++)
73  {
74  float ra = 0.0;
75  float rb = 0.0;
76 
77  Eigen::Vector3f axis = axes.col(i);
78 
79  if (axis.norm() == 0)
80  {
81  continue;
82  }
83 
84  for (int j = 0; j < 3; j++)
85  {
86  Eigen::Vector3f a = leftPose.col(j).head<3>();
87  Eigen::Vector3f b = rightPose.col(j).head<3>();
88 
89  ra += fabs(axis.dot(a) * leftExtent(j));
90  rb += fabs(axis.dot(b) * rightExtent(j));
91  }
92 
93  Eigen::Vector3f t = projectVector(axis, leftPose.col(3).head<3>());
94  t = t - projectVector(axis, rightPose.col(3).head<3>());
95 
96  if (t.norm() > (ra + rb))
97  {
98  return false;
99  }
100  }
101 
102  return true;
103 }
104 
105 Eigen::Vector3f
106 PrimitiveFusion::projectVector(Eigen::Vector3f axis, Eigen::Vector3f u)
107 {
108  float v = u.dot(axis) / axis.squaredNorm();
109  return axis * v;
110 }
111 
112 void
113 PrimitiveFusion::getIntersectingPrimitives(
114  memoryx::EnvironmentalPrimitiveBaseList& primitives,
115  memoryx::EnvironmentalPrimitiveBasePtr primitive,
116  memoryx::EnvironmentalPrimitiveBaseList& intersectingPrimitives,
117  float eps)
118 {
119  Eigen::Matrix4f leftPose = FramedPosePtr::dynamicCast(primitive->getPose())->toEigen();
120  Eigen::Vector3f leftExtent =
121  Vector3Ptr::dynamicCast(primitive->getOBBDimensions())->toEigen() / 2.0 +
122  eps * Eigen::Vector3f::Ones();
123 
124  for (memoryx::EnvironmentalPrimitiveBasePtr& p : primitives)
125  {
126  Eigen::Matrix4f rightPose = FramedPosePtr::dynamicCast(p->getPose())->toEigen();
127  Eigen::Vector3f rightExtent =
128  Vector3Ptr::dynamicCast(p->getOBBDimensions())->toEigen() / 2.0 +
129  eps * Eigen::Vector3f::Ones();
130 
131  if (primitive->ice_staticId() != p->ice_staticId())
132  {
133  continue;
134  }
135  if (primitive->getId() == p->getId())
136  {
137  continue;
138  }
139  if (testBoxIntersection(leftPose, rightPose, leftExtent, rightExtent))
140  {
141  intersectingPrimitives.push_back(p);
142  }
143  }
144 }
145 
146 void
147 PrimitiveFusion::getIntersectingSimilarPrimitives(
148  memoryx::EnvironmentalPrimitiveBaseList& primitives,
149  memoryx::EnvironmentalPrimitiveBasePtr primitive,
150  memoryx::EnvironmentalPrimitiveBaseList& intersectingPrimitives,
151  float eps)
152 {
153  memoryx::EnvironmentalPrimitiveBaseList candidates;
154 
155  getIntersectingPrimitives(primitives, primitive, candidates, eps);
156 
157  for (memoryx::EnvironmentalPrimitiveBasePtr p : candidates)
158  {
159  if (testPlane(p, primitive))
160  {
161  intersectingPrimitives.push_back(p);
162  }
163 
164  else if (testCylinder(p, primitive))
165  {
166  intersectingPrimitives.push_back(p);
167  }
168  }
169 }
170 
171 bool
172 PrimitiveFusion::testCylinder(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
173  memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
174 {
175  memoryx::CylinderPrimitivePtr leftCylinder =
176  memoryx::CylinderPrimitivePtr::dynamicCast(leftPrimitive);
177  memoryx::CylinderPrimitivePtr rightCylinder =
178  memoryx::CylinderPrimitivePtr::dynamicCast(rightPrimitive);
179 
180  if (!leftCylinder || !rightCylinder)
181  {
182  return false;
183  }
184 
185  Eigen::Vector3f leftAxisDirection =
186  Vector3Ptr::dynamicCast(leftCylinder->getCylinderAxisDirection())->toEigen();
187  Eigen::Vector3f rightAxisDirection =
188  Vector3Ptr::dynamicCast(rightCylinder->getCylinderAxisDirection())->toEigen();
189 
190  float r = leftAxisDirection.dot(rightAxisDirection);
191 
192  if (std::acos(r) > pcl::deg2rad(15.0))
193  {
194  return false;
195  }
196 
197 
198  /*
199  // radius should be almost the same
200  if (std::fabs(leftCylinder->getCylinderRadius(), rightCylinder->getCylinderRadius()) < 50.0)
201  {
202  return false;
203  }
204  */
205 
206  Eigen::Matrix4f leftPose = FramedPosePtr::dynamicCast(leftCylinder->getPose())->toEigen();
207  Eigen::Vector3f leftExtent = Eigen::Vector3f(leftCylinder->getLength() / 2.0,
208  leftCylinder->getCylinderRadius(),
209  leftCylinder->getCylinderRadius());
210 
211 
212  Eigen::Matrix4f rightPose = FramedPosePtr::dynamicCast(rightCylinder->getPose())->toEigen();
213  Eigen::Vector3f rightExtent = Eigen::Vector3f(rightCylinder->getLength() / 2.0,
214  rightCylinder->getCylinderRadius(),
215  rightCylinder->getCylinderRadius());
216 
217 
218  if (!testBoxIntersection(leftPose, rightPose, leftExtent, rightExtent))
219  {
220  return false;
221  }
222 
223  return true;
224 }
225 
226 bool
227 PrimitiveFusion::testPlane(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
228  memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
229 {
230  memoryx::PlanePrimitivePtr leftPlane = memoryx::PlanePrimitivePtr::dynamicCast(leftPrimitive);
231  memoryx::PlanePrimitivePtr rightPlane = memoryx::PlanePrimitivePtr::dynamicCast(rightPrimitive);
232 
233  if (!leftPlane || !rightPlane)
234  {
235  return false;
236  }
237 
238  Eigen::Vector3f leftPlaneNormal =
239  Vector3Ptr::dynamicCast(leftPlane->getPlaneNormal())->toEigen();
240  Eigen::Vector3f rightPlaneNormal =
241  Vector3Ptr::dynamicCast(rightPlane->getPlaneNormal())->toEigen();
242 
243  if ((leftPlaneNormal - rightPlaneNormal).norm() < 0.001)
244  {
245  return true;
246  }
247 
248  float alpha = std::acos(leftPlaneNormal.dot(rightPlaneNormal));
249 
250  if (alpha > pcl::deg2rad(10.0))
251  {
252  return false;
253  }
254 
255  return true;
256 }
257 
258 bool
259 PrimitiveFusion::isSimilar(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
260  memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
261 {
262  if (leftPrimitive->ice_staticId() != rightPrimitive->ice_staticId())
263  {
264  return false;
265  }
266 
267  Eigen::Vector3f leftOBB = Vector3Ptr::dynamicCast(leftPrimitive->getOBBDimensions())->toEigen();
268  Eigen::Vector3f rightOBB =
269  Vector3Ptr::dynamicCast(rightPrimitive->getOBBDimensions())->toEigen();
270 
271  if ((leftOBB - rightOBB).norm() > 10.0)
272  {
273  return false;
274  }
275 
276  Eigen::Vector3f leftPosition =
277  Vector3Ptr::dynamicCast(leftPrimitive->getPose()->position)->toEigen();
278  Eigen::Vector3f rightPosition =
279  Vector3Ptr::dynamicCast(rightPrimitive->getPose()->position)->toEigen();
280 
281  if ((leftPosition - rightPosition).norm() > 5.0)
282  {
283  return false;
284  }
285 
286  Eigen::Matrix3f leftRotation =
287  QuaternionPtr::dynamicCast(leftPrimitive->getPose()->orientation)->toEigen();
288  Eigen::Matrix3f rightRotation =
289  QuaternionPtr::dynamicCast(rightPrimitive->getPose()->orientation)->toEigen();
290 
291  Eigen::Vector3f ea = (leftRotation.transpose() * rightRotation).eulerAngles(0, 1, 2);
292 
293 
294  float delta = pcl::deg2rad(15.0);
295  for (int i = 0; i < 3; i++)
296  {
297  ea[i] = std::fabs(ea[i]);
298  ea[i] = std::fmod(ea[i], M_PI);
299  if (ea[i] > delta && ea[i] < (M_PI - delta))
300  {
301  return false;
302  }
303  }
304 
305  return true;
306 }
307 
308 void
309 PrimitiveFusion::findBoxPrimitives(
310  memoryx::EnvironmentalPrimitiveBaseList& primitives,
311  std::vector<memoryx::EntityBasePtr>& boxes,
312  memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment)
313 {
314  for (memoryx::EnvironmentalPrimitiveBasePtr& currentPrimitive : primitives)
315  {
316  memoryx::PlanePrimitivePtr plane =
317  memoryx::PlanePrimitivePtr::dynamicCast(currentPrimitive);
318  if (!plane)
319  {
320  continue;
321  }
322 
323  Eigen::Vector3f planeNormal = Vector3Ptr::dynamicCast(plane->getPlaneNormal())->toEigen();
324  float alpha = std::acos(planeNormal.dot(Eigen::Vector3f::UnitZ()));
325  if (alpha < pcl::deg2rad(15.0))
326  {
327  std::vector<memoryx::PlanePrimitivePtr> boxSides;
328  boxSides.push_back(plane);
329 
330  std::vector<memoryx::EnvironmentalPrimitiveBasePtr> intersectingPrimitives;
331  getIntersectingPrimitives(primitives, currentPrimitive, intersectingPrimitives, 50.0);
332 
333  for (memoryx::EnvironmentalPrimitiveBasePtr& i : intersectingPrimitives)
334  {
335  memoryx::PlanePrimitivePtr sideCandidate =
336  memoryx::PlanePrimitivePtr::dynamicCast(i);
337  if (!sideCandidate)
338  {
339  continue;
340  }
341  else if (plane->getPose()->position->z < sideCandidate->getPose()->position->z)
342  {
343  continue;
344  }
345 
346  bool isPerpendicular = true;
347  for (memoryx::PlanePrimitivePtr& side : boxSides)
348  {
349  Eigen::Vector3f sideNormal =
350  Vector3Ptr::dynamicCast(sideCandidate->getPlaneNormal())->toEigen();
351  Eigen::Vector3f otherNormal =
352  Vector3Ptr::dynamicCast(side->getPlaneNormal())->toEigen();
353 
354  float beta = std::acos(sideNormal.dot(otherNormal));
355 
356  if (std::abs(beta) < 0.01)
357  {
358  ARMARX_LOG_S << "found a parallel side. doing nothing at the moment";
359  // todo check if this is a possible opposite side;
360  isPerpendicular = false;
361  break;
362  }
363  else if (std::fabs(beta - M_PI_2) > pcl::deg2rad(5.0))
364  {
365  isPerpendicular = false;
366  break;
367  }
368  }
369  if (isPerpendicular)
370  {
371  boxSides.push_back(sideCandidate);
372  }
373  }
374 
375  if (boxSides.size() == 3)
376  {
377  memoryx::EntityRefList entityRefList;
378  memoryx::PointList graspPoints;
379  std::vector<Eigen::Vector3f> normals;
380  std::vector<Eigen::Vector3f> positions;
381  for (size_t i = 0; i < 3; i++)
382  {
383  memoryx::PlanePrimitivePtr side = boxSides[i];
384  normals.push_back(Vector3Ptr::dynamicCast(side->getPlaneNormal())->toEigen());
385  positions.push_back(
386  Vector3Ptr::dynamicCast(side->getPose()->position)->toEigen());
387 
388  memoryx::EntityRefBasePtr entityRef =
389  environmentalPrimitiveSegment->getEntityRefById(side->getId());
390  entityRefList.push_back(entityRef);
391 
392  for (armarx::Vector3BasePtr& g : side->getGraspPoints())
393  {
394  graspPoints.push_back(g);
395  }
396  }
397 
398 
399  Eigen::Matrix3f rot = Eigen::Matrix3f::Zero();
400  rot.col(0) = normals[0];
401  rot.col(1) = normals[1];
402  rot.col(2) = normals[0].cross(normals[1]);
403 
404  Eigen::Vector3f position = positions[0];
405  position(2) = positions[1](2);
406 
407  FramedPosePtr pose = new FramedPose(rot, position, armarx::GlobalFrame, "");
408 
409  Eigen::Vector3f dimE;
410  dimE(0) = normals[0].dot(positions[0] - position);
411  dimE(1) = normals[1].dot(positions[1] - position);
412  dimE(2) = normals[2].dot(positions[2] - position);
413  dimE = 2.0 * dimE.cwiseAbs();
414 
415  memoryx::BoxPrimitivePtr boxPrimitive = new memoryx::BoxPrimitive();
416  boxPrimitive->setBoxSides(entityRefList);
417  boxPrimitive->setGraspPoints(graspPoints);
418  boxPrimitive->setLabel(0);
419  boxPrimitive->setOBBDimensions(new Vector3(dimE));
420  boxPrimitive->setPose(pose);
421 
422  boxPrimitive->setProbability(1.0);
423  // for (memoryx::PlanePrimitivePtr& side : boxSides)
424  // {
425  //boxPrimitive->setProbability(boxPrimitive->getProbability(), side->getProbability());
426  // }
427 
428  boxPrimitive->setTime(boxSides[0]->getTime());
429  boxPrimitive->setName("box");
430 
431  boxes.push_back(boxPrimitive);
432 
433  bool mirrorSides = true;
434  if (mirrorSides)
435  {
436  for (size_t i = 1; i < 3; i++)
437  {
438  memoryx::PlanePrimitivePtr side = boxSides[i];
439  Eigen::Matrix4f p = FramedPosePtr::dynamicCast(side->getPose())->toEigen();
440  Eigen::Vector3f m = p.block<3, 3>(0, 0).transpose() * normals[i];
441 
442  Eigen::Vector3f mPosition = position - normals[i] * dimE(2);
443  FramedPosePtr mPose =
444  new FramedPose(p.block<3, 3>(0, 0), mPosition, armarx::GlobalFrame, "");
445 
446  memoryx::PointList graspPoints;
447  for (armarx::Vector3BasePtr& g : side->getGraspPoints())
448  {
449  Eigen::Vector4f point = Eigen::Vector4f::Identity();
450  point.head<3>() = Vector3Ptr::dynamicCast(g)->toEigen();
451 
452  point = p.inverse() * point;
453  point.head<3>() = point.head<3>() - m * dimE(i);
454  point = p * point;
455 
456  Eigen::Vector3f temp = point.head<3>();
457  graspPoints.push_back(new Vector3(temp));
458  }
459 
460 
461  memoryx::PlanePrimitivePtr mSide = new memoryx::PlanePrimitive();
462  mSide->setOBBDimensions(side->getOBBDimensions());
463  mSide->setGraspPoints(graspPoints);
464  mSide->setPose(mPose);
465  // todo rotate normal
466  mSide->setPlaneNormal(side->getPlaneNormal());
467  mSide->setLabel(0);
468  mSide->setTime(side->getTime());
469  mSide->setProbability(0.0);
470 
471  boxes.push_back(mSide);
472  }
473  }
474  }
475  }
476  }
477 }
armarx::PrimitiveFusion::~PrimitiveFusion
~PrimitiveFusion()
PrimitiveFusion Destructor.
Definition: PrimitiveFusion.cpp:36
armarx::PrimitiveFusion::testBoxIntersection
bool testBoxIntersection(Eigen::Matrix4f leftPose, Eigen::Matrix4f rightPose, Eigen::Vector3f leftExtent, Eigen::Vector3f rightExtent)
Definition: PrimitiveFusion.cpp:42
memoryx::VariantType::PlanePrimitive
const armarx::VariantTypeId PlanePrimitive
Definition: EnvironmentalPrimitive.h:38
armarx::PrimitiveFusion::PrimitiveFusion
PrimitiveFusion()
PrimitiveFusion Constructor.
Definition: PrimitiveFusion.cpp:32
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::PrimitiveFusion::getIntersectingPrimitives
void getIntersectingPrimitives(memoryx::EnvironmentalPrimitiveBaseList &primitives, memoryx::EnvironmentalPrimitiveBasePtr primitive, memoryx::EnvironmentalPrimitiveBaseList &intersectingPrimitives, float eps=0.0f)
Definition: PrimitiveFusion.cpp:113
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
IceInternal::Handle
Definition: forward_declarations.h:8
armarx::PrimitiveFusion::getIntersectingSimilarPrimitives
void getIntersectingSimilarPrimitives(memoryx::EnvironmentalPrimitiveBaseList &primitives, memoryx::EnvironmentalPrimitiveBasePtr primitive, memoryx::EnvironmentalPrimitiveBaseList &intersectingPrimitives, float eps=0.0f)
Definition: PrimitiveFusion.cpp:147
armarx::PrimitiveFusion::findBoxPrimitives
void findBoxPrimitives(memoryx::EnvironmentalPrimitiveBaseList &primitives, std::vector< memoryx::EntityBasePtr > &boxes, memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment)
Definition: PrimitiveFusion.cpp:309
PrimitiveFusion.h
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::PrimitiveFusion::isSimilar
bool isSimilar(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive, memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
Definition: PrimitiveFusion.cpp:259
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
M_PI
#define M_PI
Definition: MathTools.h:17
FramedPose.h
memoryx::VariantType::BoxPrimitive
const armarx::VariantTypeId BoxPrimitive
Definition: EnvironmentalPrimitive.h:46
ARMARX_LOG_S
#define ARMARX_LOG_S
Definition: Logging.h:145
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:772
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
norm
double norm(const Point &a)
Definition: point.hpp:102