iCub-main
skinPart.cpp
Go to the documentation of this file.
2 
3 using namespace yarp::math;
4 using namespace iCub::skinDynLib;
5 
6 /****************************************************************/
7 /* SKINPARTBASE WRAPPER
8 *****************************************************************/
9  skinPartBase::skinPartBase() : name("unknown_skin_part"), size(0), version("unknown_version") {}
10 
12  {
13  *this = _spb;
14  }
15 
17  {
18  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
19  if (this == &_spb)
20  {
21  return *this;
22  }
23 
24  name = _spb.name;
25  size = _spb.size;
26  version = _spb.version;
27  return *this;
28  }
29 
31  {
32  name = _name;
33  }
34 
36  {
37  return name;
38  }
39 
40  void skinPartBase::setSize(int _size)
41  {
42  size = _size;
43  }
44 
46  {
47  return size;
48  }
49 
50  void skinPartBase::setVersion(const std::string &_version)
51  {
52  version = _version;
53  }
54 
56  {
57  return version;
58  }
59 
60 
62  {
63  yDebug("**********\n");
64  yDebug("name: %s\t", name.c_str());
65  yDebug("total number of taxels: %i\n", size);
66  yDebug("version: %s\n", version.c_str());
67  }
68 
70  {
71  std::stringstream res;
72  res << "**********\n" << "Name: " << name << "\tSize: "<< size << "\tVersion: "<< version << std::endl;
73  return res.str();
74  }
75 
76 /****************************************************************/
77 /* SKINPART TAXEL WRAPPER
78 *****************************************************************/
80  {
81  spatial_sampling = "taxel";
82  }
83 
84  skinPart::skinPart(const std::string &_filePath)
85  {
86  setTaxelPosesFromFile(_filePath);
87  }
88 
90  {
91  *this=_sp;
92  }
93 
95  {
96  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
97  if (this == &_sp)
98  {
99  return *this;
100  }
101 
103 
105  taxel2Repr = _sp.taxel2Repr;
107 
108  clearTaxels();
109  for (std::vector<Taxel*>::const_iterator it = _sp.taxels.begin();
110  it != _sp.taxels.end(); ++it)
111  {
112  taxels.push_back(new Taxel(*(*it)));
113  }
114 
115  return *this;
116  }
117 
118  bool skinPart::setTaxelPosesFromFile(const std::string &_filePath, const std::string &_spatial_sampling)
119  {
120  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
121  // Get the filename from the full absolute path
122  std::string filename = "";
123  filename = strrchr(_filePath.c_str(), '/');
124  filename = filename.c_str() ? filename.c_str() + 1 : _filePath.c_str();
125 
126  yarp::os::ResourceFinder rf;
127  rf.setDefaultContext("skinGui"); //overridden by --context parameter
128  rf.setDefaultConfigFile(_filePath.c_str()); //overridden by --from parameter
129  rf.configure(0,NULL);
130 
131  if (rf.check("name"))
132  {
133  setName(rf.find("name").asString());
134  }
135  else
136  {
137  yWarning("[skinPart::setTaxelPosesFromFile] no name field found. Using filename.");
138  // Assign the name and version of the skinPart according to the filename (hardcoded)
139  if (filename == "left_forearm_mesh.txt") { setName(SkinPart_s[SKIN_LEFT_FOREARM]); setVersion("V1"); }
140  else if (filename == "left_forearm_nomesh.txt") { setName(SkinPart_s[SKIN_LEFT_FOREARM]); setVersion("V1"); }
141  else if (filename == "left_forearm_V2.txt") { setName(SkinPart_s[SKIN_LEFT_FOREARM]); setVersion("V2"); }
142  else if (filename == "right_forearm_mesh.txt") { setName(SkinPart_s[SKIN_RIGHT_FOREARM]); setVersion("V1"); }
143  else if (filename == "right_forearm_nomesh.txt") { setName(SkinPart_s[SKIN_RIGHT_FOREARM]); setVersion("V1"); }
144  else if (filename == "right_forearm_V2.txt") { setName(SkinPart_s[SKIN_RIGHT_FOREARM]); setVersion("V2"); }
145  else if (filename == "left_hand_V2_1.txt") { setName(SkinPart_s[SKIN_LEFT_HAND]); setVersion("V2.1"); }
146  else if (filename == "right_hand_V2_1.txt") { setName(SkinPart_s[SKIN_RIGHT_HAND]); setVersion("V2.1"); }
147  else if (filename == "left_arm_mesh.txt") { setName(SkinPart_s[SKIN_LEFT_UPPER_ARM]); setVersion("V1"); }
148  else if (filename == "right_arm_mesh.txt") { setName(SkinPart_s[SKIN_RIGHT_UPPER_ARM]); setVersion("V1"); }
149  else if (filename == "torso.txt") { setName(SkinPart_s[SKIN_FRONT_TORSO]); setVersion("V1"); }
150  else
151  {
152  yError("[skinPart::setTaxelPosesFromFile] Unexpected skin part file name: %s.\n",filename.c_str());
153  return false;
154  }
155  }
156  yTrace("[skinPart] name set to %s",name.c_str());
157 
158  if (rf.check("spatial_sampling"))
159  {
160  std::string _ss=rf.find("spatial_sampling").asString();
161 
162  // This lets us override the field without touching the .ini file
163  if (_spatial_sampling=="default" && (_ss=="taxel" || _ss=="triangle"))
164  {
165  spatial_sampling = _ss;
166  }
167  else if (_spatial_sampling=="taxel" || _spatial_sampling=="triangle")
168  {
169  spatial_sampling = _spatial_sampling;
170  }
171  else if ((_spatial_sampling!="default" && _spatial_sampling!="taxel" &&
172  _spatial_sampling!="triangle") && (_ss=="taxel" || _ss=="triangle"))
173  {
174  spatial_sampling = _ss;
175  }
176  }
177  else
178  {
179  yWarning("[skinPart::setTaxelPosesFromFile] no spatial_sampling field found.");
180  spatial_sampling = _spatial_sampling;
181  }
182 
183  yarp::os::Bottle &calibration = rf.findGroup("calibration");
184  if (calibration.isNull())
185  {
186  yWarning("[skinPart::setTaxelPosesFromFile] No calibration group found!");
187  yWarning("[skinPart::setTaxelPosesFromFile] Using old convention");
188  spatial_sampling = "taxel";
189  return setTaxelPosesFromFileOld(_filePath);
190  }
191 
192  // First item of the bottle is "calibration", so we should not use it
193  setSize(calibration.size()-1);
194  yarp::sig::Vector taxelPos(3,0.0);
195  yarp::sig::Vector taxelNrm(3,0.0);
196  yarp::sig::Vector taxelPosNrm(6,0.0);
197 
198  for (int i = 1; i < getSize(); i++)
199  {
200  taxelPosNrm = vectorFromBottle(*(calibration.get(i).asList()),0,6);
201  taxelPos = taxelPosNrm.subVector(0,2);
202  taxelNrm = taxelPosNrm.subVector(3,5);
203  // the NULL taxels will be automatically discarded
204  if (norm(taxelNrm) != 0 || norm(taxelPos) != 0)
205  {
206  taxels.push_back(new Taxel(taxelPos,taxelNrm,i-1));
207  }
208  }
209 
210  // Let's read the mapping of the taxels onto the center of their triangle
211  // even if the spatial_sampling variable is "taxel"
212  // (it might come useful later)
213  if (rf.check("taxel2Repr"))
214  {
215  yarp::os::Bottle b = *(rf.find("taxel2Repr").asList());
216 
217  for (int i = 0; i < getSize(); i++)
218  {
219  taxel2Repr.push_back(b.get(i).asInt32());
220  }
222  }
223  else
224  {
225  yError("[skinPart::setTaxelPosesFromFile] No 'taxel2Repr' field found");
226  return false;
227  }
228 
229  return true;
230  }
231 
232  // see also Compensator::setTaxelPosesFromFile
233  // in icub-main/src/modules/skinManager/src/compensator.cpp
235  {
236  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
237  std::string line;
238  std::ifstream posFile;
239  yarp::sig::Vector taxelPos(3,0.0);
240  yarp::sig::Vector taxelNrm(3,0.0);
241 
242  std::string filename = strrchr(_filePath.c_str(), '/');
243  filename = filename.c_str() ? filename.c_str() + 1 : _filePath.c_str();
244 
245  // Open File
246  posFile.open(_filePath.c_str());
247  if (!posFile.is_open())
248  {
249  yError("[skinPart::setTaxelPosesFromFileOld] File %s has not been opened!",
250  _filePath.c_str());
251  return false;
252  }
253 
254  // Acquire taxels
255  posFile.clear();
256  posFile.seekg(0, std::ios::beg);//rewind iterator
257  for(unsigned int i= 0; getline(posFile,line); i++)
258  {
259  line.erase(line.find_last_not_of(" \n\r\t")+1);
260  if(line.empty())
261  continue;
262  std::string number;
263  std::istringstream iss(line, std::istringstream::in);
264  for(unsigned int j = 0; iss >> number; j++ )
265  {
266  if(j<3)
267  taxelPos[j] = strtod(number.c_str(),NULL);
268  else
269  taxelNrm[j-3] = strtod(number.c_str(),NULL);
270  }
271 
272  // the NULL taxels will be automatically discarded
273  if (norm(taxelNrm) != 0 || norm(taxelPos) != 0)
274  {
275  setSize(getSize()+1);
276  taxels.push_back(new Taxel(taxelPos,taxelNrm,i));
277  }
278  else
279  setSize(getSize()+1);
280  }
281 
283  }
284 
286  {
287  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
288  for (int i = 0; i < getSize(); ++i)
289  {
290  bool isIvalidID=false;
291  for (int j = 0; j < getTaxelsSize(); ++j)
292  {
293  if (taxels[j]->getID()==i)
294  {
295  isIvalidID=true;
296  break;
297  }
298  }
299 
300  if (isIvalidID)
301  {
302  taxel2Repr.push_back(int(i));
303  }
304  else
305  {
306  taxel2Repr.push_back(-1);
307  }
308  }
309 
310  return true;
311  }
312 
314  {
315  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
316  std::list<int> mapp(taxel2Repr.begin(), taxel2Repr.end());
317  mapp.sort();
318  mapp.unique();
319 
320  size_t mappsize = mapp.size();
321  for (size_t i = 0; i < mappsize; i++)
322  {
323  repr2TaxelList[mapp.front()]=vectorofIntEqualto(taxel2Repr,mapp.front());
324  mapp.pop_front();
325  }
326 
327  return true;
328  }
329 
331  {
332  return taxels.size();
333  }
334 
336  {
337  std::lock_guard<std::recursive_mutex> rlg(recursive_mtx);
338  while(!taxels.empty())
339  {
340  if (taxels.back())
341  {
342  delete taxels.back();
343  }
344  taxels.pop_back();
345  }
346  taxels.clear();
347  }
348 
350  {
352  yDebug("number of valid taxels: %i", getTaxelsSize());
353  yDebug("spatial_sampling: %s", spatial_sampling.c_str());
354  if ((verbosity>=1 && spatial_sampling == "triangle") ||
355  (verbosity>=3 && spatial_sampling == "taxel"))
356  {
357  yDebug("Taxel ID -> Representative ID:");
358 
359  for (int i=0; i<size; i++)
360  {
361  printf("[ %i->%d ]\t",i,taxel2Repr[i]);
362  }
363  printf("\n");
364 
365  yDebug("Representative ID -> Taxel IDs:\n");
366  for(std::map<int, std::list<unsigned int> >::const_iterator iter_map = repr2TaxelList.begin(); iter_map != repr2TaxelList.end(); ++iter_map)
367  {
368  std::list<unsigned int> l = iter_map->second;
369  printf("\t%d -> {",iter_map->first);
370  for(std::list<unsigned int>::const_iterator iter_list = l.begin(); iter_list != l.end(); iter_list++)
371  {
372  printf("%u, ",*iter_list);
373  }
374  printf("}\n");
375  }
376  }
377  if (verbosity>=2)
378  {
379  yDebug("Taxels:");
380  for (size_t i = 0; i < taxels.size(); i++)
381  taxels[i]->print(verbosity-3>0?verbosity-3:0);
382  }
383  yDebug("**********\n");
384  }
385 
387  {
388  std::stringstream res(skinPartBase::toString(precision));
389  for (size_t i = 0; i < taxels.size(); i++)
390  res << taxels[i]->toString(precision);
391  res << "**********\n";
392  return res.str();
393  }
394 
396  {
397  clearTaxels();
398  }
399 
400 // empty line to make gcc and Francesco happy
iCub::skinDynLib::SKIN_FRONT_TORSO
@ SKIN_FRONT_TORSO
Definition: common.h:60
iCub::skinDynLib::SKIN_LEFT_HAND
@ SKIN_LEFT_HAND
Definition: common.h:58
iCub::skinDynLib::skinPartBase::size
int size
Definition: skinPart.h:55
iCub::skinDynLib::skinPart::print
void print(int verbosity=0)
Print Method.
Definition: skinPart.cpp:349
iCub::skinDynLib::skinPart::setTaxelPosesFromFileOld
bool setTaxelPosesFromFileOld(const std::string &_filePath)
Populates the skinPart by reading from a file - old convention.
Definition: skinPart.cpp:234
iCub::skinDynLib::skinPart
Definition: skinPart.h:145
iCub::skinDynLib
Definition: common.h:35
iCub::skinDynLib::skinPart::initRepresentativeTaxels
bool initRepresentativeTaxels()
Initializes the mapping between the taxels and their representatives (i.e.
Definition: skinPart.cpp:313
iCub::skinDynLib::skinPart::~skinPart
~skinPart()
Destructor.
Definition: skinPart.cpp:395
iCub::skinDynLib::Taxel
Definition: Taxel.h:56
iCub::skinDynLib::skinPartBase::setVersion
void setVersion(const std::string &_version)
Sets the version ("V1" / "V2" / "V2.1")
Definition: skinPart.cpp:50
iCub::skinDynLib::skinPartBase::operator=
virtual skinPartBase & operator=(const skinPartBase &_sp)
Copy Operator.
Definition: skinPart.cpp:16
iCub::skinDynLib::vectorofIntEqualto
std::list< unsigned int > vectorofIntEqualto(const std::vector< int > vec, const int val)
Definition: common.h:259
iCub::skinDynLib::skinPart::repr2TaxelList
std::map< int, std::list< unsigned int > > repr2TaxelList
Mapping in the opposite direction Indexed by representative taxel IDs, it stores lists of the taxels ...
Definition: skinPart.h:170
iCub::skinDynLib::skinPartBase::getVersion
std::string getVersion()
Gets the version.
Definition: skinPart.cpp:55
iCub::skinDynLib::skinPart::setTaxelPosesFromFile
bool setTaxelPosesFromFile(const std::string &_filePath, const std::string &_spatial_sampling="default")
Populates the skinPart by reading from a file.
Definition: skinPart.cpp:118
iCub::skinDynLib::skinPart::clearTaxels
void clearTaxels()
Clears the vector of taxels properly and gracefully.
Definition: skinPart.cpp:335
iCub::skinDynLib::skinPartBase::getName
std::string getName()
Gets the name of the class.
Definition: skinPart.cpp:35
verbosity
int verbosity
Definition: main.cpp:21
iCub::skinDynLib::skinPart::taxel2Repr
std::vector< int > taxel2Repr
Indexing variable used in the case of reducing the resolution - e.g.
Definition: skinPart.h:164
iCub::skinDynLib::skinPart::skinPart
skinPart()
Constructor.
Definition: skinPart.cpp:79
iCub::skinDynLib::skinPartBase::skinPartBase
skinPartBase()
Constructor.
Definition: skinPart.cpp:9
iCub::skinDynLib::skinPartBase::toString
virtual std::string toString(int precision=0)
toString Method
Definition: skinPart.cpp:69
iCub::skinDynLib::SKIN_RIGHT_UPPER_ARM
@ SKIN_RIGHT_UPPER_ARM
Definition: common.h:59
iCub::skinDynLib::skinPartBase::getSize
int getSize()
Gets the size of the class.
Definition: skinPart.cpp:45
iCub::skinDynLib::skinPart::taxels
std::vector< Taxel * > taxels
List of taxels that belong to the skinPart.
Definition: skinPart.h:151
iCub::skinDynLib::skinPartBase::setSize
void setSize(int _size)
Sets the size of the class.
Definition: skinPart.cpp:40
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
iCub::skinDynLib::skinPart::spatial_sampling
std::string spatial_sampling
Spatial_sampling used in building up the skinPart class.
Definition: skinPart.h:158
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinDynLib::skinPartBase::version
std::string version
Definition: skinPart.h:60
iCub::skinDynLib::skinPart::getTaxelsSize
int getTaxelsSize()
gets the size of the taxel vector (it differs from skinPartBase::getSize())
Definition: skinPart.cpp:330
iCub::skinDynLib::SkinPart_s
const std::string SkinPart_s[]
Definition: common.h:64
iCub::skinDynLib::skinPartBase::name
std::string name
Definition: skinPart.h:54
iCub::skinDynLib::skinPart::operator=
skinPart & operator=(const skinPart &_sp)
Copy Operator.
Definition: skinPart.cpp:94
iCub::skinDynLib::skinPartBase::print
virtual void print(int verbosity=0)
Print Method.
Definition: skinPart.cpp:61
iCub::skinDynLib::SKIN_RIGHT_FOREARM
@ SKIN_RIGHT_FOREARM
Definition: common.h:59
iCub::skinDynLib::skinPart::mapTaxelsOntoThemselves
bool mapTaxelsOntoThemselves()
Maps the taxels onto themselves, performing a 1:1 mapping.
Definition: skinPart.cpp:285
iCub::skinDynLib::SKIN_RIGHT_HAND
@ SKIN_RIGHT_HAND
Definition: common.h:59
iCub::skinDynLib::skinPart::toString
std::string toString(int precision=0)
toString Method
Definition: skinPart.cpp:386
iCub::learningmachine::math::map
yarp::sig::Vector & map(yarp::sig::Vector &v, double(op)(double))
Performs a unary operator inplace on each element of a vector.
Definition: Math.cpp:305
iCub::skinDynLib::skinPartBase
Definition: skinPart.h:51
iCub::skinDynLib::SKIN_LEFT_FOREARM
@ SKIN_LEFT_FOREARM
Definition: common.h:58
iCub::skinDynLib::skinPartBase::recursive_mtx
std::recursive_mutex recursive_mtx
Definition: skinPart.h:63
iCub::skinDynLib::vectorFromBottle
yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
Definition: common.cpp:89
iCub::skinDynLib::SKIN_LEFT_UPPER_ARM
@ SKIN_LEFT_UPPER_ARM
Definition: common.h:58
skinPart.h
iCub::skinDynLib::skinPartBase::setName
void setName(const std::string &_name)
Sets the name of the class.
Definition: skinPart.cpp:30