affordance-extraction.cc 9.26 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//
// Copyright (c) 2016 CNRS
// Authors: Anna Seppala
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/affordance/affordance-extraction.hh>
20
#include <hpp/affordance/operations.hh>
Akseppal's avatar
Akseppal committed
21
#include <hpp/fcl/collision_object.h>
22
#include <hpp/fcl/collision.h>
23
#include <hpp/fcl/BVH/BVH_model.h>
Akseppal's avatar
Akseppal committed
24
25
#include <algorithm>

26
27
28
29

namespace hpp {
  namespace affordance {

Steve Tonneau's avatar
Steve Tonneau committed
30
    BVHModelOBConst_Ptr_t GetModel (const fcl::CollisionObjectConstPtr_t& object)
31
32
    {
        assert (object->collisionGeometry ()->getNodeType () == fcl::BV_OBBRSS);
33
        const BVHModelOBConst_Ptr_t model = boost::static_pointer_cast<const BVHModelOB>
34
35
36
37
38
                                            (object->collisionGeometry ());
        assert (model->getModelType () == fcl::BVH_MODEL_TRIANGLES);
        return model;
    }

39
40
    void searchLinkedTriangles(std::vector<unsigned int>& listPotential, const OperationBasePtr_t& refOp,
                               const std::vector<Triangle>& allTris, std::vector<unsigned int>& searchableTris,
41
42
                               const unsigned int& refTriIdx, double& area)
    {
Akseppal's avatar
Akseppal committed
43
			// TODO: think of a better way of declaring margins?
Steve Tonneau's avatar
Steve Tonneau committed
44
      // STEVE: create a struct Configuration with all those params, and use it as a parameter
Akseppal's avatar
Akseppal committed
45
46
      const double marginRad = 0.3;
      const double margin = 1e-15;
Steve Tonneau's avatar
Steve Tonneau committed
47
48
      const Triangle& refTri = allTris[refTriIdx]; // STEVE no need to copy object
      searchableTris.erase(std::remove(searchableTris.begin(), searchableTris.end(), refTriIdx), searchableTris.end());
49
50
51
52
      for (unsigned int searchIdx = 0; searchIdx < allTris.size (); searchIdx++) {
        std::vector<unsigned int>::iterator it = std::find (searchableTris.begin (),
                                                            searchableTris.end (), searchIdx);
          if (it == searchableTris.end ()) {
53
            continue;
Akseppal's avatar
Akseppal committed
54
          }
Akseppal's avatar
Akseppal committed
55
56
57
58
          std::vector<fcl::Vec3f> refPoints;
          refPoints.push_back(refTri.points.p1);
          refPoints.push_back(refTri.points.p2);
          refPoints.push_back(refTri.points.p3);
59
        for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
Steve Tonneau's avatar
Steve Tonneau committed
60
          const Triangle& searchTri = allTris [searchIdx];
Akseppal's avatar
Akseppal committed
61
62
63
          if ((refPoints[vertIdx] - searchTri.points.p1).sqrLength () < margin
              || (refPoints[vertIdx] - searchTri.points.p2).sqrLength () < margin
              || (refPoints[vertIdx] - searchTri.points.p3).sqrLength () < margin) {
64
65
            if (refOp->requirement (searchTri.normal)) {
              if ((searchTri.normal - refTri.normal).sqrLength () < marginRad) {
66
67
                area += searchTri.area;
                listPotential.push_back (searchIdx);
68
                searchLinkedTriangles (listPotential, refOp, allTris,
69
                                       searchableTris, searchIdx, area);
70
71
              }
            } else {
Steve Tonneau's avatar
Steve Tonneau committed
72
73
              // if linked face does not fulfill global requirement, discard
              searchableTris.erase(std::remove(searchableTris.begin(), searchableTris.end(), searchIdx), searchableTris.end());
74
            }
Akseppal's avatar
Akseppal committed
75
            break; // jump out of vertex loop if triangle already tested for affordance
76
          }
Akseppal's avatar
Akseppal committed
77
78
79
80
        }
      }
    }

Akseppal's avatar
Akseppal committed
81
    SemanticsDataPtr_t affordanceAnalysis (const fcl::CollisionObjectPtr_t& colObj,
82
                                           const OperationBases_t& opVec)
83
84
    {
      BVHModelOBConst_Ptr_t model =  GetModel (colObj);
Akseppal's avatar
Akseppal committed
85

86
      std::vector <Triangle> triangles;
Akseppal's avatar
Akseppal committed
87
88
      std::vector <unsigned int> unsetTriangles;
      double totArea = .0;
89
90
      std::vector<std::vector<unsigned int> > potentialAffordances (opVec.size ());
      SemanticsDataPtr_t foundAffordances(new SemanticsData(opVec.size ()));
91

Steve Tonneau's avatar
Steve Tonneau committed
92
      for(int i = 0; i < model->num_tris; ++i)
93
      {
94
95
        // TODO: make sure triagle points are correct in world frame after every 
        // change!! 
96
97
98
99
100
101
102
103
104
        TrianglePoints tri;
        fcl::Triangle fcltri = model->tri_indices [i];
        tri.p1 = colObj->getRotation () *
          model->vertices [fcltri [0]] + colObj->getTranslation ();
        tri.p2 = colObj->getRotation () *
          model->vertices [fcltri [1]] + colObj->getTranslation ();
        tri.p3 = colObj->getRotation () *
          model->vertices [fcltri [2]] + colObj->getTranslation ();

105
        triangles.push_back (Triangle (fcltri, tri));
106
        // save vector index of triangles and their quantity.
Akseppal's avatar
Akseppal committed
107
				unsetTriangles.push_back(i);
108
      }
Akseppal's avatar
Akseppal committed
109
110
111
112
113
114
115
116
117
      std::vector <unsigned int> unseenTriangles;
      for (unsigned int triIdx = 0; triIdx < triangles.size (); triIdx++) {
        // look for triangle in set of triangles not yet given an affordance:
        std::vector<unsigned int>::iterator it = std::find (unsetTriangles.begin (), unsetTriangles.end (), triIdx);
        if (it == unsetTriangles.end ()) {
          continue;
        }
        // set list of searchable (unseen) triangles equal to all triangles not yet given an affordance.
        unseenTriangles = unsetTriangles;
118
119
        for (unsigned int opIdx = 0; opIdx < opVec.size (); opIdx++) {
          if (opVec[opIdx]->requirement (triangles[triIdx].normal)) {
Akseppal's avatar
Akseppal committed
120
121
             totArea += triangles[triIdx].area;
             potentialAffordances[opIdx].push_back(triIdx);
Akseppal's avatar
Akseppal committed
122
123
             searchLinkedTriangles(potentialAffordances [opIdx], opVec[opIdx],
						 	 triangles, unseenTriangles, triIdx, totArea);
124
            if (totArea > opVec[opIdx]->minArea_) {
125
              // save totArea for further use as well?
126
127
              AffordancePtr_t aff(new Affordance (potentialAffordances [opIdx], colObj));
              foundAffordances->affordances_ [opIdx].push_back (aff);
128
129
130
              for (unsigned int removeIdx = 0; removeIdx < potentialAffordances [opIdx].size (); removeIdx++) {
                std::remove (unsetTriangles.begin (), unsetTriangles.end (), potentialAffordances [opIdx][removeIdx]);
                unsetTriangles.pop_back ();
131
              }
Akseppal's avatar
Akseppal committed
132
                // potentialAffordances [opIdx].clear ();
133
             }
Akseppal's avatar
Akseppal committed
134
135
             potentialAffordances [opIdx].clear ();
             totArea = .0;
136
             break;
Akseppal's avatar
Akseppal committed
137
138
139
140
141
          } else if (opIdx >= opVec.size () -1) {
              // delete triangle if it did not fulfil any requirements
              std::remove (unsetTriangles.begin (), unsetTriangles.end (), triIdx);
              unsetTriangles.pop_back ();
            }
Akseppal's avatar
Akseppal committed
142
        }
143
      }
144
      return foundAffordances;
145
    }
146

147
148
149
150
    std::vector<CollisionObjects_t> getAffordanceObjects
                                             (const SemanticsDataPtr_t& sData)
    {
      std::vector<CollisionObjects_t> affObjs;
151
			affObjs.clear();
152
153
      for (unsigned int affIdx = 0; affIdx < sData->affordances_.size (); affIdx ++) {
        std::vector<fcl::CollisionObjectPtr_t> objVec;
154
				objVec.clear();
155
156
157
158
159
160
161
162
163
164
        affObjs.push_back (objVec);
        // get number of affordances of specific type (lean OR support etc)
        // this corresponds to number of objects to be created for specific aff type
        long unsigned int len = sData->affordances_[affIdx].size ();
        for (unsigned int idx = 0; idx < len; idx++) {
          std::vector<fcl::Vec3f> vertices;
          std::vector<fcl::Triangle> triangles;
          hpp::affordance::AffordancePtr_t affPtr = sData->affordances_[affIdx][idx];
          BVHModelOBConst_Ptr_t model =  GetModel (affPtr->colObj_);
          for (unsigned int triIdx = 0; triIdx <  affPtr->indices_.size (); triIdx++) {
Akseppal's avatar
Akseppal committed
165
166
167
						// give triangles of new object new vertex indices (start from 0
						// and go up to 3*nbTris - 1 [all tris have 3 unique indices])
            triangles.push_back (fcl::Triangle ((3*triIdx),(1 + 3*triIdx),(2 + 3*triIdx)));
168
169
170
						for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
							vertices.push_back (model->vertices [vertIdx + 3*(affPtr->indices_[triIdx])]);
						}
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
          }
          
          BVHModelOB_Ptr_t model1 (new BVHModelOB ());
          // add the mesh data into the BVHModel structure
          model1->beginModel ();
          model1->addSubModel (vertices, triangles);
          model1->endModel ();
          // create affordance collision object from created affModel and 
          // tranformation of corresponding reference collision object.
          fcl::CollisionObjectPtr_t obj (new fcl::CollisionObject(model1,
                                         affPtr->colObj_->getTransform ()));
          affObjs[affIdx].push_back (obj);
        }
      }
      return affObjs;
    }
Akseppal's avatar
Akseppal committed
187
188
189
190
191
192
193
194
195
196
197
198
199

		OperationBases_t createOperations ()
		{
			affordance::SupportOperationPtr_t support (new affordance::SupportOperation());
      affordance::LeanOperationPtr_t lean (new affordance::LeanOperation(0.1));

      affordance::OperationBases_t operations;
      operations.push_back(support);
      operations.push_back(lean);
			
			return operations;
		}

200
201
202
203
  } // namespace affordance
} // namespace hpp