Ejemplo n.º 1
0
bool GraphNode::mouseButtonEvent(const Eigen::Vector2i &p, int button, bool down, int modifiers)
{
    spdlog::get("qde")->debug(
        "GraphNode '{}': Received mouse button event at ({},{}): {}, {}",
        mTitle, p.x(), p.y(), button, down
    );

    Window::mouseButtonEvent(p, button, down, modifiers);

    if (button == GLFW_MOUSE_BUTTON_1 && mEnabled && down) {
        Graph *parentGraph = dynamic_cast<Graph *>(mParent);
        parentGraph->nodeSelectedEvent(this);

        return true;
    }
    else if (button == GLFW_MOUSE_BUTTON_2 && mEnabled && down) {
        int offsetX = p.x() - mPos.x();
        int offsetY = p.y() - mPos.y();
        Eigen::Vector2i position(offsetX, offsetY);
        mPopup->setAnchorPos(position);
        mPopup->setVisible(!mPopup->visible());

        return true;
    }

    return false;
}
void DepthObstacleGrid::setDepth(double x, double y, double depth, double variance){
  
  Eigen::Vector2i ID = getCellID(x,y);
  //std::cout << "idX: " << idX << " span: " << span.x() << " resolution: " << resolution << " x: " << x << " pos: " << position.x() << std::endl;
  
  if(ID.x() < 0)
    return;
  
  GridElement &elem = get(ID.x(), ID.y());
  
  if(variance == 0.0){
    elem.pos = base::Vector2d(x,y);
  }
  else if( std::isinf(elem.depth_variance)){
    elem.depth = depth;
    elem.depth_variance = variance;
  }
  else{
    double k = elem.depth_variance / (variance + elem.depth_variance);
    
    elem.depth = elem.depth + ( k * (depth - elem.depth ) ) ;
    elem.depth_variance = elem.depth_variance - (k * elem.depth_variance);
    
  }
  
}
Ejemplo n.º 3
0
bool Font::FontTexture::findEmpty(const Eigen::Vector2i& size, Eigen::Vector2i& cursor_out)
{
	if(size.x() >= textureSize.x() || size.y() >= textureSize.y())
		return false;

	if(writePos.x() + size.x() >= textureSize.x() &&
		writePos.y() + rowHeight + size.y() + 1 < textureSize.y())
	{
		// row full, but it should fit on the next row
		// move cursor to next row
		writePos << 0, writePos.y() + rowHeight + 1; // leave 1px of space between glyphs
		rowHeight = 0;
	}

	if(writePos.x() + size.x() >= textureSize.x() ||
		writePos.y() + size.y() >= textureSize.y())
	{
		// nope, still won't fit
		return false;
	}

	cursor_out = writePos;
	writePos[0] += size.x() + 1; // leave 1px of space between glyphs

	if(size.y() > rowHeight)
		rowHeight = size.y();

	return true;
}
BuoyColor DepthObstacleGrid::getBuoy(double x, double y){
  
  Eigen::Vector2i ID = getCellID(x,y);
  
  if(ID.x() < 0)
    return NO_BUOY;
  
  GridElement elem = get(ID.x(), ID.y());
  
  if(elem.orange_buoy_confidence > elem.white_buoy_confidence){
    if(elem.orange_buoy_confidence > 0.0){
      return ORANGE;
    }
  }
  else{
    if(elem.white_buoy_confidence > 0.0){
      return WHITE;
    }
  }
  
  if(elem.buoy_confidence > 0.0){
    return UNKNOWN;
  }
    
  return NO_BUOY;
  
  
}
Ejemplo n.º 5
0
Font::Glyph* Font::getGlyph(UnicodeChar id)
{
	// is it already loaded?
	auto it = mGlyphMap.find(id);
	if(it != mGlyphMap.end())
		return &it->second;

	// nope, need to make a glyph
	FT_Face face = getFaceForChar(id);
	if(!face)
	{
		LOG(LogError) << "Could not find appropriate font face for character " << id << " for font " << mPath;
		return NULL;
	}

	FT_GlyphSlot g = face->glyph;

	if(FT_Load_Char(face, id, FT_LOAD_RENDER))
	{
		LOG(LogError) << "Could not find glyph for character " << id << " for font " << mPath << ", size " << mSize << "!";
		return NULL;
	}

	Eigen::Vector2i glyphSize(g->bitmap.width, g->bitmap.rows);

	FontTexture* tex = NULL;
	Eigen::Vector2i cursor;
	getTextureForNewGlyph(glyphSize, tex, cursor);

	// getTextureForNewGlyph can fail if the glyph is bigger than the max texture size (absurdly large font size)
	if(tex == NULL)
	{
		LOG(LogError) << "Could not create glyph for character " << id << " for font " << mPath << ", size " << mSize << " (no suitable texture found)!";
		return NULL;
	}

	// create glyph
	Glyph& glyph = mGlyphMap[id];
	
	glyph.texture = tex;
	glyph.texPos << cursor.x() / (float)tex->textureSize.x(), cursor.y() / (float)tex->textureSize.y();
	glyph.texSize << glyphSize.x() / (float)tex->textureSize.x(), glyphSize.y() / (float)tex->textureSize.y();

	glyph.advance << (float)g->metrics.horiAdvance / 64.0f, (float)g->metrics.vertAdvance / 64.0f;
	glyph.bearing << (float)g->metrics.horiBearingX / 64.0f, (float)g->metrics.horiBearingY / 64.0f;

	// upload glyph bitmap to texture
	glBindTexture(GL_TEXTURE_2D, tex->textureId);
	glTexSubImage2D(GL_TEXTURE_2D, 0, cursor.x(), cursor.y(), glyphSize.x(), glyphSize.y(), GL_ALPHA, GL_UNSIGNED_BYTE, g->bitmap.buffer);
	glBindTexture(GL_TEXTURE_2D, 0);

	// update max glyph height
	if(glyphSize.y() > mMaxGlyphHeight)
		mMaxGlyphHeight = glyphSize.y();

	// done
	return &glyph;
}
Ejemplo n.º 6
0
bool PathCmp::checkValidTile (Eigen::Vector2i idx_) {
    if (0 <=idx_.x() && idx_.x() < _level->getXSize()
            && 0 <= idx_.y() && idx_.y() < _level->getYSize()) {
        if (_level->getTile(idx_.x(), idx_.y()))
            return false;
        return true;
    }
    return false;
}
bool DepthObstacleGrid::setBuoy( double x, double y, BuoyColor color, double confidence, bool no_neigbors){
  
  Eigen::Vector2i ID = getCellID(x,y);
  //std::cout << "idX: " << idX << " span: " << span.x() << " resolution: " << resolution << " x: " << x << " pos: " << position.x() << std::endl;
  
  if(ID.x() < 0)
    return false;
  
  bool ignore = false;
  
  if(no_neigbors){
    
    for(signed int i = -1; i <= 1; i++){
     
      for(signed int j = -1; j <= 1; j++){
        
        if(isValid( ID.x() + i, ID.y() + j)){
          
          GridElement neighbor = get(ID.x() + i, ID.y() + j);
          
          if(neighbor.buoy_confidence > 0.0){
            ignore = true;
          }
          
        }
        
      }      
    }   
    
    
  }else{
    ignore = false;
  }
  
  if(ignore){
    return false;
  }
  
  GridElement &elem = get(ID.x(), ID.y()); 
  
  if(confidence > 0.0){
    
    elem.buoy_confidence = elem.buoy_confidence + confidence - (elem.buoy_confidence * confidence);
    
    if(color == WHITE){
      elem.white_buoy_confidence = elem.white_buoy_confidence + confidence - (elem.white_buoy_confidence * confidence);
    }
    else if(color == ORANGE){
      elem.orange_buoy_confidence = elem.orange_buoy_confidence + confidence - (elem.orange_buoy_confidence * confidence);
    }
    else if(color == YELLOW){
      elem.yellow_buoy_confidence = elem.yellow_buoy_confidence + confidence - (elem.yellow_buoy_confidence * confidence);
    }    
  }  
  
  return true;
}  
Ejemplo n.º 8
0
Eigen::Matrix3f TextureWarpShader::adjustHomography(const Eigen::Vector2i &imageSize, const Eigen::Matrix3fr &H)
{
	Eigen::Matrix3f finalH;
	Eigen::Matrix3fr normHR = Eigen::Matrix3fr::Identity();
	normHR(0, 0) = (float)(imageSize.x() - 1);
	normHR(1, 1) = (float)(imageSize.y() - 1);
	Eigen::Matrix3fr normHL = Eigen::Matrix3fr::Identity();
	normHL(0, 0) = 1.0f / (imageSize.x() - 1);
	normHL(1, 1) = 1.0f / (imageSize.y() - 1);

	return (normHL * H * normHR);
}
bool ComponentGrid::moveCursor(Eigen::Vector2i dir)
{
	assert(dir.x() || dir.y());

	const Eigen::Vector2i origCursor = mCursor;

	GridEntry* currentCursorEntry = getCellAt(mCursor);

	Eigen::Vector2i searchAxis(dir.x() == 0, dir.y() == 0);
	
	while(mCursor.x() >= 0 && mCursor.y() >= 0 && mCursor.x() < mGridSize.x() && mCursor.y() < mGridSize.y())
	{
		mCursor = mCursor + dir;

		Eigen::Vector2i curDirPos = mCursor;

		GridEntry* cursorEntry;
		//spread out on search axis+
		while(mCursor.x() < mGridSize.x() && mCursor.y() < mGridSize.y()
			&& mCursor.x() >= 0 && mCursor.y() >= 0)
		{
			cursorEntry = getCellAt(mCursor);
			if(cursorEntry && cursorEntry->canFocus && cursorEntry != currentCursorEntry)
			{
				onCursorMoved(origCursor, mCursor);
				return true;
			}

			mCursor += searchAxis;
		}

		//now again on search axis-
		mCursor = curDirPos;
		while(mCursor.x() >= 0 && mCursor.y() >= 0
			&& mCursor.x() < mGridSize.x() && mCursor.y() < mGridSize.y())
		{
			cursorEntry = getCellAt(mCursor);
			if(cursorEntry && cursorEntry->canFocus && cursorEntry != currentCursorEntry)
			{
				onCursorMoved(origCursor, mCursor);
				return true;
			}

			mCursor -= searchAxis;
		}

		mCursor = curDirPos;
	}

	//failed to find another focusable element in this direction
	mCursor = origCursor;
	return false;
}
double DepthObstacleGrid::getDepth( double x, double y){
  
  Eigen::Vector2i ID = getCellID(x,y);
  
  if(ID.x() < 0)
    return NAN;
  
  GridElement elem = get(ID.x(), ID.y());
  
  return elem.depth;
  
}
void DepthObstacleGrid::setObstacle(double x, double y, bool obstacle, double confidence){

  Eigen::Vector2i ID = getCellID(x,y);
  //std::cout << "idX: " << idX << " span: " << span.x() << " resolution: " << resolution << " x: " << x << " pos: " << position.x() << std::endl;
  
  if(ID.x() < 0)
    return;
  
  GridElement &elem = get(ID.x(), ID.y());
  
  if( (!elem.obstacle) && obstacle){
      elem.init_confidences(min_depth, max_depth, depth_resolution);
    
      elem.obstacle = true;
      elem.obstacle_confidence = confidence + 0.5 - (confidence * 0.5);
      setObstacleDepthConfidence(elem.obstacle_depth_confidence, min_depth, max_depth, confidence, obstacle);
      elem.obstacle_count++;
    
  }  
  else if(confidence > 0.0){
   
    setObstacleDepthConfidence(elem.obstacle_depth_confidence, min_depth, max_depth, confidence, obstacle);
    
    if(obstacle == false && elem.obstacle && confidence > 0){ //we do not see an old feature --> reduce feature confidence
      //feature.obstacle_confidence = ((1.0 - confidence) + f.obstacle_confidence) * 0.5;
      
      //Confidence, that the cell is empty
      double confidence_empty = (1.0 - elem.obstacle_confidence) + confidence - ((1.0 - elem.obstacle_confidence) * confidence);
      
      //Obstacle ceonfidence is reverse of empty-confidence
      elem.obstacle_confidence = 1.0 - confidence_empty;
      
    }else if(obstacle == elem.obstacle && obstacle && confidence > 0){ // We recognized the feature before, update the confidence 
      //feature.obstacle_confidence = (confidence + f.obstacle_confidence) * 0.5 ;
      elem.obstacle_confidence = elem.obstacle_confidence + confidence - (elem.obstacle_confidence * confidence);
      elem.obstacle_count++;
    }
    else if(obstacle && elem.obstacle == false && confidence > 0){ //we recognize this feature for the first time
      elem.obstacle = true;
      
       //The confidence before was 0.5, because we did not know, if the cell is empty or occupied
      elem.obstacle_confidence = confidence + 0.5 - (confidence * 0.5);
      elem.obstacle_count++;
    }
      
    if(elem.obstacle_confidence <= obstacle_confidence_threshold && !elem.is_obstacle(obstacle_confidence_threshold) && elem.obstacle_count < obstacle_count_threshold){ // We have no confidence in this feature
      elem.obstacle = false; 
      elem.obstacle_count = 0;
    } 
    
  }
   
}
ComponentGrid::ComponentGrid(Window* window, const Eigen::Vector2i& gridDimensions) : GuiComponent(window), 
	mGridSize(gridDimensions), mCursor(0, 0)
{
	assert(gridDimensions.x() > 0 && gridDimensions.y() > 0);

	mCells.reserve(gridDimensions.x() * gridDimensions.y());

	mColWidths = new float[gridDimensions.x()];
	mRowHeights = new float[gridDimensions.y()];
	for(int x = 0; x < gridDimensions.x(); x++)
		mColWidths[x] = 0;
	for(int y = 0; y < gridDimensions.y(); y++)
		mRowHeights[y] = 0;
}
Ejemplo n.º 13
0
int PathCmp::calcCostG (PathNode* node_, PathNode* parentNode_) {
    Eigen::Vector2i nodeIdx = node_->getIndex();
    Eigen::Vector2i parentIdx;
    if (parentNode_)
        parentIdx = parentNode_->getIndex();
    else
        parentIdx = _srcIdx;
    int costToAdd = 10;
    if (abs(nodeIdx.x() - parentIdx.x()) == 1
            && abs(nodeIdx.y() - parentIdx.y()) == 1)
        costToAdd = 14;

    return costToAdd;
}
bool DepthObstacleGrid::getObstacle( double x, double y){
  
  Eigen::Vector2i ID = getCellID(x,y);
  
  if(ID.x() < 0)
    return false;
  
  GridElement elem = get(ID.x(), ID.y());
  
  if(elem.obstacle_confidence <= 0.0)
    return false;
  else
    return elem.obstacle;
  
}
Ejemplo n.º 15
0
Terrain::Terrain(Eigen::Vector2f a_size, Eigen::Vector2i a_res, int n_hill, TerrainType a_type){
	m_type = TypeTerrain;
	m_frictness = 10;
	
	InitBase(a_size.x(), a_size.y(), a_res.x(), a_res.y(), n_hill, a_type);

	InitDraw();
}
Ejemplo n.º 16
0
void TileLogicCmp::onDestroyed (OnTileDestroyed* event_){
	if (!event_->_tile)
		return;
  if(event_->_tile == getEntity()){
    auto level = Director::getInstance()->getRunningScene()->getLevel();
    auto pos = getEntity()->GET_CMP(PositionComponent)->getPosition();
    Eigen::Vector2i idx = level->posToTileIndex(pos);
    level->removeCell(idx.x(), idx.y());
		event_->_tile = nullptr;}}
void DepthObstacleGrid::initializeStatics(NodeMap *map){
  
  Environment env = map->getEnvironment();
  
  for(std::vector<Plane>::iterator it = env.planes.begin(); it != env.planes.end(); it++){
    
    double plane_length = Eigen::Vector2d( it->span_horizontal.x(), it->span_horizontal.y() ).norm();
    Eigen::Vector2d step = Eigen::Vector2d( it->span_horizontal.x(), it->span_horizontal.y() ) / (plane_length / (0.5 * resolution) );
    double step_length = step.norm();
        
    Eigen::Vector2d lastCell(NAN, NAN);
    Eigen::Vector2d pos = Eigen::Vector2d(it->position.x(), it->position.y());
    
    //iterate through the cells
    for(double i = 0.0; i < plane_length; i += step_length){
      
      Eigen::Vector2d cell_coord = getGridCoord( pos.x(), pos.y() );
      
      //step was not wide enough, we are still in the same cell
      if(cell_coord != lastCell){       
      
        lastCell = cell_coord;
        
        Eigen::Vector2i ID = getCellID(cell_coord.x(), cell_coord.y());
        
        //Step was invalid, we are outside the grid
        if(ID.x() == -1){
          pos += step;
          continue;
        }
        
        //Set the cell static
        GridElement &elem = get(ID.x(), ID.y());      
        elem.static_obstacle = true;
                
      }
      
      pos += step;
      
    }
    
  }
  
}
Ejemplo n.º 18
0
void GraphNodeLink::draw(NVGcontext* ctx)
{
    auto sourceSize = mSource->size().cast<float>();

    Eigen::Vector2i inputPosition(
        mSource->absolutePosition().x() - mParent->absolutePosition().x() + (sourceSize.x() * 0.5f),
        mSource->absolutePosition().y() - mParent->absolutePosition().y() + (sourceSize.y() * 0.5f)
    );
    Eigen::Vector2i outputPosition(Eigen::Vector2i::Zero());

    if (hasTarget()) {
        // Get relative position of parent (node) of the target (sink)
        Eigen::Vector2i delta = mSink->parent()->absolutePosition() - mSink->parent()->position();
        delta.x() -= (sourceSize.x() * 0.5f);
        delta.y() -= (sourceSize.y() * 0.5f);
        outputPosition = mSink->absolutePosition() - delta;
    }
    else {
        Eigen::Vector2i offset = mSource->absolutePosition() - mParent->absolutePosition();
        Eigen::Vector2i delta = mTargetPosition - mSource->position();
        outputPosition = offset + delta;
    }

    Eigen::Vector2i positionDiff = outputPosition - inputPosition;

    NVGcontext* vg = ctx;

    nvgStrokeColor(vg, nvgRGBA(131, 148, 150, 255));
    nvgStrokeWidth(vg, 2.0f);
    nvgBeginPath(vg);
    nvgMoveTo(
        vg,
        inputPosition.x(),
        inputPosition.y()
    );
    nvgQuadTo(vg,
        1.2 * positionDiff.x(), positionDiff.y(),
        outputPosition.x(), outputPosition.y()
    );
    nvgStroke(vg);

    Widget::draw(ctx);
}
Ejemplo n.º 19
0
void KisColorSelectorRing::paintCache()
{
    QImage cache(m_cachedSize, m_cachedSize, QImage::Format_ARGB32_Premultiplied);

    Eigen::Vector2i center(cache.width()/2., cache.height()/2.);

    for(int x=0; x<cache.width(); x++) {
        for(int y=0; y<cache.height(); y++) {
            Eigen::Vector2i currentPoint((float)x, (float)y);
            Eigen::Vector2i relativeVector = currentPoint-center;

            qreal currentRadius = relativeVector.squaredNorm();
            currentRadius=sqrt(currentRadius);

            if(currentRadius < outerRadius()+1
               && currentRadius > innerRadius()-1)
            {

                float angle = std::atan2((float)relativeVector.y(), (float)relativeVector.x())+((float)M_PI);
                angle/=2*((float)M_PI);
                angle*=359.f;
                if(currentRadius < outerRadius()
                   && currentRadius > innerRadius()) {
                    cache.setPixel(x, y, m_cachedColors.at(angle));
                }
                else {
                    // draw antialiased border
                    qreal coef=1.;
                    if(currentRadius > outerRadius()) {
                        // outer border
                        coef-=currentRadius;
                        coef+=outerRadius();
                    }
                    else {
                        // inner border
                        coef+=currentRadius;
                        coef-=innerRadius();
                    }
                    coef=qBound(qreal(0.), coef, qreal(1.));
                    int red=qRed(m_cachedColors.at(angle));
                    int green=qGreen(m_cachedColors.at(angle));
                    int blue=qBlue(m_cachedColors.at(angle));

                    // the format is premultiplied, so we have to take care of that
                    QRgb color = qRgba(red*coef, green*coef, blue*coef, 255*coef);
                    cache.setPixel(x, y, color);
                }
            }
            else {
                cache.setPixel(x, y, qRgba(0,0,0,0));
            }
        }
    }
    m_pixelCache = cache;
}
void ComponentGrid::setEntry(const std::shared_ptr<GuiComponent>& comp, const Eigen::Vector2i& pos, bool canFocus, bool resize, const Eigen::Vector2i& size,
	unsigned int border, GridFlags::UpdateType updateType)
{
	assert(pos.x() >= 0 && pos.x() < mGridSize.x() && pos.y() >= 0 && pos.y() < mGridSize.y());
	assert(comp != nullptr);
	assert(comp->getParent() == NULL);

	GridEntry entry(pos, size, comp, canFocus, resize, updateType, border);
	mCells.push_back(entry);

	addChild(comp.get());

	if(!cursorValid() && canFocus)
	{
		auto origCursor = mCursor;
		mCursor = pos;
		onCursorMoved(origCursor, mCursor);
	}

	updateCellComponent(mCells.back());
	updateSeparators();
}
Ejemplo n.º 21
0
void GridLineTraversal::gridLine(Eigen::Vector2i start, Eigen::Vector2i end, GridLineTraversalLine *line) {
  int i, j;
  int half;
  Eigen::Vector2i v;
  gridLineCore(start, end, line);
  if(start.x() != line->points[0].x() || start.y() != line->points[0].y()) {
    half = line->numPoints / 2;
    for(i = 0, j = line->numPoints - 1; i < half; i++, j--) {
      v = line->points[i];
      line->points[i] = line->points[j];
      line->points[j] = v;
    }
  }
}
Ejemplo n.º 22
0
	void pushClipRect(Eigen::Vector2i pos, Eigen::Vector2i dim)
	{
		Eigen::Vector4i box(pos.x(), pos.y(), dim.x(), dim.y());
		if(box[2] == 0)
			box[2] = Renderer::getScreenWidth() - box.x();
		if(box[3] == 0)
			box[3] = Renderer::getScreenHeight() - box.y();

		//glScissor starts at the bottom left of the window
		//so (0, 0, 1, 1) is the bottom left pixel
		//everything else uses y+ = down, so flip it to be consistent
		//rect.pos.y = Renderer::getScreenHeight() - rect.pos.y - rect.size.y;
		box[1] = Renderer::getScreenHeight() - box.y() - box[3];

		//make sure the box fits within clipStack.top(), and clip further accordingly
		if(clipStack.size())
		{
			Eigen::Vector4i& top = clipStack.top();
			if(top[0] > box[0])
				box[0] = top[0];
			if(top[1] > box[1])
				box[1] = top[1];
			if(top[0] + top[2] < box[0] + box[2])
				box[2] = (top[0] + top[2]) - box[0];
			if(top[1] + top[3] < box[1] + box[3])
				box[3] = (top[1] + top[3]) - box[1];
		}

		if(box[2] < 0)
			box[2] = 0;
		if(box[3] < 0)
			box[3] = 0;

		clipStack.push(box);
		glScissor(box[0], box[1], box[2], box[3]);
		glEnable(GL_SCISSOR_TEST);
	}
Ejemplo n.º 23
0
bool envire::Bresenham::getNextPoint(Eigen::Vector2i& p)
{
    return getNextPoint(p.x(), p.y());
}
Ejemplo n.º 24
0
envire::Bresenham::Bresenham(const Eigen::Vector2i& start, const Eigen::Vector2i& end)
{
    init(start.x(), start.y(), end.x(), end.y());
}
Ejemplo n.º 25
0
void Graph2occupancy::computeMap(){

  // Sort verteces
  vector<int> vertexIds(_graph->vertices().size());
  int k = 0;
  for(OptimizableGraph::VertexIDMap::iterator it = _graph->vertices().begin(); it != _graph->vertices().end(); ++it) {
    vertexIds[k++] = (it->first);
  }  
  sort(vertexIds.begin(), vertexIds.end());


  /************************************************************************
   *                          Compute map size                            *
   ************************************************************************/
  // Check the entire graph to find map bounding box
  Matrix2d boundingBox = Matrix2d::Zero();
  std::vector<RobotLaser*> robotLasers;
  std::vector<SE2> robotPoses;
  double xmin=std::numeric_limits<double>::max();
  double xmax=std::numeric_limits<double>::min();
  double ymin=std::numeric_limits<double>::max();
  double ymax=std::numeric_limits<double>::min();

  SE2 baseTransform(0,0,_angle);
  bool initialPoseFound = false;
  SE2 initialPose;
  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = _graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    if (v->fixed() && !initialPoseFound){
      initialPoseFound = true;
      initialPose = baseTransform*v->estimate();
    }
    OptimizableGraph::Data *d = v->userData();

    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      robotLasers.push_back(robotLaser);
      SE2 transformed_estimate = baseTransform*v->estimate();
      robotPoses.push_back(transformed_estimate);

      double x = transformed_estimate.translation().x();
      double y = transformed_estimate.translation().y();
      
      xmax = xmax > x+_usableRange ? xmax : x + _usableRange;
      ymax = ymax > y+_usableRange ? ymax : y + _usableRange;
      xmin = xmin < x-_usableRange ? xmin : x - _usableRange;
      ymin = ymin < y-_usableRange ? ymin : y - _usableRange;
 
      d = d->next();
    }
  }

  boundingBox(0,0)=xmin;
  boundingBox(1,0)=ymin;
  boundingBox(0,1)=xmax;
  boundingBox(1,1)=ymax;


  if(robotLasers.size() == 0)  {
    std::cout << "No laser scans found ... quitting!" << std::endl;
    return;
  }

  /************************************************************************
   *                          Compute the map                             *
   ************************************************************************/
  // Create the map
  Vector2i size;
  Vector2f offset;
  if(_rows != 0 && _cols != 0) { 
    size = Vector2i(_rows, _cols); 
  }
  else {
    size = Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ _resolution, 
		    (boundingBox(1, 1) - boundingBox(1, 0))/ _resolution);
  } 



  if(size.x() == 0 || size.y() == 0) {
    std::cout << "Zero map size ... quitting!" << std::endl;
    return;
  }


  offset = Eigen::Vector2f(boundingBox(0, 0),boundingBox(1, 0));
  FrequencyMapCell unknownCell;
  
  _map = FrequencyMap(_resolution, offset, size, unknownCell);

  for (size_t i = 0; i < robotPoses.size(); ++i) {
    _map.integrateScan(robotLasers[i], robotPoses[i], _maxRange, _usableRange, _infinityFillingRange, _gain, _squareSize);
  }

  /************************************************************************
   *                  Convert frequency map into int[8]                   *
   ************************************************************************/

  *_mapImage = cv::Mat(_map.rows(), _map.cols(), CV_8UC1);
  _mapImage->setTo(cv::Scalar(0));

  for(int c = 0; c < _map.cols(); c++) {
    for(int r = 0; r < _map.rows(); r++) {
      if(_map(r, c).misses() == 0 && _map(r, c).hits() == 0) {
	_mapImage->at<unsigned char>(r, c) = _unknownColor; }
      else {
	float fraction = (float)_map(r, c).hits()/(float)(_map(r, c).hits()+_map(r, c).misses());
	if (_freeThreshold && fraction < _freeThreshold){
	  _mapImage->at<unsigned char>(r, c) = _freeColor; }
	else if (_threshold && fraction > _threshold){
	  _mapImage->at<unsigned char>(r, c) = _occupiedColor; }
	else {
	  _mapImage->at<unsigned char>(r, c) = _unknownColor; }
      }
          
    }
  }

  Eigen::Vector2f origin(0.0f, 0.0f);
  if (initialPoseFound){
    Eigen::Vector2i originMap = _map.world2map(Eigen::Vector2f(initialPose.translation().x(),
							       initialPose.translation().y()));
    origin = Eigen::Vector2f(((-_resolution * originMap.y())+initialPose.translation().y()),
			     -(_resolution * (_mapImage->rows-originMap.x()) +initialPose.translation().x()));
    
  }

  _mapCenter = origin;

}
Ejemplo n.º 26
0
 explicit Index( const Eigen::Vector2i &index ) : x(index.x()), y(index.y()) {}
Ejemplo n.º 27
0
/**
 * Convert from pixel coordinates to camera image plane coordinates
 * @param image_coordinate The 2D coordinate in the image space
 * @return camera_coordinate The 2D coordinate in camera image plane
 */
Eigen::Vector2f Camera::pixel_to_image_plane( const Eigen::Vector2i & image_coordinate ) const {
    using namespace Eigen;

    return pixel_to_image_plane(image_coordinate.x(), image_coordinate.y() );
}
Ejemplo n.º 28
0
void GridLineTraversal::gridLineCore(Eigen::Vector2i start, Eigen::Vector2i end, GridLineTraversalLine *line ) {
  int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag;
  int cnt = 0;

  dx = std::abs(end.x() - start.x());
  dy = std::abs(end.y() - start.y());
  
  if(dy <= dx) {
    d = 2 * dy - dx; 
    incr1 = 2 * dy; 
    incr2 = 2 * (dy - dx);
    if(start.x() > end.x()) {
      x = end.x(); 
      y = end.y();
      ydirflag = (-1);
      xend = start.x();
    } 
    else {
      x = start.x(); 
      y = start.y();
      ydirflag = 1;
      xend = end.x();
    }
    line->points[cnt].x() = x;
    line->points[cnt].y() = y;
    cnt++;
    if(((end.y() - start.y()) * ydirflag) > 0) {
      while(x < xend) {
	x++;
	if(d < 0) {
	  d += incr1;
	} 
	else {
	  y++; 
	  d += incr2;
	}
	line->points[cnt].x() = x;
	line->points[cnt].y() = y;
	cnt++;
      }
    } 
    else {
      while(x < xend) {
	x++;
	if(d < 0) {
	  d += incr1;
	} 
	else {
	  y--; 
	  d += incr2;
	}
	line->points[cnt].x() = x;
	line->points[cnt].y() = y;
	cnt++;
      }
    }		
  } 
  else {
    d = 2 * dx - dy;
    incr1 = 2 * dx; 
    incr2 = 2 * (dx - dy);
    if(start.y() > end.y()) {
      y = end.y(); 
      x = end.x();
      yend = start.y();
      xdirflag = (-1);
    } 
    else {
      y = start.y(); 
      x = start.x();
      yend = end.y();
      xdirflag = 1;
    }
    line->points[cnt].x() = x;
    line->points[cnt].y() = y;
    cnt++;
    if(((end.x() - start.x()) * xdirflag) > 0) {
      while(y < yend) {
	y++;
	if(d < 0) {
	  d += incr1;
	} 
	else {
	  x++; 
	  d += incr2;
	}
	line->points[cnt].x()=x;
	line->points[cnt].y()=y;
	cnt++;
      }
    } 
    else {
      while(y < yend) {
	y++;
	if(d < 0) {
	  d += incr1;
	} 
	else {
	  x--; 
	  d += incr2;
	}
	line->points[cnt].x() = x;
	line->points[cnt].y() = y;
	cnt++;
      }
    }
  }
  line->numPoints = cnt;
}
Ejemplo n.º 29
0
inline void Histogram::insert_point<Histogram::Dim>( const Point& input ) {
    Eigen::Vector2i bin = floor(input / bin_size).cast<int>();
    assert( bin.x() >= -1 && bin.y() >= -1 && bin.x() <= bins.width_in_pixels() - 2 && bin.y() <= bins.height_in_pixels() - 2 );
    bins( bin.x() + 1, bin.y() + 1 ).push_back( input );
}
Ejemplo n.º 30
0
int main(int argc, char **argv) {
  /************************************************************************
   *                          Input handling                              *
   ************************************************************************/
  float rows, cols, gain, square_size;
  float resolution, max_range, usable_range, angle, threshold;
  string g2oFilename, mapFilename;
  g2o::CommandArgs arg;
  arg.param("resolution", resolution, 0.05f, "resolution of the map (how much is in meters a pixel)");
  arg.param("threshold", threshold, -1.0f, "threshold to apply to the frequency map (values under the threshold are discarded)");
  arg.param("rows", rows, 0, "impose the resulting map to have this number of rows");
  arg.param("cols", cols, 0, "impose the resulting map to have this number of columns");
  arg.param("max_range", max_range, -1.0f, "max laser range to consider for map building");
  arg.param("usable_range", usable_range, -1.0f, "usable laser range for map building");
  arg.param("gain", gain, 1, "gain to impose to the pixels of the map");
  arg.param("square_size", square_size, 1, "square size of the region where increment the hits");
  arg.param("angle", angle, 0, "rotate the map of x degrees");
  arg.paramLeftOver("input_graph.g2o", g2oFilename, "", "input g2o graph to use to build the map", false);
  arg.paramLeftOver("output_map", mapFilename, "", "output filename where to save the map (without extension)", false);  
  arg.parseArgs(argc, argv);

  angle = angle*M_PI/180.0;

  /************************************************************************
   *                          Loading Graph                               *
   ************************************************************************/
  // Load graph
  typedef BlockSolver< BlockSolverTraits<-1, -1> >  SlamBlockSolver;
  typedef LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
  SlamLinearSolver *linearSolver = new SlamLinearSolver();
  linearSolver->setBlockOrdering(false);
  SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);
  OptimizationAlgorithmGaussNewton *solverGauss = new OptimizationAlgorithmGaussNewton(blockSolver);
  SparseOptimizer *graph = new SparseOptimizer();
  graph->setAlgorithm(solverGauss);    
  graph->load(g2oFilename.c_str());
  
  // Sort verteces
  vector<int> vertexIds(graph->vertices().size());
  int k = 0;
  for(OptimizableGraph::VertexIDMap::iterator it = graph->vertices().begin(); it != graph->vertices().end(); ++it) {
    vertexIds[k++] = (it->first);
  }  
  sort(vertexIds.begin(), vertexIds.end());
  
  /************************************************************************
   *                          Compute map size                            *
   ************************************************************************/
  // Check the entire graph to find map bounding box
  Eigen::Matrix2d boundingBox = Eigen::Matrix2d::Zero();
  std::vector<RobotLaser*> robotLasers;
  std::vector<SE2> robotPoses;
  double xmin=std::numeric_limits<double>::max();
  double xmax=std::numeric_limits<double>::min();
  double ymin=std::numeric_limits<double>::max();
  double ymax=std::numeric_limits<double>::min();

  SE2 baseTransform(0,0,angle);

  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    v->setEstimate(baseTransform*v->estimate());
    OptimizableGraph::Data *d = v->userData();

    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      robotLasers.push_back(robotLaser);
      robotPoses.push_back(v->estimate());
      double x = v->estimate().translation().x();
      double y = v->estimate().translation().y();
      
      xmax = xmax > x+usable_range ? xmax : x+usable_range;
      ymax = ymax > y+usable_range ? ymax : y+usable_range;
      xmin = xmin < x-usable_range ? xmin : x-usable_range;
      ymin = ymin < y-usable_range ? ymin : y-usable_range;
 
      d = d->next();
    }
  }

  boundingBox(0,0)=xmin;
  boundingBox(0,1)=xmax;
  boundingBox(1,0)=ymin;
  boundingBox(1,1)=ymax;

  std::cout << "Found " << robotLasers.size() << " laser scans"<< std::endl;
  std::cout << "Bounding box: " << std::endl << boundingBox << std::endl; 
  if(robotLasers.size() == 0)  {
    std::cout << "No laser scans found ... quitting!" << std::endl;
    return 0;
  }

  /************************************************************************
   *                          Compute the map                             *
   ************************************************************************/
  // Create the map
  Eigen::Vector2i size;
  if(rows != 0 && cols != 0) { size = Eigen::Vector2i(rows, cols); }
  else {
    size = Eigen::Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ resolution,
			   (boundingBox(1, 1) - boundingBox(1, 0))/ resolution);
    } 
  std::cout << "Map size: " << size.transpose() << std::endl;
  if(size.x() == 0 || size.y() == 0) {
    std::cout << "Zero map size ... quitting!" << std::endl;
    return 0;
  }

  

  //Eigen::Vector2f offset(-size.x() * resolution / 2.0f, -size.y() * resolution / 2.0f);
  Eigen::Vector2f offset(boundingBox(0, 0),boundingBox(1, 0));
  FrequencyMapCell unknownCell;
  FrequencyMap map = FrequencyMap(resolution, offset, size, unknownCell);

  for(size_t i = 0; i < vertexIds.size(); ++i) {
    OptimizableGraph::Vertex *_v = graph->vertex(vertexIds[i]);
    VertexSE2 *v = dynamic_cast<VertexSE2*>(_v);
    if(!v) { continue; }
    OptimizableGraph::Data *d = v->userData();
    SE2 robotPose = v->estimate();
    
    while(d) {
      RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
      if(!robotLaser) {
	d = d->next();
	continue;
      }      
      map.integrateScan(robotLaser, robotPose, max_range, usable_range, gain, square_size);
      d = d->next();
    }
  }

  /************************************************************************
   *                          Save map image                              *
   ************************************************************************/
  cv::Mat mapImage(map.rows(), map.cols(), CV_8UC1);
  mapImage.setTo(cv::Scalar(0));
  for(int c = 0; c < map.cols(); c++) {
    for(int r = 0; r < map.rows(); r++) {
      if(map(r, c).misses() == 0 && map(r, c).hits() == 0) {
	mapImage.at<unsigned char>(r, c) = 127;
      } else {
	float fraction = (float)map(r, c).hits()/(float)(map(r, c).hits()+map(r, c).misses());
	
	if (threshold > 0 && fraction > threshold)
	  mapImage.at<unsigned char>(r, c) = 0;
	else if (threshold > 0 && fraction <= threshold)
	  mapImage.at<unsigned char>(r, c) = 255;
	else {
	  float val = 255*(1-fraction);
	  mapImage.at<unsigned char>(r, c) = (unsigned char)val;
	}

      }
      // else if(map(r, c).hits() > threshold) {
      // 	mapImage.at<unsigned char>(r, c) = 255;
      // }
      // else {
      // 	mapImage.at<unsigned char>(r, c) = 0;
      // }
    }
  }
  cv::imwrite(mapFilename + ".png", mapImage);

  /************************************************************************
   *                          Write yaml file                             *
   ************************************************************************/
  std::ofstream ofs(string(mapFilename + ".yaml").c_str());
  Eigen::Vector3f origin(0.0f, 0.0f, 0.0f);
  ofs << "image: " << mapFilename << ".png" << std::endl
      << "resolution: " << resolution << std::endl
      << "origin: [" << origin.x() << ", " << origin.y() << ", " << origin.z() << "]" << std::endl
      << "negate: 0" << std::endl
      << "occupied_thresh: " << 0.65f << std::endl
      << "free_thresh: " << 0.2f << std::endl;
  return 0;
}