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);
    }
}