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; }
bool SolverSLAM2DLinear::solveOrientation() { assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph"); assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0"); VectorXD b, x; // will be used for theta and x/y update b.setZero(_optimizer->indexMapping().size()); x.setZero(_optimizer->indexMapping().size()); typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix; ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]); for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) blockIndeces[i] = i+1; SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size()); // building the structure, diagonal for each active vertex for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i]; int poseIdx = v->hessianIndex(); ScalarMatrix* m = H.block(poseIdx, poseIdx, true); m->setZero(); } HyperGraph::VertexSet fixedSet; // off diagonal for each edge for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) { # ifndef NDEBUG EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it); assert(e && "Active edges contain non-odometry edge"); // # else EdgeSE2* e = static_cast<EdgeSE2*>(*it); # endif OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]); OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]); int ind1 = from->hessianIndex(); int ind2 = to->hessianIndex(); if (ind1 == -1 || ind2 == -1) { if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices if (ind2 == -1) fixedSet.insert(to); continue; } bool transposedBlock = ind1 > ind2; if (transposedBlock){ // make sure, we allocate the upper triangle block std::swap(ind1, ind2); } ScalarMatrix* m = H.block(ind1, ind2, true); m->setZero(); } // walk along the Minimal Spanning Tree to compute the guess for the robot orientation assert(fixedSet.size() == 1); VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin()); VectorXD thetaGuess; thetaGuess.setZero(_optimizer->indexMapping().size()); UniformCostFunction uniformCost; HyperDijkstra hyperDijkstra(_optimizer); hyperDijkstra.shortestPaths(root, &uniformCost); HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap()); ThetaTreeAction thetaTreeAction(thetaGuess.data()); HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction); // construct for the orientation for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) { EdgeSE2* e = static_cast<EdgeSE2*>(*it); VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]); VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]); double omega = e->information()(2,2); double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()]; double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()]; double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess); bool fromNotFixed = !(from->fixed()); bool toNotFixed = !(to->fixed()); if (fromNotFixed || toNotFixed) { double omega_r = - omega * error; if (fromNotFixed) { b(from->hessianIndex()) -= omega_r; (*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega; if (toNotFixed) { if (from->hessianIndex() > to->hessianIndex()) (*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega; else (*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega; } } if (toNotFixed ) { b(to->hessianIndex()) += omega_r; (*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega; } } } // solve orientation typedef LinearSolverCSparse<ScalarMatrix> SystemSolver; SystemSolver linearSystemSolver; linearSystemSolver.init(); bool ok = linearSystemSolver.solve(H, x.data(), b.data()); if (!ok) { cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl; return false; } // update the orientation of the 2D poses and set translation to 0, GN shall solve that root->setToOrigin(); for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) { VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]); int poseIdx = v->hessianIndex(); SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx))); v->setEstimate(poseUpdate); } return true; }