21 #include <boost/shared_ptr.hpp> 25 #include <Eigen/Geometry> 35 class OcmSceneGraphNode
39 std::vector<Eigen::Vector3f> samples;
40 std::vector<Eigen::Vector4f> samplesOrientation;
41 std::vector<boost::shared_ptr<OcmSceneGraphNode> > childs;
42 std::vector<GMM_Ptr> childsRelativePoseGmm;
43 std::vector<GMM_Ptr> childsRelativeRotationGmm;
48 unsigned int votes_per_node;
51 bool print_debug_messages;
59 OcmSceneGraphNode(
bool enable_debug_mode =
true)
65 print_debug_messages = enable_debug_mode;
71 OcmSceneGraphNode(std::string name, Eigen::Vector3f root_position, Eigen::Vector4f root_orientation,
int votes = 100,
bool enable_debug_mode =
true)
73 this->print_debug_messages = enable_debug_mode;
76 this->votes_per_node = votes;
77 observe(root_position, root_orientation);
89 for(
unsigned int i=0;i<childs.size(); i++)
93 if(getChild(i)->getName().compare(key) == 0)
96 out_pointer = getChild(i);
101 for(
unsigned int i=0;i<childs.size(); i++)
103 if(childs.at(i)->findNode(key, out_pointer))
107 if(print_debug_messages) printf(
"Object %s not found\n", key.c_str());
117 node->setParent(parent);
118 childsRelativePoseGmm.push_back(gmm);
119 childsRelativeRotationGmm.push_back(orientationGMM);
121 node->setVotesPerNode(parent->getVotesPerNode());
123 childs.push_back(node);
124 if(print_debug_messages) printf(
"Child %s added to node %s\n", name.c_str(), parent->getName().c_str());
132 Eigen::Vector3f getPose()
134 if(isObjectObserved())
135 return samples.at(0);
138 std::random_device rd;
139 std::mt19937 mt(rd());
141 std::uniform_real_distribution<float> rand_id(1, samples.size());
142 int id = floor(rand_id(mt));
143 return samples.at(
id);
147 Eigen::Vector4f getOrientation()
149 if(isObjectObserved())
150 return samplesOrientation.at(0);
153 std::random_device rd;
154 std::mt19937 mt(rd());
156 std::uniform_real_distribution<float> rand_id(1, samplesOrientation.size());
157 int id = floor(rand_id(mt));
158 return samplesOrientation.at(
id);
166 void observe(Eigen::Vector3f position, Eigen::Vector4f orientation)
171 samples.push_back(position);
173 samplesOrientation.clear();
174 samplesOrientation.push_back(orientation);
184 void calcChildPoseHypothesesRecursive()
186 ROS_ASSERT(childs.size() == childsRelativePoseGmm.size());
187 if(print_debug_messages)
ROS_INFO(
"Current Node %s, Number of childs: %i, Number of GMMs: %i, #Votes %i", name.c_str(), (int)childs.size(), (int)childsRelativePoseGmm.size(), getVotesPerNode());
189 for(
unsigned int i=0;i<childs.size();i++)
191 if (!childs.at(i)->isObjectObserved())
194 childs.at(i)->samples.clear();
197 for(
unsigned int j=0; j < getVotesPerNode() ;j++)
199 std::vector<float> v;
201 childsRelativePoseGmm.at(i)->sampleRandomValues(v);
207 hyp += this->getPose();
211 Eigen::Vector4f orientationSample;
212 childsRelativeRotationGmm.at(i)->sampleRandomValues(v);
214 orientationSample.w() = v.at(0);
215 orientationSample.x() = v.at(1);
216 orientationSample.y() = v.at(2);
217 orientationSample.z() = v.at(3);
219 Eigen::Quaternion<float> q(orientationSample);
220 Eigen::Quaternion<float> q_parent(getOrientation());
224 orientationSample.w() = q.w();
225 orientationSample.x() = q.x();
226 orientationSample.y() = q.y();
227 orientationSample.z() = q.z();
229 childs.at(i)->addSample(hyp, orientationSample);
233 childs.at(i)->calcChildPoseHypothesesRecursive();
244 void collectPoseHypothesesRecursive(std::vector<ASR::AttributedPoint> &out_hypotheses,
245 std::vector<ASR::AttributedPoint> &out_found_objects)
248 if(this->isObjectObserved())
250 Eigen::Vector3f v = this->getPose();
251 Eigen::Vector4f ori = this->getOrientation();
252 out_found_objects.push_back(AttributedPoint(
getName().c_str(),
254 ori.w(), ori.x(), ori.y(), ori.z()));
257 for(
unsigned int i=0;i<childs.size();i++)
260 for(
unsigned int s=0;
s < getChild(i)->getPoseSamples().size();
s++)
262 Eigen::Vector3f pos_sample = getChild(i)->getPoseSamples().at(s);
263 Eigen::Vector4f ori_sample = getChild(i)->getOrientationSamples().at(s);
265 if(print_debug_messages)
ROS_INFO(
"type %s, pos: (%.2f %.2f %.2f), ori: (%.2f %.2f %.2f %.2f) ",
266 getChild(i)->
getName().c_str(),
267 pos_sample.x(), pos_sample.y(), pos_sample.z(),
268 ori_sample.w(), ori_sample.x(), ori_sample.y(), ori_sample.z());
271 pos_sample.x(), pos_sample.y(), pos_sample.z(),
272 ori_sample.w(), ori_sample.x(), ori_sample.y(), ori_sample.z());
275 if(getChild(i)->isObjectObserved())
277 out_found_objects.push_back(hyp);
280 out_hypotheses.push_back(hyp);
285 childs.at(i)->collectPoseHypothesesRecursive(out_hypotheses, out_found_objects);
295 void setName(std::string name) { this->name = name;}
296 std::string
getName() {
return this->name;}
300 bool isObjectObserved() {
return isObserved;}
302 std::vector<Eigen::Vector3f> getPoseSamples() {
return samples; }
303 std::vector<Eigen::Vector4f> getOrientationSamples() {
return samplesOrientation; }
307 void addSample(Eigen::Vector3f positionSample, Eigen::Vector4f orientationSample) { samples.push_back(positionSample); samplesOrientation.push_back(orientationSample); }
311 return childs.at(index);
314 unsigned int getNumberOfChilds() {
return childs.size();}
316 unsigned int getVotesPerNode() {
return this->votes_per_node;}
318 Eigen::Vector3f getMeanOfChildPosition(
unsigned int index)
320 ROS_ASSERT(index < childsRelativePoseGmm.size());
321 std::vector<float> means = childsRelativePoseGmm.at(index)->getModel(0)->getMean();
322 Eigen::Vector3f mean;
323 mean.x() = means.at(0);
324 mean.y() = means.at(1);
325 mean.z() = means.at(2);
334 void setVotesPerNode(
unsigned int n)
337 this->votes_per_node = n;
338 for(
unsigned int i=0;i<childs.size();i++)
340 childs.at(i)->setVotesPerNode(n);
std::string getName(void *handle)
boost::shared_ptr< GMM > GMM_Ptr