int get(int key) {
mymap::iterator it = m.find(key);
if (it == m.end()) {
return -1;
}
setused(it->second);
return it->second->value;
}
void insertelement(int key, int value) {
mymap::iterator it = m.find(key);
if (it == m.end()) {
Node* n = new Node(key, value);
insertathead(n);
m.insert(std::pair<int, Node*>(key, n));
if (m.size() > maxsize) {
invalidate();
}
} else {
it->second->value = value;
setused(it->second);
}
}
float Spinner::squaredErrorEstimation(sensor_msgs::PointCloud & cloud1, sensor_msgs::PointCloud & cloud2, mymap &fancyMap){
float error=0.0f;
mymap::iterator i = fancyMap.begin();
while(i!=fancyMap.end()){
std::pair<int,mapData> data=(*i);
pointpair p;
geometry_msgs::Point32 p1=this->cloud1.points.at(data.first);
geometry_msgs::Point32 p2=this->cloud2.points.at((data.second).first);
error+=compute2DSquaredDistance(p1,p2);
i++;
}
return error;
}
void invalidate() {
Node* n = removefromlist(tail);
if (n != 0) {
m.erase(n->key);
delete n;
}
}
float Spinner::weightedSquaredErrorEstimation(sensor_msgs::PointCloud & cloud1, sensor_msgs::PointCloud & cloud2, mymap &fancyMap){
float error=squaredErrorEstimation(cloud1,cloud2,fancyMap);
error=error/fancyMap.size();
return error;
}
