void init(){
std::cout << "Initializing stuff" << std::endl;
_confirm_obs = 10;
_optimize_every = 50;
next_id = 0;
odom_info = new Eigen::Matrix3d();
(*odom_info) << 500, 0, 0,
0, 500, 0,
0, 0, 100;
obs_info = new Eigen::MatrixXd(1,1);
(*obs_info) << 500;
// allocating the optimizer
optimizer = new g2o::SparseOptimizer();
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
optimizer->setAlgorithm(solver);
//optimizer->setVerbose(true);
// creating the drawer
_drawer = new rdrawer::RobotDrawer();
optimize_this_much = 30;
optimization_active = true;
}
void GraphSLAM::init(double resolution, double kernelRadius, int windowLoopClosure_, double maxScore_, double inlierThreshold_, int minInliers_){
boost::mutex::scoped_lock lockg(graphMutex);
//Init graph
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(blockSolver);
_graph->setAlgorithm(solver);
_graph->setVerbose(false);
//Init scan matchers
_closeMatcher.initializeKernel(resolution, kernelRadius);
_closeMatcher.initializeGrid(Vector2f(-15, -15), Vector2f(15, 15), resolution);
_LCMatcher.initializeKernel(0.1, 0.5); //before 0.1, 0.5
_LCMatcher.initializeGrid(Vector2f(-35, -35), Vector2f(35, 35), 0.1); //before 0.1
cerr << "Grids initialized\n";
windowLoopClosure = windowLoopClosure_;
maxScore = maxScore_;
inlierThreshold = inlierThreshold_;
minInliers = minInliers_;
//
_odominf = 100 * Matrix3d::Identity();
_odominf(2,2) = 1000;
_SMinf = 1000 * Matrix3d::Identity();
_SMinf(2,2) = 10000;
}
g2o::SparseOptimizer * G2OBridge::g2oInit(){
// graph construction
typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solverGauss = new g2o::OptimizationAlgorithmLevenberg(blockSolver);
//OptimizationAlgorithmGaussNewton* solverGauss = new OptimizationAlgorithmGaussNewton(blockSolver);
g2o::SparseOptimizer * g = new g2o::SparseOptimizer();
g->setAlgorithm(solverGauss);
return g;
}
g2o::SparseOptimizer * MapG2OReflector::g2oInit(){
// graph construction
typedef BlockSolver< BlockSolverTraits<-1, -1> > SlamBlockSolver;
typedef LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
OptimizationAlgorithmLevenberg* solverGauss = new OptimizationAlgorithmLevenberg(blockSolver);
//OptimizationAlgorithmGaussNewton* solverGauss = new OptimizationAlgorithmGaussNewton(blockSolver);
SparseOptimizer * graph = new SparseOptimizer();
graph->setAlgorithm(solverGauss);
g2o::ParameterSE3Offset* imuOffset = new ParameterSE3Offset();
imuOffset->setOffset(Eigen::Isometry3d::Identity());
imuOffset->setId(0);
graph->addParameter(imuOffset);
return graph;
}
int main(int argc, char ** argv){
// check input
if(argc < 3){
std::cout << "Usage: difference <file1.g2o> <file2.g2o>" << std::endl;
return 0;
}
init();
// allocate the optimizers
g2o::SparseOptimizer * optimizer1 = new g2o::SparseOptimizer();
g2o::SparseOptimizer * optimizer2 = new g2o::SparseOptimizer();
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
optimizer1->setAlgorithm(solver);
optimizer2->setAlgorithm(solver);
// load the graphs
std::ifstream ifs1(argv[1]);
std::ifstream ifs2(argv[2]);
if(!ifs1){
std::cout << "Cannot open file " << argv[1] << std::endl;
return 1;
}
if(!ifs2){
std::cout << "Cannot open file " << argv[2] << std::endl;
return 1;
}
if(!optimizer1->load(ifs1)){
std::cout << "Error loading graph #1" << std::endl;
return 2;
}
if(!optimizer2->load(ifs2)){
std::cout << "Error loading graph #2" << std::endl;
return 2;
}
std::cout << "graph #1:";
std::cout << "\tLoaded " << optimizer1->vertices().size() << " vertices" << std::endl;
std::cout << "\tLoaded " << optimizer1->edges().size() << " edges" << std::endl;
std::cout << "graph #2:";
std::cout << "\tLoaded " << optimizer1->vertices().size() << " vertices" << std::endl;
std::cout << "\tLoaded " << optimizer1->edges().size() << " edges" << std::endl;
g2o::HyperGraph::VertexIDMap map1 = optimizer1->vertices();
g2o::HyperGraph::VertexIDMap map2 = optimizer2->vertices();
// safety checks
std::cout << "safety checks...";
if(optimizer1->vertices().size() != optimizer2->vertices().size()){
std::cout << "!!! the two graphs don't have the same number of vertices !!!" << std::endl;
return 3;
}
for(g2o::HyperGraph::VertexIDMap::iterator it=map1.begin(); it!=map1.end(); it++){
g2o::OptimizableGraph::Vertex * v1 = (g2o::OptimizableGraph::Vertex *) it->second;
int dim = v1->minimalEstimateDimension();
if(dim!=3 & dim!=2){
std::cerr << "Vertex #" << v1->id() << " has strange dimension: " << dim << std::endl;
return 4;
}
// get the corresponding vertex in the other graph
g2o::OptimizableGraph::Vertex * v2 = (g2o::OptimizableGraph::Vertex *) map2[v1->id()];
assert(v2->id() == v1->id());
if(v2->minimalEstimateDimension() != dim){
std::cerr << "ERROR: vertex " << v2->id() << " has different dimension in the two graphs" << std::endl;
return 5;
}
}
std::cout << "done" << std::endl;
std::vector<g2o::OptimizableGraph::Vertex*> vertices;
for(g2o::HyperGraph::VertexIDMap::iterator it=map1.begin(); it!=map1.end(); it++){
vertices.push_back((g2o::OptimizableGraph::Vertex *)it->second);
}
std::cout << "vertices vector filled" << std::endl;
bool visited[vertices.size()];
for(unsigned int i=0; i<vertices.size(); i++){
visited[i] = false;
}
thread_struct t_struct;
t_struct.map1 = &map1;
t_struct.map2 = &map2;
t_struct.vertices = &vertices;
t_struct.visited = visited;
pthread_t threads[_num_threads + 1];
for(unsigned int i=0; i<_num_threads; i++){
pthread_create(threads+i, NULL, launch_analyze, &t_struct);
}
visualizer_struct v_struct;
v_struct.visited = visited;
v_struct.size = vertices.size();
pthread_create(threads+_num_threads, NULL, launch_visualize, &v_struct);
std::cout << "threads launched" << std::endl;
for(unsigned int i=0; i<_num_threads+1; i++){
pthread_join(threads[i], NULL);
}
std::cout << "minimum error = " << _best_error;
}
void ViewerState::init(){
imageRows = 0;
imageCols = 0;
ng_worldRadius = 0.1f;
ng_minImageRadius = 10;
ng_curvatureThreshold = 1.0f;
al_innerIterations = 1;
al_outerIterations = 10;
vz_step = 5;
if_curvatureThreshold = 0.1f;
reduction = 2;
cameraMatrix <<
525.0f, 0.0f, 319.5f,
0.0f, 525.0f, 239.5f,
0.0f, 0.0f, 1.0f;
float scale = 1./reduction;
cameraMatrix*=scale;
cameraMatrix(2,2) = 1;
if (0){
sensorOffset.setIdentity();
} else {
sensorOffset.translation() = Vector3f(0.15f, 0.0f, 0.05f);
Quaternionf quat = Quaternionf(0.5, -0.5, 0.5, -0.5);
sensorOffset.linear() = quat.toRotationMatrix();
}
sensorOffset.matrix().row(3) << 0,0,0,1;
projector = new PinholePointProjector();
statsCalculator = new StatsCalculator();
pointInformationMatrixCalculator = new PointInformationMatrixCalculator();
normalInformationMatrixCalculator = new NormalInformationMatrixCalculator();
converter = new DepthImageConverter(projector, statsCalculator,
pointInformationMatrixCalculator, normalInformationMatrixCalculator);
projector->setTransform(Eigen::Isometry3f::Identity());
projector->setCameraMatrix(cameraMatrix);
// Creating and setting aligner object.
//Aligner aligner;
correspondenceFinder = new CorrespondenceFinder();
linearizer = new Linearizer() ;
#ifdef _PWN_USE_CUDA_
aligner = new CuAligner() ;
#else
aligner = new Aligner();
#endif
aligner->setProjector(projector);
aligner->setLinearizer(linearizer);
linearizer->setAligner(aligner);
aligner->setCorrespondenceFinder(correspondenceFinder);
statsCalculator->setWorldRadius(ng_worldRadius);
//statsFinder->setCurvatureThreshold(ng_curvatureThreshold);
statsCalculator->setMinPoints(ng_minImageRadius);
aligner->setInnerIterations(al_innerIterations);
aligner->setOuterIterations(al_outerIterations);
converter->_curvatureThreshold = ng_curvatureThreshold;
pointInformationMatrixCalculator->setCurvatureThreshold(if_curvatureThreshold);
normalInformationMatrixCalculator->setCurvatureThreshold(if_curvatureThreshold);
refScn = pwnGMW->scene0();
currScn = pwnGMW->scene1();
listWidget = pwnGMW->listWidget;
drawableFrameParameters = new DrawableFrameParameters();
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);
graph = new SparseOptimizer();
graph->setAlgorithm(solverGauss);
continuousMode = false;
}
void Graph::solve(unsigned int iterations){
ROS_INFO(":: SOLVING! ::");
//Setup solver
g2o::SparseOptimizer sparseOptimizer;
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
g2o::OptimizationAlgorithmGaussNewton* solverGauss =
new g2o::OptimizationAlgorithmGaussNewton(blockSolver);
sparseOptimizer.setAlgorithm(solverGauss);
//Convert pose nodes to g2o node structure and add in the graph.
for(unsigned int i = 0; i < node_list.size(); i ++){
Node* curNode = node_list[i];
// ROS_INFO("Curnode id: %d", curNode->id);
// Convert the node.
GraphPose graph_pose = curNode->graph_pose;
g2o::SE2 converted_pose(graph_pose.x, graph_pose.y, graph_pose.theta);
// Create the vertex to put in the graph.
g2o::VertexSE2* vertex = new g2o::VertexSE2;
vertex->setId(curNode->id);
// ROS_INFO("Converted node id: %d", vertex->id());
vertex->setEstimate(converted_pose);
// Add to the graph
sparseOptimizer.addVertex(vertex);
}
// Set one pose fixed, to reduce complexity. This pose wont be changed by the optimizer.
sparseOptimizer.vertex(1)->setFixed(true);
// Convert the edges to g2o edges and add them in the graph
for(unsigned int i = 0; i < edge_list.size(); i++) {
// ROS_INFO("Adding edge: %d", i);
Edge* edge = edge_list[i];
//ROS_INFO("Edge parent id: %d, child id: %d", edge->parent_id, edge->child_id);
//Actually make the edge for the optimizer.
g2o::EdgeSE2* graph_edge = new g2o::EdgeSE2;
graph_edge->vertices()[0] = sparseOptimizer.vertex(edge->parent_id);
graph_edge->vertices()[1] = sparseOptimizer.vertex(edge->child_id);
//
g2o::SE2 se_mean(edge->mean[0], edge->mean[1], edge->mean[2]);
graph_edge->setMeasurement(se_mean);
Matrix3d cov;
cov = MatrixXd::Zero(3,3);
for(unsigned int i = 0; i < 3; i++) {
for(unsigned int j = 0; j < 3; j++) {
cov(i, j) = edge->covariance[i][j];
// ROS_INFO("Covariance[%d]: %f", i+j, covariance[i][j]);
}
}
graph_edge->setInformation(cov.inverse());
//Add edge to optimizer
sparseOptimizer.addEdge(graph_edge);
}
//Optimize!
sparseOptimizer.setVerbose(false);
sparseOptimizer.initializeOptimization();
sparseOptimizer.optimize(iterations);
//Convert the solved poses back
for(unsigned int i = 0; i < node_list.size(); i++){
GraphPose* currentPose = &(node_list[i]->graph_pose);
g2o::SE2 optimized_pose = ((g2o::VertexSE2*) sparseOptimizer.vertex(node_list[i]->id))->estimate();
//
if(rot_distance(currentPose->theta, optimized_pose[2]) != 0. || distance(currentPose->x, optimized_pose[0], currentPose->y, optimized_pose[1]) > 0.) {
ROS_INFO("Node %d, pose before optimize: x %f, y %f, t %f", node_list[i]->id, currentPose->x, currentPose->y, currentPose->theta);
ROS_INFO("Node %d, pose after optimize: x %f, y %f, t %f", node_list[i]->id, optimized_pose[0], optimized_pose[1], optimized_pose[2]);
}
currentPose->x = optimized_pose[0];
currentPose->y = optimized_pose[1];
currentPose->theta = optimized_pose[2];
}
sparseOptimizer.clear();
};
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;
}
