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