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);
}
}
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;
}
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();
}
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;
}
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;}}
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;
}
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 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;
}
}
}
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;
}
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;
}
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;
}
}
}
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;
}
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;
}
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;
}
}
}
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;
}
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();
}
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);
}
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;
}
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;
}
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);
}
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 );
}
/**
* 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() );
}
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;
}
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;
}
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;
}
envire::Bresenham::Bresenham(const Eigen::Vector2i& start, const Eigen::Vector2i& end)
{
init(start.x(), start.y(), end.x(), end.y());
}
bool envire::Bresenham::getNextPoint(Eigen::Vector2i& p)
{
return getNextPoint(p.x(), p.y());
}
explicit Index( const Eigen::Vector2i &index ) : x(index.x()), y(index.y()) {}