void MapG2OReflector::copyEstimatesToG2O() {
for (std::map<MapNode*, g2o::VertexSE3*>::iterator it=_nm2g.begin();
it!=_nm2g.end(); it++){
MapNode* n = it->first;
g2o::VertexSE3* v = it->second;
v->setEstimate(n->transform());
}
}
/** This function can be optimized*/
void ManifoldVoronoiExtractor::process(Serializable* s)
{
put(s);
NewKeyNodeMessage* km = dynamic_cast<NewKeyNodeMessage*>(s);
int cx = _xSize * 0.5;
int cy = _ySize * 0.5;
float ires = 1./_resolution;
if(km)
{
PwnCloudCache::HandleType h = _cache->get((SyncSensorDataNode*) km->keyNode);
cacheHandles.push_back(h);
while(cacheHandles.size() > _dequeSize)
{
cacheHandles.pop_front();
}
MapNode* n = km->keyNode;
Isometry3d inT = n->transform().inverse();
ManifoldVoronoiData* vdata = new ManifoldVoronoiData();
vdata->resolution = _resolution;
boss_map::ImageBLOB* imageBlob = new boss_map::ImageBLOB();
imageBlob->cvImage().create(_xSize,_ySize, CV_16UC1);
imageBlob->cvImage().setTo(30000);
imageBlob->adjustFormat();
vdata->setTimestamp(0);
uint16_t obstacle = 65535;
for(list<PwnCloudCache::HandleType>::iterator it = cacheHandles.begin(); it != cacheHandles.end(); it++)
{
PwnCloudCache::HandleType& h = *it;
CloudWithImageSize* cloud_ = h.get();
MapNode* cn = h.key();
Isometry3d currentTransform=inT * cn->transform();
pwn::Cloud cloud = *cloud_;
Isometry3f currentTransformf;
convertScalar(currentTransformf, currentTransform);
cloud.transformInPlace( currentTransformf);
for(size_t i = 0; i < cloud.points().size(); i++)
{
pwn::Normal& n = cloud.normals()[i];
pwn::Point& p = cloud.points()[i];
int x = cx + p.x()*ires;
int y = cy + p.y()*ires;
if ( (x<0) ||
(x>=_xSize) ||
(y < 0) ||
(y>=_ySize) ){
continue;
}
uint16_t& imZ = imageBlob->cvImage().at<uint16_t>(x,y);
int pz = 10000 - 1000 * p.z();
if(imZ == obstacle)
{
continue;
}
if(imZ < pz)
{
continue;
}
if(n.squaredNorm() < 0.1)
{
continue;
}
if(n.z() < _normalThreshold)
{
imZ = obstacle;
}
else
{
imZ = pz;
}
}
}
vdata->imageBlob().set(imageBlob);
vdata->node = n;
put(vdata);
}
}