GCC Code Coverage Report

Directory: ./
File: src/affordance-extraction.cc
Date: 2025-05-07 12:06:29
Exec Total Coverage
Lines: 80 175 45.7%
Branches: 69 316 21.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Anna Seppala
4 //
5 // This file is part of hpp-core
6 // hpp-core is free software: you can redistribute it
7 // and/or modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation, either version
9 // 3 of the License, or (at your option) any later version.
10 //
11 // hpp-core is distributed in the hope that it will be
12 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 // General Lesser Public License for more details. You should have
15 // received a copy of the GNU Lesser General Public License along with
16 // hpp-core If not, see
17 // <http://www.gnu.org/licenses/>.
18
19 #include <coal/BVH/BVH_model.h>
20 #include <coal/collision.h>
21 #include <coal/collision_object.h>
22
23 #include <algorithm>
24 #include <hpp/affordance/affordance-extraction.hh>
25 #include <hpp/affordance/operations.hh>
26
27 namespace hpp {
28 namespace affordance {
29
30 17 BVHModelOBConst_Ptr_t GetModel(FclConstCollisionObjectPtr_t object) {
31
2/4
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 17 times.
 17 assert(object->collisionGeometry()->getNodeType() == coal::BV_OBBRSS);
32 const BVHModelOBConst_Ptr_t model =
33 17 std::static_pointer_cast<const BVHModelOB>(object->collisionGeometry());
34
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 17 times.
 17 assert(model->getModelType() == coal::BVH_MODEL_TRIANGLES);
35 17 return model;
36 }
37
38 51 void searchLinkedTriangles(std::vector<unsigned int>& listPotential,
39 const OperationBasePtr_t& refOp,
40 const std::vector<Triangle>& allTris,
41 std::vector<unsigned int>& searchableTris,
42 const unsigned int& refTriIdx, double& area) {
43 // TODO: think of a better way of declaring margins?
44 51 const double margin = 1e-15;
45 51 const Triangle& refTri = allTris[refTriIdx];
46
1/2
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
 102 searchableTris.erase(
47
1/2
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
 51 std::remove(searchableTris.begin(), searchableTris.end(), refTriIdx),
48 51 searchableTris.end());
49
2/2
✓ Branch 1 taken 233 times.
✓ Branch 2 taken 51 times.
 284 for (unsigned int searchIdx = 0; searchIdx < allTris.size(); searchIdx++) {
50 std::vector<unsigned int>::iterator it =
51
1/2
✓ Branch 3 taken 233 times.
✗ Branch 4 not taken.
 233 std::find(searchableTris.begin(), searchableTris.end(), searchIdx);
52
2/2
✓ Branch 2 taken 197 times.
✓ Branch 3 taken 36 times.
 233 if (it == searchableTris.end()) {
53 197 continue;
54 }
55 36 std::vector<coal::Vec3f> refPoints;
56
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 refPoints.push_back(refTri.points.p1);
57
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 refPoints.push_back(refTri.points.p2);
58
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 refPoints.push_back(refTri.points.p3);
59
1/2
✓ Branch 0 taken 36 times.
✗ Branch 1 not taken.
 36 for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
60 36 const Triangle& searchTri = allTris[searchIdx];
61
2/4
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 36 times.
✗ Branch 6 not taken.
 36 if ((refPoints[vertIdx] - searchTri.points.p1).squaredNorm() < margin ||
62
1/8
✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 36 (refPoints[vertIdx] - searchTri.points.p2).squaredNorm() < margin ||
63
1/8
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 36 times.
✗ Branch 10 not taken.
 36 (refPoints[vertIdx] - searchTri.points.p3).squaredNorm() < margin) {
64
2/4
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 if (refOp->requirement(searchTri.normal)) {
65
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 if ((searchTri.normal - refTri.normal).squaredNorm() <
66
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 refOp->neighbouringTriangleMargin_) {
67 36 area += searchTri.area;
68
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 listPotential.push_back(searchIdx);
69
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 searchLinkedTriangles(listPotential, refOp, allTris, searchableTris,
70 searchIdx, area);
71 }
72 } else {
73 // if linked face does not fulfil global requirement, discard
74 ✗ searchableTris.erase(std::remove(searchableTris.begin(),
75 searchableTris.end(), searchIdx),
76 ✗ searchableTris.end());
77 }
78 36 break; // jump out of vertex loop if triangle already tested for
79 // affordance
80 }
81 }
82 36 }
83 51 }
84
85 17 SemanticsDataPtr_t affordanceAnalysis(FclConstCollisionObjectPtr_t colObj,
86 const OperationBases_t& opVec) {
87
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 BVHModelOBConst_Ptr_t model = GetModel(colObj);
88
89 17 std::vector<Triangle> triangles;
90 17 std::vector<unsigned int> unsetTriangles;
91 17 double totArea = .0;
92
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 std::vector<std::vector<unsigned int> > potentialAffordances(opVec.size());
93
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
 17 SemanticsDataPtr_t foundAffordances(new SemanticsData());
94
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 foundAffordances->affordances_.resize(opVec.size());
95
96
2/2
✓ Branch 1 taken 66 times.
✓ Branch 2 taken 17 times.
 83 for (int i = 0; i < model->num_tris; ++i) {
97
1/2
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
 66 TrianglePoints tri;
98 66 coal::Triangle coaltri = (*model->tri_indices)[i];
99
2/4
✓ Branch 6 taken 66 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 66 times.
✗ Branch 10 not taken.
 66 tri.p1 = colObj->getRotation() * (*model->vertices)[coaltri[0]] +
100
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 132 colObj->getTranslation();
101
2/4
✓ Branch 6 taken 66 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 66 times.
✗ Branch 10 not taken.
 66 tri.p2 = colObj->getRotation() * (*model->vertices)[coaltri[1]] +
102
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 132 colObj->getTranslation();
103
2/4
✓ Branch 6 taken 66 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 66 times.
✗ Branch 10 not taken.
 66 tri.p3 = colObj->getRotation() * (*model->vertices)[coaltri[2]] +
104
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 132 colObj->getTranslation();
105
106
2/4
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 66 times.
✗ Branch 5 not taken.
 66 triangles.push_back(Triangle(tri));
107 // save vector index of triangles and their quantity.
108
1/2
✓ Branch 1 taken 66 times.
✗ Branch 2 not taken.
 66 unsetTriangles.push_back(i);
109 }
110 17 std::vector<unsigned int> unseenTriangles;
111
2/2
✓ Branch 1 taken 66 times.
✓ Branch 2 taken 17 times.
 83 for (unsigned int triIdx = 0; triIdx < triangles.size(); triIdx++) {
112 // look for triangle in set of triangles not yet given an affordance:
113 std::vector<unsigned int>::iterator it =
114
1/2
✓ Branch 3 taken 66 times.
✗ Branch 4 not taken.
 66 std::find(unsetTriangles.begin(), unsetTriangles.end(), triIdx);
115
2/2
✓ Branch 2 taken 36 times.
✓ Branch 3 taken 30 times.
 66 if (it == unsetTriangles.end()) {
116 36 continue;
117 }
118 // set list of searchable (unseen) triangles equal to all triangles not yet
119 // given an affordance.
120
1/2
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
 30 unseenTriangles = unsetTriangles;
121
2/2
✓ Branch 1 taken 52 times.
✓ Branch 2 taken 15 times.
 67 for (unsigned int opIdx = 0; opIdx < opVec.size(); opIdx++) {
122
3/4
✓ Branch 4 taken 52 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 15 times.
✓ Branch 7 taken 37 times.
 52 if (opVec[opIdx]->requirement(triangles[triIdx].normal)) {
123 15 totArea += triangles[triIdx].area;
124
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 potentialAffordances[opIdx].push_back(triIdx);
125
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 searchLinkedTriangles(potentialAffordances[opIdx], opVec[opIdx],
126 triangles, unseenTriangles, triIdx, totArea);
127
1/2
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
 15 if (totArea > opVec[opIdx]->minArea_) {
128 // save totArea for further use as well?
129 AffordancePtr_t aff(
130
3/6
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 15 times.
✗ Branch 9 not taken.
 15 new Affordance(potentialAffordances[opIdx], colObj));
131
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 foundAffordances->affordances_[opIdx].push_back(aff);
132 66 for (unsigned int removeIdx = 0;
133
2/2
✓ Branch 2 taken 51 times.
✓ Branch 3 taken 15 times.
 66 removeIdx < potentialAffordances[opIdx].size(); removeIdx++) {
134
1/2
✓ Branch 3 taken 51 times.
✗ Branch 4 not taken.
 51 std::remove(unsetTriangles.begin(), unsetTriangles.end(),
135 51 potentialAffordances[opIdx][removeIdx]);
136 51 unsetTriangles.pop_back();
137 }
138 // potentialAffordances [opIdx].clear ();
139 15 }
140 15 potentialAffordances[opIdx].clear();
141 15 totArea = .0;
142 15 break;
143
2/2
✓ Branch 1 taken 15 times.
✓ Branch 2 taken 22 times.
 37 } else if (opIdx >= opVec.size() - 1) {
144 // delete triangle if it did not fulfil any requirements
145
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 std::remove(unsetTriangles.begin(), unsetTriangles.end(), triIdx);
146 15 unsetTriangles.pop_back();
147 }
148 }
149 }
150 34 return foundAffordances;
151 17 }
152
153 ✗ Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& from,
154 const Eigen::Vector3d& to) {
155 ✗ Eigen::Quaterniond quat;
156 ✗ quat.setFromTwoVectors(from, to);
157 ✗ return quat.toRotationMatrix();
158 }
159
160 ✗ Eigen::Matrix3d GetRotationMatrix(const std::vector<coal::Vec3f>& points) {
161 ✗ assert(points.size() > 2);
162 ✗ coal::Vec3f z(0., 0., 1.);
163 ✗ coal::Vec3f normal = (points[1] - points[0]).cross(points[2] - points[0]);
164 ✗ return GetRotationMatrix(normal, z);
165 }
166
167 typedef coal::Vec3f Point;
168 typedef std::vector<Point> T_Point;
169 typedef T_Point::const_iterator CIT_Point;
170
171 ✗ std::vector<coal::Vec3f> projectPoints(const Eigen::Matrix3d& project,
172 const std::vector<coal::Vec3f>& points) {
173 ✗ std::vector<coal::Vec3f> res;
174 ✗ for (CIT_Point cit = points.begin(); cit != points.end(); ++cit)
175 ✗ res.push_back(project * (*cit));
176 ✗ return res;
177 }
178
179 ✗ coal::Vec3f computeCentroid(const std::vector<coal::Vec3f>& points) {
180 ✗ coal::Vec3f acc(0., 0., 0.);
181 ✗ for (CIT_Point cit = points.begin(); cit != points.end(); ++cit)
182 ✗ acc += (*cit);
183 ✗ return acc / points.size();
184 }
185
186 ✗ std::vector<coal::Vec3f> translatePoints(const std::vector<coal::Vec3f>& points,
187 const double reduceSize) {
188 ✗ coal::Vec3f centroid = computeCentroid(points);
189 ✗ coal::Vec3f dir;
190 ✗ std::vector<coal::Vec3f> res;
191 ✗ for (CIT_Point cit = points.begin(); cit != points.end(); ++cit) {
192 ✗ dir = centroid - (*cit);
193 ✗ dir.normalize();
194 ✗ res.push_back((*cit) + dir * reduceSize);
195 }
196 ✗ return res;
197 }
198
199 ✗ void createAffordanceModel(hpp::affordance::AffordancePtr_t affPtr,
200 const std::vector<coal::Vec3f>& vertices,
201 const std::vector<coal::Triangle>& triangles,
202 const unsigned int affIdx,
203 std::vector<CollisionObjects_t>& affObjs) {
204 ✗ BVHModelOB_Ptr_t model1(new BVHModelOB());
205 // add the mesh data into the BVHModel structure
206 ✗ model1->beginModel();
207 ✗ model1->addSubModel(vertices, triangles);
208 ✗ model1->endModel();
209 // create affordance collision object from created affModel and
210 // tranformation of corresponding reference collision object.
211 coal::CollisionObjectPtr_t obj(
212 ✗ new coal::CollisionObject(model1, affPtr->colObj_->getTransform()));
213 ✗ affObjs[affIdx].push_back(obj);
214 }
215
216 ✗ void addTriangles(hpp::affordance::AffordancePtr_t affPtr,
217 BVHModelOBConst_Ptr_t model,
218 std::vector<std::size_t>& triIndices,
219 const unsigned int triIdx, std::vector<coal::Vec3f>& vertices,
220 std::vector<coal::Triangle>& triangles) {
221 // give triangles of new object new vertex indices (start from 0
222 // and go up to 3*nbTris - 1 [all tris have 3 unique indices])
223 ✗ std::vector<std::size_t> triPoints;
224 const coal::Triangle& refTri =
225 ✗ (*model->tri_indices)[affPtr->indices_[triIdx]];
226 // triangles.push_back (refTri);
227 ✗ for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
228 std::vector<std::size_t>::iterator it =
229 ✗ std::find(triIndices.begin(), triIndices.end(), refTri[vertIdx]);
230 ✗ if (it == triIndices.end()) {
231 ✗ triIndices.push_back(refTri[vertIdx]);
232 ✗ vertices.push_back((*model->vertices)[refTri[vertIdx]]);
233 ✗ triPoints.push_back(std::size_t(vertices.size() - 1));
234 } else {
235 ✗ triPoints.push_back(it - triIndices.begin());
236 }
237 }
238 ✗ if (triPoints.size() != 3) {
239 ✗ std::ostringstream oss("wrong number of vertex indices in triangle!!");
240 ✗ throw std::runtime_error(oss.str());
241 }
242 ✗ triangles.push_back(coal::Triangle(triPoints[0], triPoints[1], triPoints[2]));
243 }
244
245 ✗ std::vector<CollisionObjects_t> getAffordanceObjects(
246 const SemanticsDataPtr_t& sData) {
247 ✗ std::vector<CollisionObjects_t> affObjs;
248 ✗ for (unsigned int affIdx = 0; affIdx < sData->affordances_.size(); affIdx++) {
249 ✗ std::vector<coal::CollisionObjectPtr_t> objVec;
250 ✗ affObjs.push_back(objVec);
251 // get number of affordances of specific type (lean OR support etc)
252 // this corresponds to number of objects to be created for specific aff type
253 ✗ long unsigned int len = sData->affordances_[affIdx].size();
254 ✗ for (unsigned int idx = 0; idx < len; idx++) {
255 ✗ std::vector<coal::Vec3f> vertices;
256 ✗ std::vector<coal::Triangle> triangles;
257 ✗ std::vector<std::size_t> triIndices;
258 hpp::affordance::AffordancePtr_t affPtr =
259 ✗ sData->affordances_[affIdx][idx];
260 ✗ BVHModelOBConst_Ptr_t model = GetModel(affPtr->colObj_);
261 ✗ for (unsigned int triIdx = 0; triIdx < affPtr->indices_.size(); triIdx++)
262 ✗ addTriangles(affPtr, model, triIndices, triIdx, vertices, triangles);
263 ✗ createAffordanceModel(affPtr, vertices, triangles, affIdx, affObjs);
264 }
265 }
266 ✗ return affObjs;
267 }
268
269 // same as getAffordanceObject but apply a reduction of a given size on the mesh
270 ✗ std::vector<CollisionObjects_t> getReducedAffordanceObjects(
271 const SemanticsDataPtr_t& sData, std::vector<double> reduceSizes) {
272 ✗ if (reduceSizes.size() != sData->affordances_.size()) {
273 std::ostringstream oss(
274 "Error : the vector 'reduceSizes' must have one element per affordance "
275 ✗ "types");
276 ✗ throw std::runtime_error(oss.str());
277 }
278 ✗ std::vector<CollisionObjects_t> affObjs;
279 ✗ affObjs.clear();
280 ✗ for (unsigned int affIdx = 0; affIdx < sData->affordances_.size(); affIdx++) {
281 ✗ std::vector<coal::CollisionObjectPtr_t> objVec;
282 ✗ affObjs.push_back(objVec);
283 // get number of affordances of specific type (lean OR support etc)
284 // this corresponds to number of objects to be created for specific aff type
285 ✗ long unsigned int len = sData->affordances_[affIdx].size();
286 ✗ for (unsigned int idx = 0; idx < len; idx++) {
287 ✗ std::vector<coal::Vec3f> vertices;
288 ✗ std::vector<coal::Triangle> triangles;
289 ✗ std::vector<std::size_t> triIndices;
290 hpp::affordance::AffordancePtr_t affPtr =
291 ✗ sData->affordances_[affIdx][idx];
292 ✗ BVHModelOBConst_Ptr_t model = GetModel(affPtr->colObj_);
293 ✗ for (unsigned int triIdx = 0; triIdx < affPtr->indices_.size(); triIdx++)
294 ✗ addTriangles(affPtr, model, triIndices, triIdx, vertices, triangles);
295
296 ✗ if (reduceSizes[affIdx] > std::numeric_limits<double>::epsilon()) {
297 // std::cout<<"For points of affordance : "<<std::endl;
298 ✗ for (unsigned int id = 0; id < vertices.size(); ++id)
299 ✗ std::cout << "( " << vertices[id][0] << " , " << vertices[id][1]
300 ✗ << " , " << vertices[id][2] << " ) " << std::endl;
301
302 // first we need to sort the vertices in clockwise order.
303 // but as we cannot change the order in the 'vertices' vector we use
304 // another vector for correspondance of index first getTransformation
305 // matrix to project triangles in a 2d plane
306 ✗ Eigen::Matrix3d project = GetRotationMatrix(vertices);
307 std::vector<coal::Vec3f> projectedVertices =
308 ✗ projectPoints(project, vertices);
309
310 // now translate each point by the reducesize factor in the direction
311 // towards the centroid
312 std::vector<coal::Vec3f> translatedVertices =
313 ✗ translatePoints(projectedVertices, reduceSizes[affIdx]);
314 ✗ vertices = projectPoints(project.transpose(), translatedVertices);
315 }
316 ✗ createAffordanceModel(affPtr, vertices, triangles, affIdx, affObjs);
317 }
318 }
319 ✗ return affObjs;
320 }
321
322 } // namespace affordance
323 } // namespace hpp
324