grasp
boundingBox.cpp
1 /* Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
2  * Authors: Ilaria Gori
3  * email: ilaria.gori@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found in the file LICENSE located in the
9  * root directory.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15 */
16 
17 #include <iCub/data3D/private/helpers.h>
18 #include <iCub/data3D/boundingBox.h>
19 
20 using namespace std;
21 using namespace yarp::sig;
22 using namespace yarp::os;
23 using namespace yarp::math;
24 using namespace iCub::data3D;
25 
26 /************************************************************************/
27 BoundingBox::BoundingBox(const iCub::data3D::Box3D &boundingBox)
28 {
29  this->boundingBox.corners=boundingBox.corners;
30  this->boundingBox.orientation=boundingBox.orientation;
31 }
32 
33 /************************************************************************/
34 iCub::data3D::Box3D BoundingBox::getBoundingBox()
35 {
36  return this->boundingBox;
37 }
38 
39 /************************************************************************/
40 void BoundingBox::setBoundingBox(const iCub::data3D::Box3D &boundingBox)
41 {
42  this->boundingBox.corners=boundingBox.corners;
43  this->boundingBox.orientation=boundingBox.orientation;
44 }
45 
46 /************************************************************************/
47 std::vector<iCub::data3D::PointXYZ> BoundingBox::getCorners()
48 {
49  return this->boundingBox.corners;
50 }
51 
52 /************************************************************************/
53 void BoundingBox::setCorners(const std::vector<iCub::data3D::PointXYZ> &corners)
54 {
55  this->boundingBox.corners=corners;
56 }
57 
58 /************************************************************************/
59 yarp::sig::Matrix BoundingBox::getOrientation()
60 {
61  return this->boundingBox.orientation;
62 }
63 
64 /************************************************************************/
65 void BoundingBox::setOrientation(const yarp::sig::Matrix &orientation)
66 {
67  this->boundingBox.orientation=orientation;
68 }
69 
70 /************************************************************************/
71 Vector BoundingBox::findIndexes(const yarp::sig::Matrix &corner_i)
72 {
73  Vector indexes(3);
74  yarp::sig::Vector point1(3); point1[0]=corner_i(0,0); point1[1]=corner_i(0,1); point1[2]=corner_i(0,2);
75 
76  int m=0;
77  for (int i=0; i<3; i++)
78  {
79  for (int j=0; j<3; j++)
80  {
81  if (j<=i)
82  continue;
83  int index=-1;
84  double minimum=10000;
85  for (int t=1; t<corner_i.rows(); t++)
86  {
87  yarp::sig::Vector tmp(3); tmp[0]=corner_i(t,0); tmp[1]=corner_i(t,1); tmp[2]=corner_i(t,2);
88  double value=norm(Helpers::extractSubVector(point1,i,j)-Helpers::extractSubVector(tmp,i,j));
89  if (value<minimum)
90  {
91  index=t;
92  minimum=value;
93  }
94  }
95  indexes[m]=index;
96  m+=1;
97  }
98  }
99  return indexes;
100 }
101 
102 /************************************************************************/
103 Matrix BoundingBox::convertCorners()
104 {
105  Matrix cornerpoints(boundingBox.corners.size(), 3);
106 
107  for (int i=0; i<boundingBox.corners.size(); i++)
108  {
109  cornerpoints(i,0)=boundingBox.corners[i].x;
110  cornerpoints(i,1)=boundingBox.corners[i].y;
111  cornerpoints(i,2)=boundingBox.corners[i].z;
112  }
113 
114  Matrix corner_i=cornerpoints*boundingBox.orientation;
115  return corner_i;
116 }
117 
118 /************************************************************************/
119 void BoundingBox::getAxis(yarp::sig::Vector &x, yarp::sig::Vector &y, yarp::sig::Vector &z)
120 {
121  Matrix corner_i=convertCorners();
122  yarp::sig::Vector indexes=findIndexes(corner_i);
123 
124  yarp::sig::Vector point1(3); point1[0]=corner_i(0,0); point1[1]=corner_i(0,1); point1[2]=corner_i(0,2);
125 
126  yarp::sig::Vector index0(3); index0[0]=corner_i(indexes[0],0); index0[1]=corner_i(indexes[0],1); index0[2]=corner_i(indexes[0],2);
127  yarp::sig::Vector index1(3); index1[0]=corner_i(indexes[1],0); index1[1]=corner_i(indexes[1],1); index1[2]=corner_i(indexes[1],2);
128  yarp::sig::Vector index2(3); index2[0]=corner_i(indexes[2],0); index2[1]=corner_i(indexes[2],1); index2[2]=corner_i(indexes[2],2);
129 
130  yarp::sig::Vector vectz=point1-index0;
131  z=vectz*boundingBox.orientation.transposed();
132  yarp::sig::Vector vecty=point1-index1;
133  y=vecty*boundingBox.orientation.transposed();
134  yarp::sig::Vector vectx=point1-index2;
135  x=vectx*boundingBox.orientation.transposed();
136 }
137 
138 /************************************************************************/
139 Vector BoundingBox::getDim()
140 {
141  Matrix corner_i=convertCorners();
142  yarp::sig::Vector indexes=findIndexes(corner_i);
143 
144  yarp::sig::Vector point1(3); point1[0]=corner_i(0,0); point1[1]=corner_i(0,1); point1[2]=corner_i(0,2);
145 
146  Vector dim(3);
147  dim[2]=max(point1[2],corner_i(indexes[0],2))-min(point1[2],corner_i(indexes[0],2));
148  dim[1]=max(point1[1],corner_i(indexes[1],1))-min(point1[1],corner_i(indexes[1],1));
149  dim[0]=max(point1[0],corner_i(indexes[2],0))-min(point1[0],corner_i(indexes[2],0));
150 
151  return dim;
152 }
153 
154 /************************************************************************/
155 Vector BoundingBox::getCenter()
156 {
157  Matrix corner_i=convertCorners();
158  yarp::sig::Vector indexes=findIndexes(corner_i);
159 
160  yarp::sig::Vector point1(3); point1[0]=corner_i(0,0); point1[1]=corner_i(0,1); point1[2]=corner_i(0,2);
161 
162  yarp::sig::Vector index0(3); index0[0]=corner_i(indexes[0],0); index0[1]=corner_i(indexes[0],1); index0[2]=corner_i(indexes[0],2);
163  yarp::sig::Vector index1(3); index1[0]=corner_i(indexes[1],0); index1[1]=corner_i(indexes[1],1); index1[2]=corner_i(indexes[1],2);
164  yarp::sig::Vector index2(3); index2[0]=corner_i(indexes[2],0); index2[1]=corner_i(indexes[2],1); index2[2]=corner_i(indexes[2],2);
165 
166  yarp::sig::Vector center(3);
167  center[2]=(point1[2]+index0[2])/2;
168  center[1]=(point1[1]+index1[1])/2;
169  center[0]=(point1[0]+index2[0])/2;
170 
171  yarp::sig::Vector centerroot=center*boundingBox.orientation.transposed();
172  return centerroot;
173 }
174 
175 /************************************************************************/
176 void BoundingBox::drawBoundingBox(boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer, int viewport)
177 {
178  pcl::PointXYZRGB point1;
179  point1.x=boundingBox.corners[0].x;
180  point1.y=boundingBox.corners[0].y;
181  point1.z=boundingBox.corners[0].z;
182  point1.r=255;
183  point1.g=0;
184  point1.b=0;
185  pcl::PointXYZRGB point2;
186  point2.x=boundingBox.corners[1].x;
187  point2.y=boundingBox.corners[1].y;
188  point2.z=boundingBox.corners[1].z;
189  point2.r=255;
190  point2.g=0;
191  point2.b=0;
192  pcl::PointXYZRGB point3;
193  point3.x=boundingBox.corners[2].x;
194  point3.y=boundingBox.corners[2].y;
195  point3.z=boundingBox.corners[2].z;
196  point3.r=255;
197  point3.g=0;
198  point3.b=0;
199  pcl::PointXYZRGB point4;
200  point4.x=boundingBox.corners[3].x;
201  point4.y=boundingBox.corners[3].y;
202  point4.z=boundingBox.corners[3].z;
203  point4.r=255;
204  point4.g=0;
205  point4.b=0;
206  pcl::PointXYZRGB point5;
207  point5.x=boundingBox.corners[4].x;
208  point5.y=boundingBox.corners[4].y;
209  point5.z=boundingBox.corners[4].z;
210  point5.r=255;
211  point5.g=0;
212  point5.b=0;
213  pcl::PointXYZRGB point6;
214  point6.x=boundingBox.corners[5].x;
215  point6.y=boundingBox.corners[5].y;
216  point6.z=boundingBox.corners[5].z;
217  point6.r=255;
218  point6.g=0;
219  point6.b=0;
220  pcl::PointXYZRGB point7;
221  point7.x=boundingBox.corners[6].x;
222  point7.y=boundingBox.corners[6].y;
223  point7.z=boundingBox.corners[6].z;
224  point7.r=255;
225  point7.g=0;
226  point7.b=0;
227  pcl::PointXYZRGB point8;
228  point8.x=boundingBox.corners[7].x;
229  point8.y=boundingBox.corners[7].y;
230  point8.z=boundingBox.corners[7].z;
231  point8.r=255;
232  point8.g=0;
233  point8.b=0;
234 
235  Vector centerroot=getCenter();
236  Vector vectxroot,vectyroot,vectzroot;
237  getAxis(vectxroot,vectyroot,vectzroot);
238 
239  pcl::PointXYZ z1;
240  z1.x=centerroot[0]+(vectzroot[0]/2);
241  z1.y=centerroot[1]+(vectzroot[1]/2);
242  z1.z=centerroot[2]+(vectzroot[2]/2);
243 
244  pcl::PointXYZ z2;
245  z2.x=centerroot[0]+(-vectzroot[0]/2);
246  z2.y=centerroot[1]+(-vectzroot[1]/2);
247  z2.z=centerroot[2]+(-vectzroot[2]/2);
248 
249  pcl::PointXYZ y1;
250  y1.x=centerroot[0]+(vectyroot[0]/2);
251  y1.y=centerroot[1]+(vectyroot[1]/2);
252  y1.z=centerroot[2]+(vectyroot[2]/2);
253 
254  pcl::PointXYZ y2;
255  y2.x=centerroot[0]+(-vectyroot[0]/2);
256  y2.y=centerroot[1]+(-vectyroot[1]/2);
257  y2.z=centerroot[2]+(-vectyroot[2]/2);
258 
259  pcl::PointXYZ x1;
260  x1.x=centerroot[0]+(vectxroot[0]/2);
261  x1.y=centerroot[1]+(vectxroot[1]/2);
262  x1.z=centerroot[2]+(vectxroot[2]/2);
263 
264  pcl::PointXYZ x2;
265  x2.x=centerroot[0]+(-vectxroot[0]/2);
266  x2.y=centerroot[1]+(-vectxroot[1]/2);
267  x2.z=centerroot[2]+(-vectxroot[2]/2);
268 
269  pcl::PointXYZ center(centerroot[0],centerroot[1],centerroot[2]);
270 
271  if (viewport==0)
272  {
273  viewer->addLine<pcl::PointXYZRGB>(point1,point2,"line1");
274  viewer->addLine<pcl::PointXYZRGB>(point2,point3,"line2");
275  viewer->addLine<pcl::PointXYZRGB>(point3,point4,"line3");
276  viewer->addLine<pcl::PointXYZRGB>(point4,point1,"line4");
277  viewer->addLine<pcl::PointXYZRGB>(point5,point6,"line5");
278  viewer->addLine<pcl::PointXYZRGB>(point6,point7,"line6");
279  viewer->addLine<pcl::PointXYZRGB>(point7,point8,"line7");
280  viewer->addLine<pcl::PointXYZRGB>(point8,point5,"line8");
281  viewer->addLine<pcl::PointXYZRGB>(point1,point5,"line9");
282  viewer->addLine<pcl::PointXYZRGB>(point2,point6,"line10");
283  viewer->addLine<pcl::PointXYZRGB>(point3,point7,"line11");
284  viewer->addLine<pcl::PointXYZRGB>(point4,point8,"line12");
285 
286  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(z1,center,1,1,1,1,"z1");
287  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(z2,center,1,1,1,1,"z2");
288  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(y1,center,1,1,1,1,"y1");
289  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(y2,center,1,1,1,1,"y2");
290  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(x1,center,1,1,1,1,"x1");
291  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(x2,center,1,1,1,1,"x2");
292  }
293  else
294  {
295  viewer->addLine<pcl::PointXYZRGB>(point1,point2,"line1",viewport);
296  viewer->addLine<pcl::PointXYZRGB>(point2,point3,"line2",viewport);
297  viewer->addLine<pcl::PointXYZRGB>(point3,point4,"line3",viewport);
298  viewer->addLine<pcl::PointXYZRGB>(point4,point1,"line4",viewport);
299  viewer->addLine<pcl::PointXYZRGB>(point5,point6,"line5",viewport);
300  viewer->addLine<pcl::PointXYZRGB>(point6,point7,"line6",viewport);
301  viewer->addLine<pcl::PointXYZRGB>(point7,point8,"line7",viewport);
302  viewer->addLine<pcl::PointXYZRGB>(point8,point5,"line8",viewport);
303  viewer->addLine<pcl::PointXYZRGB>(point1,point5,"line9",viewport);
304  viewer->addLine<pcl::PointXYZRGB>(point2,point6,"line10",viewport);
305  viewer->addLine<pcl::PointXYZRGB>(point3,point7,"line11",viewport);
306  viewer->addLine<pcl::PointXYZRGB>(point4,point8,"line12",viewport);
307 
308  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(z1,center,0,0,1,1,"z1",viewport);
309  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(z2,center,0,0,1,1,"z2",viewport);
310  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(y1,center,0,1,0,1,"y1",viewport);
311  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(y2,center,0,1,0,1,"y2",viewport);
312  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(x1,center,1,0,0,1,"x1",viewport);
313  viewer->addArrow<pcl::PointXYZ,pcl::PointXYZ>(x2,center,1,0,0,1,"x2",viewport);
314  }
315 }
316 
317 
Generated by   doxygen 1.8.12