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