GCC Code Coverage Report

Directory: ./
File: include/hpp/affordance/affordance-extraction.hh
Date: 2025-03-07 12:08:39
Exec Total Coverage
Lines: 18 18 100.0%
Branches: 5 10 50.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016 CNRS
3 // Authors: Anna Seppala
4 //
5 // This file is part of hpp-affordance
6 // hpp-affordance 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-affordance 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-affordance If not, see
17 // <http://www.gnu.org/licenses/>.
18
19 #ifndef HPP_AFFORDANCE_AFFORDANCE_EXTRACTION_HH
20 #define HPP_AFFORDANCE_AFFORDANCE_EXTRACTION_HH
21
22 #include <hpp/affordance/config.hh>
23 #include <hpp/affordance/fwd.hh>
24
25 namespace hpp {
26 namespace affordance {
27
28 /// Helper struct that saves the global position of the triangle
29 /// vertices of a coal::Triangle.
30 struct TrianglePoints {
31 coal::Vec3f p1, p2, p3;
32 };
33 /// Helper class to save triangle information.
34 struct Triangle {
35 Triangle() {}
36 /// Constructor that takes in a TrianglePoints object.
37 66 Triangle(const TrianglePoints& inPoints) : points(inPoints) {
38 66 TriangleArea(points);
39 66 TriangleNormal(points);
40 66 }
41 /// Computes the area of a triangle.
42 /// \param tri The global position of a triangles vertices
43 66 void TriangleArea(TrianglePoints& tri) {
44 double a, b, c;
45
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 66 a = (tri.p1 - tri.p2).norm();
46
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 66 b = (tri.p2 - tri.p3).norm();
47
1/2
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
 66 c = (tri.p3 - tri.p1).norm();
48 66 double s = 0.5 * (a + b + c);
49 66 area = sqrt(s * (s - a) * (s - b) * (s - c));
50 66 }
51 /// Computes the normal vector of a triangle based on the
52 /// global position of its vertices. The normal is subject to convention!
53 /// \param tri The global position of a triangles vertices
54 66 void TriangleNormal(TrianglePoints& tri) {
55
2/4
✓ Branch 2 taken 66 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 66 times.
✗ Branch 6 not taken.
 66 normal = (tri.p2 - tri.p1).cross(tri.p3 - tri.p1);
56 66 normal.normalize();
57 66 }
58 /// The global position of a triangles vertices.
59 TrianglePoints points;
60 /// The area of a triangle.
61 double area;
62 /// The normal vector of a triangle.
63 coal::Vec3f normal;
64 };
65
66 // helper function to extract mesh model of an coal::collisionObstacle
67 BVHModelOBConst_Ptr_t GetModel(FclConstCollisionObjectPtr_t object);
68
69 /// \addtogroup affordance
70 /// \{
71
72 /// Class that saves a reference collision object and indices to
73 /// those of its mesh triangles that form one affordance object.
74 /// This information will later be used to create coal::collisionObjects
75 /// for each individual affordance.
76 class Affordance {
77 public:
78 Affordance() {}
79 /// Constructor for an Affordance object
80 ///
81 /// \param idxVec vector of triangle indices corresponding to
82 /// the mesh model within the reference coal::collisionObject
83 /// \param colObj reference to pointer to coal::collisionObject
84 /// containing the found affordance objects.
85 15 Affordance(const std::vector<unsigned int>& idxVec,
86 FclConstCollisionObjectPtr_t colObj)
87 15 : indices_(idxVec), colObj_(colObj) {}
88 /// Triangle indices that correspond to the triangles of the reference
89 /// collisionObstacle, and form the affordance object.
90 std::vector<unsigned int> indices_;
91 /// Reference to the collisionObstacle the surfaces of which are used to
92 /// create the affordance object.
93 FclConstCollisionObjectPtr_t colObj_;
94 };
95 /// Class containing a vector of vectors of Affordance objects.
96 /// The length of the outer vector corresponds to the amount of different
97 /// affordance types found, and that of the inner vector to the amount
98 /// of affordance objects found for each affordance type.
99 class SemanticsData {
100 public:
101 17 SemanticsData() {}
102 /// Vector of vectors of Affordance objects. The size of the outer
103 /// vector corresponds to the number of affordance types, and that
104 /// of the inner vector to the amount of individual affordance objects
105 /// for each affordance type.
106 std::vector<std::vector<AffordancePtr_t> > affordances_;
107
108 private:
109 SemanticsData(const SemanticsData&); // Prevent copy-construction
110 SemanticsData& operator=(const SemanticsData&);
111 };
112
113 /// Free function that searches through a vector of mesh triangles
114 /// and saves the triangle indices that form a potential affordance
115 /// object. Given a reference triangle index that fullfils the affordance
116 /// requirements of a given affordance type, the function recursively goes
117 /// all triangles that ara linked to the the reference. Also saves the
118 /// through total area of found potential affordance.
119 ///
120 /// \param listPotential reference to the vector of triangle indices
121 /// that form one potential affordance object. At
122 /// every recursive
123 /// step maximum one triangle is added to the vector.
124 /// \param refOp operation that determines which affordance type the
125 /// triangles will be tested for.
126 /// \param allTris all triangles of a given coal::collisionObject mesh.
127 /// This parameter is only used as to verify global
128 ///(affordance type)
129 /// and local (neighbouring triangles) requirements, and is
130 /// not
131 /// modified within the function.
132 /// \param searchableTris vector of triangle indices that should be tested
133 /// in the search for more triangles. Whenever a triangle
134 /// is
135 /// found, it is deleted from the vector and will not be
136 /// tested again.
137 /// Similarly, if a triangle is tested once and
138 /// does not
139 /// fullfil the global requirement set by the
140 /// affordance
141 /// type, it is deleted and will not be tested again in
142 /// subsequent recursive steps. \param refTriIdx index corresponding to the last
143 /// found triangle that
144 /// fullfils both the local and the global
145 /// requirement. It
146 /// is then used as reference in the following
147 /// recursive step. \param area total area of all triangles that are part of the
148 /// potential
149 /// affordance object. Every time a triangle
150 /// fulfilling all
151 /// set requirements is found, its are is added
152 /// to the previous total before going to the
153 /// next recursive step.
154 void searchLinkedTriangles(std::vector<unsigned int>& listPotential,
155 const OperationBasePtr_t& refOp,
156 const std::vector<Triangle>& allTris,
157 std::vector<unsigned int>& searchableTris,
158 const unsigned int& refTriIdx, double& area);
159
160 /// Free function that extracts all affordances (of all types) from a given
161 /// coal::collisionObject.
162 ///
163 /// \param colObj reference to a coal::collisionObject pointer the triangles
164 /// of which will be searched for affordance
165 /// objects.
166 /// \param opVec vector of operation objects that determine which requirements
167 /// are set for which affordance type. The length
168 /// of this vector corresponds to the amount of
169 /// different
170 /// affordance types considered.
171 SemanticsDataPtr_t affordanceAnalysis(FclConstCollisionObjectPtr_t colObj,
172 const OperationBases_t& opVec);
173
174 /// Free function that, given a semanticsData pointer, creates one
175 /// coal::collisionObject for every Affordance object.
176 ///
177 /// \param sData reference to all found Affordance objects.
178 std::vector<CollisionObjects_t> getAffordanceObjects(
179 const SemanticsDataPtr_t& sData);
180
181 /// Free function that, given a semanticsData pointer, creates one
182 /// coal::collisionObject for every Affordance object.
183 /// The object are reduced of \param reduceSize : each vertice are moved toward
184 /// the center of this value
185 ///
186 /// \param sData reference to all found Affordance objects.
187 /// NOTE : if there is changes in affordance.impl:: Afford::createOperations (),
188 /// you should be carefull to the index of the affordance type. The order of the
189 /// values in reduceSizes correspond to the order of the affordance type in
190 /// affordance.impl:: Afford::createOperations ()
191 /// FIXME : not working with plane that contain the z axis
192 std::vector<CollisionObjects_t> getReducedAffordanceObjects(
193 const SemanticsDataPtr_t& sData, std::vector<double> reduceSizes);
194
195 /// \}
196 } // namespace affordance
197 } // namespace hpp
198
199 #endif // HPP_AFFORDANCE_AFFORDANCE_EXTRACTION_HH
200