void EdgeSE2::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /* to */) { VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]); VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]); if (from.count(fromEdge) > 0) toEdge->setEstimate(fromEdge->estimate() * _measurement); else fromEdge->setEstimate(toEdge->estimate() * _inverseMeasurement); }
void EdgeSE2PlaceVicinity::initialEstimate( const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) { VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]); VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]); if (from.count(fromEdge) > 0) toEdge->setEstimate(fromEdge->estimate()); else fromEdge->setEstimate(toEdge->estimate()); }
void EdgeSE2DistanceOrientation::initialEstimate( const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) { VertexSE2* fromEdge = static_cast<VertexSE2*>(_vertices[0]); VertexSE2* toEdge = static_cast<VertexSE2*>(_vertices[1]); double dist = sqrt(_measurement[0] * _measurement[0] + _measurement[1] * _measurement[1]); double theta = _measurement[2]; double x = dist * cos(theta), y = dist * sin(theta); SE2 tmpMeasurement(x,y,theta); SE2 inverseTmpMeasurement = tmpMeasurement.inverse(); if (from.count(fromEdge) > 0) toEdge->setEstimate(fromEdge->estimate() * tmpMeasurement); else fromEdge->setEstimate(toEdge->estimate() * inverseTmpMeasurement); }
void GraphSLAM::addData(SE2 currentOdom, RobotLaser* laser){ boost::mutex::scoped_lock lockg(graphMutex); //Add current vertex VertexSE2 *v = new VertexSE2; SE2 displacement = _lastOdom.inverse() * currentOdom; SE2 currEst = _lastVertex->estimate() * displacement; v->setEstimate(currEst); v->setId(++_runningVertexId + idRobot() * baseId()); //Add covariance information //VertexEllipse *ellipse = new VertexEllipse; //Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance //ellipse->setCovariance(cov); //v->setUserData(ellipse); v->addUserData(laser); std::cout << std::endl << "Current vertex: " << v->id() << " Estimate: "<< v->estimate().translation().x() << " " << v->estimate().translation().y() << " " << v->estimate().rotation().angle() << std::endl; _graph->addVertex(v); //Add current odometry edge EdgeSE2 *e = new EdgeSE2; e->setId(++_runningEdgeId + idRobot() * baseId()); e->vertices()[0] = _lastVertex; e->vertices()[1] = v; e->setMeasurement(displacement); // //Computing covariances depending on the displacement // Vector3d dis = displacement.toVector(); // dis.x() = fabs(dis.x()); // dis.y() = fabs(dis.y()); // dis.z() = fabs(dis.z()); // dis += Vector3d(1e-3,1e-3,1e-2); // Matrix3d dis2 = dis*dis.transpose(); // Matrix3d newcov = dis2.cwiseProduct(_odomK); e->setInformation(_odominf); _graph->addEdge(e); _odomEdges.insert(e); _lastOdom = currentOdom; _lastVertex = v; }
void Localization::InitG2OGraph() { optimizer.clear(); LandmarkCount = 0; { VertexSE2 * l = new VertexSE2; l->setEstimate(Eigen::Vector3d(0, 0, 0)); l->setFixed(true); l->setId(CenterL); optimizer.addVertex(l); LandmarkCount++; } { VertexSE2 * l = new VertexSE2; l->setEstimate(Eigen::Vector3d(A2, 0, 0)); l->setFixed(true); l->setId(FrontL); optimizer.addVertex(l); LandmarkCount++; } { VertexSE2 * l = new VertexSE2; l->setEstimate(Eigen::Vector3d(-A2, 0, 0)); l->setFixed(true); l->setId(BackL); optimizer.addVertex(l); LandmarkCount++; } { VertexSE2 * l = new VertexSE2; l->setEstimate(Eigen::Vector3d(0, -B2, 0)); l->setFixed(true); l->setId(RightL); optimizer.addVertex(l); LandmarkCount++; } { VertexSE2 * l = new VertexSE2; l->setEstimate(Eigen::Vector3d(0, B2, 0)); l->setFixed(true); l->setId(LeftL); optimizer.addVertex(l); LandmarkCount++; } PreviousVertexId = -1; CurrentVertexId = LandmarkCount; { VertexSE2 * l = new VertexSE2; l->setEstimate( Eigen::Vector3d(location.x, location.y, 0)); l->setFixed(false); l->setId(LandmarkCount); optimizer.addVertex(l); } }
inline void updateVertexIdx() { if ((ros::Time::now() - lastSavedNodeTime).toSec() >= 0.03) { nodeCounter++; lastSavedNodeTime = ros::Time::now(); PreviousVertexId = CurrentVertexId; CurrentVertexId++; if (CurrentVertexId - LandmarkCount >= 100) { CurrentVertexId = LandmarkCount; } { VertexSE2 * r = new VertexSE2; r->setEstimate(Eigen::Vector3d(location.x, location.y, 0)); r->setFixed(false); r->setId(CurrentVertexId); if (optimizer.vertex(CurrentVertexId) != NULL) { optimizer.removeVertex(optimizer.vertex(CurrentVertexId)); } optimizer.addVertex(r); } { EdgeSE2 * e = new EdgeSE2; e->vertices()[0] = optimizer.vertex(PreviousVertexId); e->vertices()[1] = optimizer.vertex(CurrentVertexId); Point2d dead_reck = getOdometryFromLastGet(); e->setMeasurement(SE2(dead_reck.x, dead_reck.y, 0)); Matrix3d information; information.fill(0.); information(0, 0) = 200; information(1, 1) = 200; information(2, 2) = 1; e->setInformation(information); optimizer.addEdge(e); } } }
int main(int argc, char** argv) { bool fixLaser; int maxIterations; bool verbose; string inputFilename; string outputfilename; string rawFilename; string odomTestFilename; string dumpGraphFilename; // command line parsing CommandArgs commandLineArguments; commandLineArguments.param("i", maxIterations, 10, "perform n iterations"); commandLineArguments.param("v", verbose, false, "verbose output of the optimization process"); commandLineArguments.param("o", outputfilename, "", "output final version of the graph"); commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data"); commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk"); commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization"); commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed"); commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry"); commandLineArguments.parseArgs(argc, argv); SparseOptimizer optimizer; optimizer.setVerbose(verbose); allocateSolverForSclam(optimizer); // loading DataQueue odometryQueue; int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue); if (numLaserOdom == 0) { cerr << "No raw information read" << endl; return 0; } cerr << "Read " << numLaserOdom << " laser readings from file" << endl; Eigen::Vector3d odomCalib(1., 1., 1.); SE2 initialLaserPose; DataQueue robotLaserQueue; int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue); if (numRobotLaser == 0) { cerr << "No robot laser read" << endl; return 0; } else { RobotLaser* rl = dynamic_cast<RobotLaser*>(robotLaserQueue.buffer().begin()->second); initialLaserPose = rl->odomPose().inverse() * rl->laserPose(); cerr << PVAR(initialLaserPose.toVector().transpose()) << endl; } // adding the measurements vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions; { std::map<double, RobotData*>::const_iterator it = robotLaserQueue.buffer().begin(); std::map<double, RobotData*>::const_iterator prevIt = it++; for (; it != robotLaserQueue.buffer().end(); ++it) { MotionInformation mi; RobotLaser* prevLaser = dynamic_cast<RobotLaser*>(prevIt->second); RobotLaser* curLaser = dynamic_cast<RobotLaser*>(it->second); mi.laserMotion = prevLaser->laserPose().inverse() * curLaser->laserPose(); // get the motion of the robot in that time interval RobotLaser* prevOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(prevLaser->timestamp())); RobotLaser* curOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(curLaser->timestamp())); mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose(); mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp(); prevIt = it; motions.push_back(mi); } } if (1) { VertexSE2* laserOffset = new VertexSE2; laserOffset->setId(Gm2dlIO::ID_LASERPOSE); laserOffset->setEstimate(initialLaserPose); optimizer.addVertex(laserOffset); VertexOdomDifferentialParams* odomParamsVertex = new VertexOdomDifferentialParams; odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB); odomParamsVertex->setEstimate(Eigen::Vector3d(1., 1., 1.)); optimizer.addVertex(odomParamsVertex); for (size_t i = 0; i < motions.size(); ++i) { const SE2& odomMotion = motions[i].odomMotion; const SE2& laserMotion = motions[i].laserMotion; const double& timeInterval = motions[i].timeInterval; // add the edge MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval); OdomAndLaserMotion meas; meas.velocityMeasurement = OdomConvert::convertToVelocity(mm); meas.laserMotion = laserMotion; EdgeSE2PureCalib* calibEdge = new EdgeSE2PureCalib; calibEdge->setVertex(0, laserOffset); calibEdge->setVertex(1, odomParamsVertex); calibEdge->setInformation(Eigen::Matrix3d::Identity()); calibEdge->setMeasurement(meas); if (! optimizer.addEdge(calibEdge)) { cerr << "Error adding calib edge" << endl; delete calibEdge; } } if (fixLaser) { cerr << "Fix position of the laser offset" << endl; laserOffset->setFixed(true); } cerr << "\nPerforming full non-linear estimation" << endl; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); optimizer.optimize(maxIterations); cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl; odomCalib = odomParamsVertex->estimate(); cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl; optimizer.clear(); } // linear least squares for some parameters { Eigen::MatrixXd A(motions.size(), 2); Eigen::VectorXd x(motions.size()); for (size_t i = 0; i < motions.size(); ++i) { const SE2& odomMotion = motions[i].odomMotion; const SE2& laserMotion = motions[i].laserMotion; const double& timeInterval = motions[i].timeInterval; MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval); VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm); A(i, 0) = velMeas.vl() * timeInterval; A(i, 1) = velMeas.vr() * timeInterval; x(i) = laserMotion.rotation().angle(); } //linearSolution = (A.transpose() * A).inverse() * A.transpose() * x; linearSolution = A.colPivHouseholderQr().solve(x); //cout << PVAR(linearSolution.transpose()) << endl; } //constructing non-linear least squares VertexSE2* laserOffset = new VertexSE2; laserOffset->setId(Gm2dlIO::ID_LASERPOSE); laserOffset->setEstimate(initialLaserPose); optimizer.addVertex(laserOffset); VertexBaseline* odomParamsVertex = new VertexBaseline; odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB); odomParamsVertex->setEstimate(1.); optimizer.addVertex(odomParamsVertex); for (size_t i = 0; i < motions.size(); ++i) { const SE2& odomMotion = motions[i].odomMotion; const SE2& laserMotion = motions[i].laserMotion; const double& timeInterval = motions[i].timeInterval; // add the edge MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval); OdomAndLaserMotion meas; meas.velocityMeasurement = OdomConvert::convertToVelocity(mm); meas.laserMotion = laserMotion; EdgeCalib* calibEdge = new EdgeCalib; calibEdge->setVertex(0, laserOffset); calibEdge->setVertex(1, odomParamsVertex); calibEdge->setInformation(Eigen::Matrix3d::Identity()); calibEdge->setMeasurement(meas); if (! optimizer.addEdge(calibEdge)) { cerr << "Error adding calib edge" << endl; delete calibEdge; } } if (fixLaser) { cerr << "Fix position of the laser offset" << endl; laserOffset->setFixed(true); } cerr << "\nPerforming partial non-linear estimation" << endl; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); optimizer.optimize(maxIterations); cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl; odomCalib(0) = -1. * linearSolution(0) * odomParamsVertex->estimate(); odomCalib(1) = linearSolution(1) * odomParamsVertex->estimate(); odomCalib(2) = odomParamsVertex->estimate(); cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomCalib.transpose() << endl; { SE2 closedFormLaser; Eigen::Vector3d closedFormOdom; ClosedFormCalibration::calibrate(motions, closedFormLaser, closedFormOdom); cerr << "\nObtaining closed form solution" << endl; cerr << "Calibrated laser offset (x, y, theta):" << closedFormLaser.toVector().transpose() << endl; cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << closedFormOdom.transpose() << endl; } if (dumpGraphFilename.size() > 0) { cerr << "Writing " << dumpGraphFilename << " ... "; optimizer.save(dumpGraphFilename.c_str()); cerr << "done." << endl; } // optional input of a separate file for applying the odometry calibration if (odomTestFilename.size() > 0) { DataQueue testRobotLaserQueue; int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue); if (numTestOdom == 0) { cerr << "Unable to read test data" << endl; } else { ofstream rawStream("odometry_raw.txt"); ofstream calibratedStream("odometry_calibrated.txt"); RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second); SE2 prevCalibratedPose = prev->odomPose(); for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) { RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second); assert(cur); double dt = cur->timestamp() - prev->timestamp(); SE2 motion = prev->odomPose().inverse() * cur->odomPose(); // convert to velocity measurement MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt); VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement); // apply calibration VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement; calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl()); calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr()); MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2)); // combine calibrated odometry with the previous pose SE2 remappedOdom; remappedOdom.fromVector(mm.measurement()); SE2 calOdomPose = prevCalibratedPose * remappedOdom; // write output rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl; calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl; prevCalibratedPose = calOdomPose; prev = cur; } } } return 0; }
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; }
void GraphSLAM::addDataSM(SE2 currentOdom, RobotLaser* laser){ boost::mutex::scoped_lock lockg(graphMutex); //Add current vertex VertexSE2 *v = new VertexSE2; SE2 displacement = _lastOdom.inverse() * currentOdom; SE2 currEst = _lastVertex->estimate() * displacement; v->setEstimate(currEst); v->setId(++_runningVertexId + idRobot() * baseId()); //Add covariance information //VertexEllipse *ellipse = new VertexEllipse; //Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance //ellipse->setCovariance(cov); //v->setUserData(ellipse); v->addUserData(laser); std::cout << endl << "Current vertex: " << v->id() << " Estimate: "<< v->estimate().translation().x() << " " << v->estimate().translation().y() << " " << v->estimate().rotation().angle() << std::endl; _graph->addVertex(v); //Add current odometry edge EdgeSE2 *e = new EdgeSE2; e->setId(++_runningEdgeId + idRobot() * baseId()); e->vertices()[0] = _lastVertex; e->vertices()[1] = v; OptimizableGraph::VertexSet vset; vset.insert(_lastVertex); int j = 1; int gap = 5; while (j <= gap){ VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j)); if (vj) vset.insert(vj); else break; j++; } SE2 transf; bool shouldIAdd = _closeMatcher.closeScanMatching(vset, _lastVertex, v, &transf, maxScore); if (shouldIAdd){ e->setMeasurement(transf); e->setInformation(_SMinf); }else{ //Trust the odometry e->setMeasurement(displacement); // Vector3d dis = displacement.toVector(); // dis.x() = fabs(dis.x()); // dis.y() = fabs(dis.y()); // dis.z() = fabs(dis.z()); // dis += Vector3d(1e-3,1e-3,1e-2); // Matrix3d dis2 = dis*dis.transpose(); // Matrix3d newcov = dis2.cwiseProduct(_odomK); // e->setInformation(newcov.inverse()); e->setInformation(_odominf); } _graph->addEdge(e); _lastOdom = currentOdom; _lastVertex = v; }
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; }