32 #include <opencv2/highgui/highgui.hpp>
57 const std::string & label,
70 _invalidWordsCount(0),
72 _groundTruthPose(groundTruthPose),
73 _sensorData(sensorData)
92 _invalidWordsCount(0),
94 _groundTruthPose(
data.groundTruth()),
107 for(std::list<Link>::const_iterator
iter = links.begin();
iter!=links.end(); ++
iter)
114 for(std::map<int, Link>::const_iterator
iter = links.begin();
iter!=links.end(); ++
iter)
125 _links.insert(std::make_pair(link.
to(), link));
160 std::multimap<int, Link>::iterator
iter =
_links.find(idFrom);
166 _links.insert(std::make_pair(idTo, link));
168 UDEBUG(
"(%d) neighbor ids changed from %d to %d",
_id, idFrom, idTo);
174 size_t sizeBefore =
_links.size();
175 if(keepSelfReferringLinks)
179 if(
iter->second.from() ==
iter->second.to())
193 if(
_links.size() != sizeBefore)
202 UDEBUG(
"Removed link %d from %d", idTo, this->
id());
228 _landmarks.insert(std::make_pair(landmark.
to(), landmark));
245 UDEBUG(
"Removed landmark %d from %d", landmarkId, this->
id());
254 float similarity = 0.0f;
263 UASSERT_MSG(dotProd>=0,
"Global descriptors should be normalized!");
264 similarity += dotProd;
271 similarity /= totalDescs;
275 const std::multimap<int, int> & words =
s.getWords();
277 if(!
s.isBadSignature() && !
this->isBadSignature())
279 std::list<std::pair<int, std::pair<int, int> > > pairs;
284 similarity =
float(pairs.size()) /
float(totalWords);
305 _wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
306 for(std::list<int>::const_iterator
iter=words.begin();
iter!=words.end(); ++
iter)
308 _words.insert(std::pair<int, int>(activeWordId, (*
iter)));
314 const std::vector<cv::KeyPoint> & keypoints,
315 const std::vector<cv::Point3f> & points,
316 const cv::Mat & descriptors)
318 UASSERT_MSG(descriptors.empty() || descriptors.rows == (
int)words.size(),
uFormat(
"words=%d, descriptors=%d", (
int)words.size(), descriptors.rows).c_str());
319 UASSERT_MSG(points.empty() || points.size() == words.size(),
uFormat(
"words=%d, points=%d", (
int)words.size(), (
int)points.size()).c_str());
320 UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(),
uFormat(
"words=%d, descriptors=%d", (
int)words.size(), (
int)keypoints.size()).c_str());
321 UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
324 for(std::multimap<int, int>::const_iterator
iter=words.begin();
iter!=words.end(); ++
iter)
331 UASSERT_MSG(
iter->second >=0 &&
iter->second < (
int)words.size(),
uFormat(
"iter->second=%d words.size()=%d",
iter->second, (
int)words.size()).c_str());
357 if(descriptors.empty())
377 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
382 if(
iter->second.kNeighbor)
385 if(
iter->second.to() <
iter->second.from())
387 covariance =
iter->second.infMatrix().inv();
399 total += _words.size() * (
sizeof(
int)*2+
sizeof(std::multimap<int, cv::KeyPoint>::iterator)) +
sizeof(std::multimap<int, cv::KeyPoint>);
400 total += _wordsKpts.size() *
sizeof(cv::KeyPoint) +
sizeof(std::vector<cv::KeyPoint>);
401 total += _words3.size() *
sizeof(cv::Point3f) +
sizeof(std::vector<cv::Point3f>);
402 total += _wordsDescriptors.empty()?0:_wordsDescriptors.total() * _wordsDescriptors.elemSize() +
sizeof(cv::Mat);
403 total += _wordsChanged.size() * (
sizeof(
int)*2+
sizeof(std::map<int, int>::iterator)) +
sizeof(std::map<int, int>);
406 total+=_sensorData.getMemoryUsed();
408 total += _pose.size() * (
sizeof(
Transform) +
sizeof(
float)*12);
409 total += _groundTruthPose.size() * (
sizeof(
Transform) +
sizeof(
float)*12);
410 total += _velocity.size() *
sizeof(
float);
411 total += _links.size() * (
sizeof(
int) +
sizeof(
Transform) + 12 *
sizeof(
float) +
sizeof(cv::Mat) + 36 *
sizeof(double)+
sizeof(std::multimap<int, Link>::iterator)) +
sizeof(std::multimap<int, Link>);
412 total += _landmarks.size() * (
sizeof(
int) +
sizeof(
Transform) + 12 *
sizeof(
float) +
sizeof(cv::Mat) + 36 *
sizeof(double)+
sizeof(std::map<int, Link>::iterator)) +
sizeof(std::map<int, Link>);