36 updateError_(
Parameters::defaultGridGlobalUpdateError()),
37 occupancyThr_(
Parameters::defaultGridGlobalOccupancyThr()),
38 logOddsHit_(logodds(
Parameters::defaultGridGlobalProbHit())),
39 logOddsMiss_(logodds(
Parameters::defaultGridGlobalProbMiss())),
40 logOddsClampingMin_(logodds(
Parameters::defaultGridGlobalProbClampingMin())),
41 logOddsClampingMax_(logodds(
Parameters::defaultGridGlobalProbClampingMax())),
95 unsigned long memoryUsage = 0;
97 memoryUsage +=
addedNodes_.size()*(
sizeof(
int) +
sizeof(
Transform)+
sizeof(
float)*12 +
sizeof(std::map<int, Transform>::iterator)) +
sizeof(std::map<int, Transform>);
104 UDEBUG(
"Update (poses=%d addedNodes_=%d)", (
int)poses.size(), (
int)
addedNodes_.size());
107 bool graphOptimized =
false;
112 std::map<int, Transform>::const_iterator jter = poses.find(
iter->first);
113 if(jter != poses.end())
115 graphChanged =
false;
116 UASSERT(!
iter->second.isNull() && !jter->second.isNull());
117 if(
iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
119 graphOptimized =
true;
124 UDEBUG(
"Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.",
iter->first);
128 if(graphOptimized || graphChanged)
134 std::list<std::pair<int, Transform> > orderedPoses;
137 for(std::map<int, Transform>::const_iterator
iter=poses.lower_bound(1);
iter!=poses.end(); ++
iter)
141 UDEBUG(
"Pose %d not found in current added poses, it will be added to map",
iter->first);
142 orderedPoses.push_back(*
iter);
147 if(poses.find(0) != poses.end())
149 orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
152 if(!orderedPoses.empty())
157 return !orderedPoses.empty();