Scene.cpp
Go to the documentation of this file.
1 
18 #include "Scene.h"
19 
21 {
23  DbName = "void";
24  SceneName = "void";
25  RankingMethod = 0;
26  Alpha = 0;
27  Beta = 0;
28  Gamma = 0;
29 }
30 
31 Scene::Scene(std::string dbName, std::string sceneName, int rankingMethodEntry, double alphaEntry, double betaEntry, double gammaEntry)
32 :DbName(dbName),SceneName(sceneName),RankingMethod(rankingMethodEntry),Alpha(alphaEntry),Beta(betaEntry),Gamma(gammaEntry)
33 {
34  getNumberSet();
35 }
36 
38 {
39 }
40 
42 {
43  sqlite3 *database;
44  ROS_DEBUG_STREAM("Scene::getObjectFromDb BEGIN");
45  sqlite3_open(DbName.c_str(), &database);
46  sqlite3_stmt *statement;
47  std::stringstream sqlReq;
48  sqlReq << "SELECT type,px,py,pz,setId,observedId FROM recorded_objects "
49  << "INNER JOIN recorded_patterns on recorded_sets.patternId = recorded_patterns.id "
50  << "INNER JOIN recorded_sets on recorded_objects.setId = recorded_sets.id "
51  << "WHERE recorded_patterns.name = '"
52  << SceneName
53  << "';";
54  const std::string& tmp = sqlReq.str();
55  const char* SQLREQ = tmp.c_str();
56  int returnPrepare = sqlite3_prepare_v2(database, SQLREQ, -1,&statement, 0);
57  ROS_DEBUG_STREAM("Scene::getObjectFromDb prepare database returned code : " << returnPrepare);
58  if(returnPrepare == SQLITE_OK)
59  {
60  int cols = sqlite3_column_count(statement);
61  int result = 0;
62  while(true)
63  {
64  result = sqlite3_step(statement);
65  if(cols != 6)
66  {
67  ROS_ERROR_STREAM("Scene::getObjectFromDb something went terribly wrong when recovering objects for scene : " << SceneName);
68  continue;
69  }
70  if(result == SQLITE_ROW)
71  {
73  std::string name = (char*)sqlite3_column_text(statement, 0);
74  std::string X = (char*)sqlite3_column_text(statement, 1);
75  std::string Y = (char*)sqlite3_column_text(statement, 2);
76  std::string Z = (char*)sqlite3_column_text(statement, 3);
77  std::string setId = (char*)sqlite3_column_text(statement, 4);
78  std::string observedId = (char*)sqlite3_column_text(statement, 5);
79  std::string objectMetaName = name + observedId;
80  if(this->ObjectMap.find(objectMetaName)== this->ObjectMap.end())
81  {
82  this->ObjectMap[objectMetaName] = ObjectSharedPointer(new Object(name, observedId, RankingMethod, Alpha, Beta, Gamma));
83  ROS_DEBUG_STREAM("Scene::getObjectFromDb found object " << name
84  << " with observed Id " << observedId
85  << " in scene " << SceneName);
86  }
87  objectPoint->setX(boost::lexical_cast<double>(X));
88  objectPoint->setY(boost::lexical_cast<double>(Y));
89  objectPoint->setZ(boost::lexical_cast<double>(Z));
90  objectPoint->setSetId(boost::lexical_cast<int>(setId));
91  this->ObjectMap[objectMetaName]->AddPoint(objectPoint);
92  }
93  else
94  {
95  break;
96  }
97  }
98 
99  sqlite3_finalize(statement);
100  }
101  sqlite3_close(database);
102  ROS_ERROR_COND(ObjectMap.size() == 0, "Scene::getObjectFromDb Could not get any Object. Please check the database location and the pattern name");
103  ROS_DEBUG_STREAM("Scene::getObjectFromDb END");
104 }
105 
106 
108 {
109  ROS_DEBUG_STREAM("Scene::getNumberSet BEGIN");
110  //Get the number of sets recorded for the scene.
111  std::stringstream sqlReq;
112  sqlReq << "SELECT COUNT(DISTINCT recorded_sets.id) FROM recorded_sets "
113  << "INNER JOIN recorded_patterns on recorded_sets.patternId = recorded_patterns.id "
114  << "WHERE recorded_patterns.name = '"
115  << SceneName
116  << "';";
117  const std::string& tmp = sqlReq.str();
118  const char* SQLREQ = tmp.c_str();
119  sqlite3 *database;
120  sqlite3_open(DbName.c_str(), &database);
121  sqlite3_stmt *statement;
122  if(sqlite3_prepare_v2(database, SQLREQ, -1, &statement, 0) == SQLITE_OK)
123  {
124  int cols = sqlite3_column_count(statement);
125  int result = sqlite3_step(statement);
126  if(result == SQLITE_ROW)
127  {
128  for(int col = 0; col < cols; col++)
129  {
130  std::string s = (char*)sqlite3_column_text(statement, col);
131  SceneObjectsCount = boost::lexical_cast<int>(s);
132  }
133  }
134  sqlite3_finalize(statement);
135  }
136  sqlite3_close(database);
137  ROS_DEBUG_STREAM_COND(SceneObjectsCount > 0,"Scene::getNumberSet Got "<< SceneObjectsCount <<" sets for scene " << SceneName);
138  ROS_ERROR_STREAM_COND(SceneObjectsCount == 0, "Scene::getNumberSet Could not get the amount of Sets in the scene"
139  << SceneName);
140  ROS_DEBUG_STREAM("Scene::getNumberSet END");
141 }
142 
144 {
145  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
146  {
147  objectIterator->second->CalcAveragePos();
148  objectIterator->second->CalcPosVar();
149  }
150 }
151 
153 {
154  bool safetyTest = false;
155 
156  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator) //Parcours liste Obj
157  {
158  for(int point=0; point<objectIterator->second->getObjetsCount(); point++) //All the point for one object
159  {
160  double distance=0;
161  int counter=0;
162  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator2=ObjectMap.begin(); objectIterator2!=ObjectMap.end(); ++objectIterator2)
163  {
164  if(objectIterator2->first != objectIterator->first) //If objects are different
165  {
166  safetyTest = true;
167  for(int k=0; k<objectIterator2->second->getObjetsCount(); k++) //Other points for other objects
168  {
169  if((objectIterator2->second->getPoint(k))->getSetId() == (objectIterator->second->getPoint(point))->getSetId())
170  {
171  //Add the distance to the variable
172  distance += (objectIterator->second->getPoint(point))->calcDistance(*(objectIterator2->second->getPoint(k)));
173  counter++;
174  }
175  }
176  }
177  }
178  if(safetyTest==true){objectIterator->second->AddDistance(distance/counter);safetyTest=false;}
179  }
180  objectIterator->second->AverageDistance();
181  }
182 }
183 
185 {
186  ROS_DEBUG_STREAM("Scene::calcPresenceInSceneForEachObject BEGIN");
187  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
188  {
189  double presenceInScene = (boost::lexical_cast<double>(objectIterator->second->getObjetsCount())/boost::lexical_cast<double>(this->SceneObjectsCount));
190  ROS_DEBUG_STREAM("Scene::calcPresenceInSceneForEachObject Object occurence in scene : "
191  << objectIterator->second->getObjetsCount()
192  << ", amount of sets recorded in the scene : " << this->SceneObjectsCount
193  << ", presence in scene : " << presenceInScene
194  << ", for object : " << objectIterator->second->getObjectName()
195  << " with observed Id " << objectIterator->second->getObservedId());
196  objectIterator->second->setPresenceInScene(presenceInScene);
197  }
198  ROS_DEBUG_STREAM("Scene::calcPresenceInSceneForEachObject END");
199 }
200 
202 {
203  ROS_DEBUG_STREAM("Scene::DisplayStats Stats For Scene " << SceneName);
204  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
205  {
206  objectIterator->second->DisplayStats();
207  }
208 }
209 
210 
212 {
213  std::function<bool(const std::pair<std::string,ObjectSharedPointer>,const std::pair<std::string,ObjectSharedPointer>)> compare_distance_min =
214  [](const std::pair<std::string,ObjectSharedPointer>& i, const std::pair<std::string,ObjectSharedPointer>& j)
215  { return i.second->getAverageDistanceToOtherObjects() < j.second->getAverageDistanceToOtherObjects();};
216 
217  std::function<bool(const std::pair<std::string,ObjectSharedPointer>,const std::pair<std::string,ObjectSharedPointer>)> compare_position_min =
218  [](const std::pair<std::string,ObjectSharedPointer>& i, const std::pair<std::string,ObjectSharedPointer>& j)
219  { return i.second->getPosVar() < j.second->getPosVar();};
220 
221  //Get the min values for all parameters
222  double minValueDist = (std::min_element(ObjectMap.begin(),ObjectMap.end(),compare_distance_min))->second->getAverageDistanceToOtherObjects();
223  double minValuePos = (std::min_element(ObjectMap.begin(),ObjectMap.end(),compare_position_min))->second->getPosVar();
224 
225  ROS_DEBUG_STREAM("Scene::normalize minimum average distance value found " << minValueDist);
226  ROS_DEBUG_STREAM("Scene::normalize minimum average position value found " << minValuePos);
227 
228  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
229  {
230  double bufferMin = (objectIterator->second->getAverageDistanceToOtherObjects()-minValueDist);
231  ROS_DEBUG_STREAM("Scene::normalize difference to min value for object "
232  << objectIterator->second->getObjectName() << " : " << bufferMin);
233  (objectIterator->second)->setNormalizedAverageDistanceToOtherObjects(bufferMin);
234  (objectIterator->second)->setNormalizedPosVar((objectIterator->second->getPosVar()-minValuePos));
235  }
236 
237  std::function<bool(const std::pair<std::string,ObjectSharedPointer>,const std::pair<std::string,ObjectSharedPointer>)> compare_distance_max =
238  [](const std::pair<std::string,ObjectSharedPointer>& i, const std::pair<std::string,ObjectSharedPointer>& j)
239  { return i.second->getNormalizedAverageDistanceToOtherObjects() < j.second->getNormalizedAverageDistanceToOtherObjects();};
240 
241  std::function<bool(const std::pair<std::string,ObjectSharedPointer>,const std::pair<std::string,ObjectSharedPointer>)> compare_position_max =
242  [](const std::pair<std::string,ObjectSharedPointer>& i, const std::pair<std::string,ObjectSharedPointer>& j)
243  { return i.second->getNormalizedPosVar() < j.second->getNormalizedPosVar();};
244 
245  //get the max values for all paramters
246  double maxValueDist = (std::max_element(ObjectMap.begin(),ObjectMap.end(),compare_distance_max))->second->getNormalizedAverageDistanceToOtherObjects();
247  double maxValuePos = (std::max_element(ObjectMap.begin(),ObjectMap.end(),compare_position_max))->second->getNormalizedPosVar();
248 
249  ROS_DEBUG_STREAM("Scene::normalize max average distance value found " << maxValueDist);
250  ROS_DEBUG_STREAM("Scene::normalize max average position value found " << maxValuePos);
251 
252  double valueDistCoef = 1/maxValueDist;
253  double posVarCoef = 1/maxValuePos;
254  //Normalize each value.
255  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
256  {
257  objectIterator->second->setNormalizedAverageDistanceToOtherObjects(objectIterator->second->getNormalizedAverageDistanceToOtherObjects()*valueDistCoef+0.1);
258  objectIterator->second->setNormalizedPosVar(objectIterator->second->getNormalizedPosVar()*posVarCoef+0.1);
259  }
260 
261 }
262 
264 {
265  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
266  {
267  objectIterator->second->rank();
268  }
269 }
270 
271 void Scene::publishLogs(std::string filePath)
272 {
273  std::ofstream file(filePath, std::ios::out | std::ios::app);
274  if(file)
275  {
276  file << std::endl << std::endl <<"Scene " << this->SceneName << std::endl;
277  file.close();
278  }
279  for (std::unordered_map<std::string,ObjectSharedPointer>::iterator objectIterator=ObjectMap.begin(); objectIterator!=ObjectMap.end(); ++objectIterator)
280  {
281  objectIterator->second->publishLogs(filePath);
282  }
283 }
284 
285 
Definition: Object.h:30
#define ROS_DEBUG_STREAM_COND(cond, args)
double Beta
Definition: Scene.h:41
void rank()
Definition: Scene.cpp:263
XmlRpcServer s
virtual ~Scene()
Definition: Scene.cpp:37
std::string SceneName
Definition: Scene.h:37
#define ROS_ERROR_STREAM_COND(cond, args)
void getNumberSet()
Definition: Scene.cpp:107
void getObjectAverageDistance()
Definition: Scene.cpp:152
#define ROS_ERROR_COND(cond,...)
void getObjectFromDb()
Definition: Scene.cpp:41
void displayStats()
Definition: Scene.cpp:201
Scene()
Definition: Scene.cpp:20
void calcAveragePositionForEachObject()
Definition: Scene.cpp:143
void calcPresenceInSceneForEachObject()
Definition: Scene.cpp:184
int SceneObjectsCount
Definition: Scene.h:38
std::shared_ptr< Object > ObjectSharedPointer
Definition: Scene.h:31
#define ROS_DEBUG_STREAM(args)
int RankingMethod
Definition: Scene.h:39
std::shared_ptr< ObjectPoint > ObjectPointSharedPointer
Definition: Object.h:28
double Alpha
Definition: Scene.h:40
std::unordered_map< std::string, ObjectSharedPointer > ObjectMap
Definition: Scene.h:35
double Gamma
Definition: Scene.h:42
#define ROS_ERROR_STREAM(args)
void normalize()
Definition: Scene.cpp:211
void publishLogs(std::string filePath)
Definition: Scene.cpp:271
std::string DbName
Definition: Scene.h:36


asr_intermediate_object_generator
Author(s): Borella Jocelyn, Mei├čner Pascal
autogenerated on Thu Nov 21 2019 03:53:41