HyperGraphElementAction* VertexSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _drawActions){
_drawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE2* that = static_cast<VertexSE2*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glPushMatrix();
glTranslatef((float)that->estimate().translation().x(),(float)that->estimate().translation().y(),0.f);
glRotatef((float)RAD2DEG(that->estimate().rotation().angle()),0.f,0.f,1.f);
float tx=0.1f, ty=0.05f;
if (_triangleX && _triangleY){
tx=_triangleX->value();
ty=_triangleY->value();
}
glBegin(GL_TRIANGLE_FAN);
glVertex3f( tx ,0.f ,0.f);
glVertex3f(-tx ,-ty, 0.f);
glVertex3f(-tx , ty, 0.f);
glEnd();
if (that->userData() && _drawActions )
(*_drawActions)(that->userData(), params_);
glPopMatrix();
glPopAttrib();
return this;
}
void GraphRosPublisher::publishGraph(){
assert(_graph && "Cannot publish: undefined graph");
geometry_msgs::PoseArray traj;
sensor_msgs::PointCloud pcloud;
traj.poses.resize(_graph->vertices().size());
pcloud.points.clear();
int i = 0;
for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) {
VertexSE2* v = (VertexSE2*) (it->second);
traj.poses[i].position.x = v->estimate().translation().x();
traj.poses[i].position.y = v->estimate().translation().y();
traj.poses[i].position.z = 0;
traj.poses[i].orientation = tf::createQuaternionMsgFromYaw(v->estimate().rotation().angle());
RobotLaser *laser = dynamic_cast<RobotLaser*>(v->userData());
if (laser){
RawLaser::Point2DVector vscan = laser->cartesian();
SE2 trl = laser->laserParams().laserPose;
SE2 transf = v->estimate() * trl;
RawLaser::Point2DVector wscan;
ScanMatcher::applyTransfToScan(transf, vscan, wscan);
size_t s= 0;
while ( s<wscan.size()){
geometry_msgs::Point32 point;
point.x = wscan[s].x();
point.y = wscan[s].y();
pcloud.points.push_back(point);
s = s+10;
}
}
i++;
}
traj.header.frame_id = _fixedFrame;
pcloud.header.frame_id = traj.header.frame_id;
_publm.publish(pcloud);
_pubtj.publish(traj);
}
void transformPointsFromVSet(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _referenceVertex, RawLaser::Point2DVector& scansInRefVertex){
VertexSE2* referenceVertex=dynamic_cast<VertexSE2*>(_referenceVertex);
scansInRefVertex.clear();
for (OptimizableGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
if (laserv){
RawLaser::Point2DVector vscan = laserv->cartesian();
SE2 trl = laserv->laserParams().laserPose;
RawLaser::Point2DVector scanInRefVertex;
if (vertex->id() == referenceVertex->id()){
ScanMatcher::applyTransfToScan(trl, vscan, scanInRefVertex);
}else{
SE2 trel = referenceVertex->estimate().inverse() * vertex->estimate();
SE2 transf = trel * trl;
ScanMatcher::applyTransfToScan(transf, vscan, scanInRefVertex);
}
scansInRefVertex.insert(scansInRefVertex.end(), scanInRefVertex.begin(), scanInRefVertex.end());
}
}
}
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;
}
bool ScanMatcher::verifyMatching(OptimizableGraph::VertexSet& vset1, OptimizableGraph::Vertex* _referenceVertex1,
OptimizableGraph::VertexSet& vset2, OptimizableGraph::Vertex* _referenceVertex2,
SE2 trel12, double *score){
VertexSE2* referenceVertex2=dynamic_cast<VertexSE2*>(_referenceVertex2);
resetGrid();
CharGrid auxGrid = _grid;
//Transform points from vset2 in the reference of referenceVertex1 using trel12
RawLaser::Point2DVector scansvset2inref1;
for (OptimizableGraph::VertexSet::iterator it = vset2.begin(); it != vset2.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
RobotLaser* laserv = dynamic_cast<RobotLaser*>(vertex->userData());
RawLaser::Point2DVector vscan = laserv->cartesian();
SE2 trl = laserv->laserParams().laserPose;
RawLaser::Point2DVector scanInRefVertex1;
if (vertex->id() == referenceVertex2->id()){
applyTransfToScan(trel12 * trl, vscan, scanInRefVertex1);
}else{
//Transform scans to the referenceVertex2 coordinates
SE2 tref2_v = referenceVertex2->estimate().inverse() * vertex->estimate();
applyTransfToScan(trel12 * tref2_v * trl, vscan, scanInRefVertex1);
}
scansvset2inref1.insert(scansvset2inref1.end(), scanInRefVertex1.begin(), scanInRefVertex1.end());
}
//Scans in vset1
RawLaser::Point2DVector scansvset1;
transformPointsFromVSet(vset1, _referenceVertex1, scansvset1);
//Add local map from vset2 into the grid
_grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset2inref1.begin(), scansvset2inref1.end(), _kernel);
//Find points from vset1 not explained by map vset2
RawLaser::Point2DVector nonmatchedpoints;
_grid.searchNonMatchedPoints(scansvset1, nonmatchedpoints, .3);
//Add those points to a grid to count them
auxGrid.addAndConvolvePoints<RawLaser::Point2DVector>(nonmatchedpoints.begin(), nonmatchedpoints.end(), _kernel);
// ofstream image1;
// std::stringstream filename1;
// filename1 << "map2.ppm";
// image1.open(filename1.str().c_str());
// _LCGrid.grid().saveAsPPM(image1, false);
// ofstream image2;
// std::stringstream filename2;
// filename2 << "mapnonmatched.ppm";
// image2.open(filename2.str().c_str());
// auxGrid.grid().saveAsPPM(image2, false);
// //Just for saving the image
// resetLCGrid();
// _LCGrid.addAndConvolvePoints<RawLaser::Point2DVector>(scansvset1.begin(), scansvset1.end(), _LCKernel);
// ofstream image3;
// std::stringstream filename3;
// filename3 << "map1.ppm";
// image3.open(filename3.str().c_str());
// _LCGrid.grid().saveAsPPM(image3, false);
//Counting points around trel12
Vector2f lower(-.3+trel12.translation().x(), -.3+trel12.translation().y());
Vector2f upper(+.3+trel12.translation().x(), +.3+trel12.translation().y());
auxGrid.countPoints(lower, upper, score);
cerr << "Score: " << *score << endl;
double threshold = 40.0;
if (*score <= threshold)
return true;
return false;
}
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;
}
