HyperGraphElementAction* EdgeSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; EdgeSE2* e = static_cast<EdgeSE2*>(element); VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0)); VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1)); glColor3f(0.5,0.5,0.8); glPushAttrib(GL_ENABLE_BIT); glDisable(GL_LIGHTING); glBegin(GL_LINES); glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.); glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.); glEnd(); glPopAttrib(); return this; }
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 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; }
HyperGraphElementAction* VertexSE2WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_); if (!params || !params->os){ std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid output stream specified" << std::endl; return 0; } VertexSE2* v = static_cast<VertexSE2*>(element); *(params->os) << v->estimate().translation().x() << " " << v->estimate().translation().y() << " " << v->estimate().rotation().angle() << std::endl; return this; }
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); }
HyperGraphElementAction* EdgeSE2OdomDifferentialCalibDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* ) { if (typeid(*element).name()!=_typeName) return 0; EdgeSE2OdomDifferentialCalib* e = static_cast<EdgeSE2OdomDifferentialCalib*>(element); VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0)); VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1)); glColor3f(0.5,0.5,0.5); glPushAttrib(GL_ENABLE_BIT); glDisable(GL_LIGHTING); glBegin(GL_LINES); glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.); glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.); glEnd(); glPopAttrib(); return this; }
void GraphSLAM::optimize(int nrunnings){ boost::mutex::scoped_lock lockg(graphMutex); _graph->initializeOptimization(); _graph->optimize(nrunnings); ////////////////////////////////////// //Update laser data for (SparseOptimizer::VertexIDMap::const_iterator it = _graph->vertices().begin(); it != _graph->vertices().end(); ++it) { VertexSE2* v = dynamic_cast<VertexSE2*>(it->second); RobotLaser* robotLaser = findLaserData(v); if (robotLaser) robotLaser->setOdomPose(v->estimate()); } ////////////////////////////////////// //Update ellipse data //Covariance computation /* CovarianceEstimator ce(_graph); OptimizableGraph::VertexSet vset; for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) { OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) (it->second); vset.insert(v); } ce.setVertices(vset); ce.setGauge(_lastVertex); ce.compute(); /////////////////////////////////// for (OptimizableGraph::VertexIDMap::iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it) { VertexSE2* v = dynamic_cast<VertexSE2*>(it->second); VertexEllipse* ellipse = findEllipseData(v); if (ellipse && (v != lastVertex())){ MatrixXd PvX = ce.getCovariance(v); Matrix3d Pv = PvX; Matrix3f Pvf = Pv.cast<float>(); ellipse->setCovariance(Pvf); ellipse->clearMatchingVertices(); }else { if(ellipse && v == lastVertex()){ ellipse->clearMatchingVertices(); for (size_t i = 0; i<ellipse->matchingVerticesIDs().size(); i++){ int id = ellipse->matchingVerticesIDs()[i]; VertexSE2* vid = dynamic_cast<VertexSE2*>(_graph->vertex(id)); SE2 relativetransf = _lastVertex->estimate().inverse() * vid->estimate(); ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y()); } } } }*/ }
void EdgeSE2PointXYCalib::initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/) { assert(from.size() == 1 && from.count(_vertices[0]) == 1 && "Can not initialize VertexSE2 position by VertexPointXY"); if (from.count(_vertices[0]) != 1) return; VertexSE2* vi = static_cast<VertexSE2*>(_vertices[0]); VertexPointXY* vj = static_cast<VertexPointXY*>(_vertices[1]); vj->setEstimate(vi->estimate() * _measurement); }
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); }
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; 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); if (_show && _show->value()) { 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(); } OptimizableGraph::Data* d=that->userData(); while (d && _drawActions ){ (*_drawActions)(d, params_); d=d->next(); } glPopMatrix(); glPopAttrib(); return this; }
HyperGraphElementAction* EdgeSE2DistanceOrientationWriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_); if (!params->os){ std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl; return 0; } EdgeSE2DistanceOrientation* e = static_cast<EdgeSE2DistanceOrientation*>(element); VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0)); VertexPointXY* toEdge = static_cast<VertexPointXY*>(e->vertex(1)); *(params->os) << fromEdge->estimate().translation().x() << " " << fromEdge->estimate().translation().y() << " " << fromEdge->estimate().rotation().angle() << std::endl; *(params->os) << toEdge->estimate().x() << " " << toEdge->estimate().y() << std::endl; *(params->os) << std::endl; return this; }
HyperGraphElementAction* EdgeSE2DistanceOrientationDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; EdgeSE2DistanceOrientation* e = static_cast<EdgeSE2DistanceOrientation*>(element); VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0)); VertexPointXY* to = static_cast<VertexPointXY*>(e->vertex(1)); if (! from) return this; double guessRange=5; double theta = e->measurement(); Vector2D p(cos(theta)*guessRange, sin(theta)*guessRange); glPushAttrib(GL_ENABLE_BIT|GL_LIGHTING|GL_COLOR); glDisable(GL_LIGHTING); if (!to){ p=from->estimate()*p; glColor3f(LANDMARK_EDGE_GHOST_COLOR); glPushAttrib(GL_POINT_SIZE); glPointSize(3); glBegin(GL_POINTS); glVertex3f((float)p.x(),(float)p.y(),0.f); glEnd(); glPopAttrib(); } else { p=to->estimate(); glColor3f(LANDMARK_EDGE_COLOR); } glBegin(GL_LINES); glVertex3f((float)from->estimate().translation().x(),(float)from->estimate().translation().y(),0.f); glVertex3f((float)p.x(),(float)p.y(),0.f); glEnd(); glPopAttrib(); return this; }
HyperGraphElementAction* EdgeSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ if (typeid(*element).name()!=_typeName) return 0; refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; EdgeSE2* e = static_cast<EdgeSE2*>(element); VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0)); VertexSE2* to = static_cast<VertexSE2*>(e->vertex(1)); if (! from && ! to) return this; SE2 fromTransform; SE2 toTransform; glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING | GL_COLOR); glDisable(GL_LIGHTING); if (! from) { glColor3f(POSE_EDGE_GHOST_COLOR); toTransform = to->estimate(); fromTransform = to->estimate()*e->measurement().inverse(); // DRAW THE FROM EDGE AS AN ARROW glPushMatrix(); glTranslatef((float)fromTransform.translation().x(), (float)fromTransform.translation().y(),0.f); glRotatef((float)RAD2DEG(fromTransform.rotation().angle()),0.f,0.f,1.f); opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f); glPopMatrix(); } else if (! to){ glColor3f(POSE_EDGE_GHOST_COLOR); fromTransform = from->estimate(); toTransform = from->estimate()*e->measurement(); // DRAW THE TO EDGE AS AN ARROW glPushMatrix(); glTranslatef(toTransform.translation().x(),toTransform.translation().y(),0.f); glRotatef((float)RAD2DEG(toTransform.rotation().angle()),0.f,0.f,1.f); opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f); glPopMatrix(); } else { glColor3f(POSE_EDGE_COLOR); fromTransform = from->estimate(); toTransform = to->estimate(); } glBegin(GL_LINES); glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),0.f); glVertex3f((float)toTransform.translation().x(),(float)toTransform.translation().y(),0.f); glEnd(); glPopAttrib(); return this; }
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){ /////////////////////////////////// // we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one CovarianceEstimator ce(_graph); ce.setVertices(vset); ce.setGauge(_lastVertex); ce.compute(); assert(!_lastVertex->fixed() && "last Vertex is fixed"); assert(_firstRobotPose->fixed() && "first Vertex is not fixed"); OptimizableGraph::VertexSet tmpvset = vset; for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){ VertexSE2 *vertex = (VertexSE2*) *it; MatrixXd Pv = ce.getCovariance(vertex); Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1); SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate(); Vector2d hxy (delta.translation().x(), delta.translation().y()); double perceptionRange =1; if (hxy.x()-perceptionRange>0) hxy.x() -= perceptionRange; else if (hxy.x()+perceptionRange<0) hxy.x() += perceptionRange; else hxy.x() = 0; if (hxy.y()-perceptionRange>0) hxy.y() -= perceptionRange; else if (hxy.y()+perceptionRange<0) hxy.y() += perceptionRange; else hxy.y() = 0; double d2 = hxy.transpose() * Pxy.inverse() * hxy; if (d2 > 5.99) vset.erase(*it); } }
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) { 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; }
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; }
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; }
bool ScanMatcher::scanMatchingLChierarchical(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::VertexSet& currvset, OptimizableGraph::Vertex* _currentVertex, std::vector<SE2>& trel, double maxScore){ //cerr << "Loop Closing Scan Matching" << endl; //cerr << "Size of Vset " << referenceVset.size() << endl; VertexSE2* currentVertex=dynamic_cast<VertexSE2*>(_currentVertex); VertexSE2* referenceVertex =dynamic_cast<VertexSE2*>(_referenceVertex); resetGrid(); trel.clear(); RawLaser::Point2DVector scansInRefVertex; transformPointsFromVSet(referenceVset, _referenceVertex, scansInRefVertex); _grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel); RawLaser::Point2DVector scansInCurVertex; transformPointsFromVSet(currvset, _currentVertex, scansInCurVertex); Vector2dVector reducedScans; CharGrid::subsample(reducedScans, scansInCurVertex, 0.1); //cerr << "subsampling: " << scansInCurVertex.size() << " -> " << reducedScans.size() << endl; SE2 delta = referenceVertex->estimate().inverse() * currentVertex->estimate(); Vector3d initGuess(delta.translation().x(), delta.translation().y(), delta.rotation().angle()); Vector3f lower(-2.+initGuess.x(), -2.+initGuess.y(), -1.+initGuess.z()); Vector3f upper(+2.+initGuess.x(), 2.+initGuess.y(), 1.+initGuess.z()); RegionVector regions; Region reg; reg.lowerLeft = lower; reg.upperRight = upper; regions.push_back(reg); std::vector<MatcherResult> mresvec; double thetaRes = 0.025; // was 0.0125*.5 // clock_t t_ini, t_fin; // double secs; // t_ini = clock(); _grid.hierarchicalSearch(mresvec, reducedScans, regions, thetaRes, maxScore, 0.5, 0.5, 0.2, 3); // t_fin = clock(); // secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC; // printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size()); if (mresvec.size()){ Vector3d adj=mresvec[0].transformation; SE2 transf; transf.setTranslation(Vector2d(adj.x(), adj.y())); transf.setRotation(adj.z()); // cerr << " bestScore = " << mresvec[0].score << endl; //cerr << "Found Loop Closure Edge. Transf: " << adj.x() << " " << adj.y() << " " << adj.z() << endl << endl; trel.push_back(transf); } if (trel.size()) return true; return false; }
bool ScanMatcher::scanMatchingLC(OptimizableGraph::VertexSet& referenceVset, OptimizableGraph::Vertex* _referenceVertex, OptimizableGraph::VertexSet& currvset, OptimizableGraph::Vertex* _currentVertex, std::vector<SE2>& trel, double maxScore){ cerr << "Loop Closing Scan Matching" << endl; //cerr << "Size of Vset " << referenceVset.size() << endl; VertexSE2* referenceVertex =dynamic_cast<VertexSE2*>(_referenceVertex); resetGrid(); trel.clear(); RawLaser::Point2DVector scansInRefVertex; transformPointsFromVSet(referenceVset, _referenceVertex, scansInRefVertex); _grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel); RawLaser::Point2DVector scansInCurVertex; transformPointsFromVSet(currvset, _currentVertex, scansInCurVertex); Vector2dVector reducedScans; CharGrid::subsample(reducedScans, scansInCurVertex, 0.1); RegionVector regions; RegionVector regionspi; for (OptimizableGraph::VertexSet::iterator it = referenceVset.begin(); it != referenceVset.end(); it++){ VertexSE2 *vertex = (VertexSE2*) *it; Region reg; SE2 relposv(.0, .0, .0); if (vertex->id() != referenceVertex->id()) relposv = referenceVertex->estimate().inverse() * vertex->estimate(); Vector3f lower(-.5+relposv.translation().x(), -2.+relposv.translation().y(), -1.+relposv.rotation().angle()); Vector3f upper( .5+relposv.translation().x(), 2.+relposv.translation().y(), 1.+relposv.rotation().angle()); reg.lowerLeft = lower; reg.upperRight = upper; regions.push_back(reg); lower[2] += M_PI; upper[2] += M_PI; reg.lowerLeft = lower; reg.upperRight = upper; regionspi.push_back(reg); } std::vector<MatcherResult> mresvec; double thetaRes = 0.025; // was 0.0125*.5 //Results discretization double dx = 0.5, dy = 0.5, dth = 0.2; std::map<DiscreteTriplet, MatcherResult> resultsMap; clock_t t_ini, t_fin; double secs; t_ini = clock(); _grid.greedySearch(mresvec, reducedScans, regions, thetaRes, maxScore, dx, dy, dth); t_fin = clock(); secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC; printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size()); if (mresvec.size()){ mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]); cerr << "Found Loop Closure Edge. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl; CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth); } t_ini = clock(); _grid.greedySearch(mresvec, reducedScans, regionspi, thetaRes, maxScore, dx, dy, dth); t_fin = clock(); secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC; printf("%.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size()); if (mresvec.size()){ mresvec[0].transformation[2] = normalize_theta(mresvec[0].transformation[2]); cerr << "Found Loop Closure Edge PI. Transf: " << mresvec[0].transformation.x() << " " << mresvec[0].transformation.y() << " " << mresvec[0].transformation.z() << endl; CharGrid::addToPrunedMap(resultsMap, mresvec[0], dx, dy, dth); } for (std::map<DiscreteTriplet, MatcherResult>::iterator it = resultsMap.begin(); it!= resultsMap.end(); it++){ MatcherResult res = it->second; Vector3d adj=res.transformation; SE2 transf; transf.setTranslation(Vector2d(adj.x(), adj.y())); transf.setRotation(normalize_theta(adj.z())); trel.push_back(transf); std::cerr << "Final result: " << transf.translation().x() << " " << transf.translation().y() << " " << transf.rotation().angle() << std::endl; } if (trel.size()) return true; return false; }
bool ScanMatcher::closeScanMatching(OptimizableGraph::VertexSet& vset, OptimizableGraph::Vertex* _originVertex, OptimizableGraph::Vertex* _currentVertex, SE2 *trel, double maxScore){ VertexSE2* currentVertex=dynamic_cast<VertexSE2*>(_currentVertex); VertexSE2* originVertex =dynamic_cast<VertexSE2*>(_originVertex); resetGrid(); RawLaser::Point2DVector scansInRefVertex; transformPointsFromVSet(vset, _originVertex, scansInRefVertex); _grid.addAndConvolvePoints<RawLaser::Point2DVector>(scansInRefVertex.begin(), scansInRefVertex.end(), _kernel); //Current vertex RobotLaser* lasercv = dynamic_cast<RobotLaser*>(currentVertex->userData()); if (!lasercv) return false; RawLaser::Point2DVector cvscan = lasercv->cartesian(); SE2 laserPoseCV = lasercv->laserParams().laserPose; RawLaser::Point2DVector cvScanRobot; applyTransfToScan(laserPoseCV, cvscan, cvScanRobot); SE2 delta = originVertex->estimate().inverse() * currentVertex->estimate(); Vector3d initGuess(delta.translation().x(), delta.translation().y(), delta.rotation().angle()); std::vector<MatcherResult> mresvec; clock_t t_ini, t_fin; double secs; t_ini = clock(); double thetaRes = 0.0125*.5; // was 0.01 Vector3f lower(-.3+initGuess.x(), -.3+initGuess.y(), -0.2+initGuess.z()); Vector3f upper(+.3+initGuess.x(), .3+initGuess.y(), 0.2+initGuess.z()); _grid.greedySearch(mresvec, cvScanRobot, lower, upper, thetaRes, maxScore, 0.5, 0.5, 0.2); t_fin = clock(); secs = (double)(t_fin - t_ini) / CLOCKS_PER_SEC; printf("Greedy search: %.16g ms. Matcher results: %i\n", secs * 1000.0, (int) mresvec.size()); if (mresvec.size()){ Vector3d adj=mresvec[0].transformation; trel->setTranslation(Vector2d(adj.x(), adj.y())); trel->setRotation(adj.z()); //cerr << "bestScore = " << mresvec[0].score << endl << endl; // if (currentVertex->id() > 120 && currentVertex->id() < 200){ // CharMatcher auxGrid = _grid; // Vector2dVector transformedScan; // transformedScan.resize(cvScanRobot.size()); // for (unsigned int i = 0; i<cvScanRobot.size(); i++){ // SE2 point; // point.setTranslation(cvScanRobot[i]); // SE2 transformedpoint = *trel * point; // transformedScan[i] = transformedpoint.translation(); // } // auxGrid.addPoints<RawLaser::Point2DVector>(transformedScan.begin(), transformedScan.end()); // ofstream image; // std::stringstream filename; // filename << "matchedmap" << currentVertex->id() << "_" << mresvec[0].score << ".ppm"; // image.open(filename.str().c_str()); // auxGrid.grid().saveAsPPM(image, false); // } return true; } cerr << endl; 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; }
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); optimizer.setForceStopFlag(&hasToStop); allocateSolverForSclam(optimizer); // loading if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) { cerr << "Error while loading gm2dl file" << endl; } DataQueue robotLaserQueue; int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue); if (numLaserOdom == 0) { cerr << "No raw information read" << endl; return 0; } cerr << "Read " << numLaserOdom << " laser readings from file" << endl; bool gaugeFreedom = optimizer.gaugeFreedom(); OptimizableGraph::Vertex* gauge = optimizer.findGauge(); if (gaugeFreedom) { if (! gauge) { cerr << "# cannot find a vertex to fix in this thing" << endl; return 2; } else { cerr << "# graph is fixed by node " << gauge->id() << endl; gauge->setFixed(true); } } else { cerr << "# graph is fixed by priors" << endl; } addOdometryCalibLinksDifferential(optimizer, robotLaserQueue); // sanity check HyperDijkstra d(&optimizer); UniformCostFunction f; d.shortestPaths(gauge, &f); //cerr << PVAR(d.visited().size()) << endl; if (d.visited().size()!=optimizer.vertices().size()) { cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl; cerr << "visited: " << d.visited().size() << endl; cerr << "vertices: " << optimizer.vertices().size() << endl; if (1) for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second); if (d.visited().count(v) == 0) { cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl; v->setFixed(true); } } } for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) { OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second); if (v->fixed()) { cerr << "\t fixed vertex " << it->first << endl; } } VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE)); VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB)); if (fixLaser) { cerr << "Fix position of the laser offset" << endl; laserOffset->setFixed(true); } signal(SIGINT, sigquit_handler); cerr << "Doing full estimation" << endl; optimizer.initializeOptimization(); optimizer.computeActiveErrors(); cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl; int i=optimizer.optimize(maxIterations); if (maxIterations > 0 && !i){ cerr << "optimize failed, result might be invalid" << endl; } if (laserOffset) { cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl; } if (odomParamsVertex) { cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl; } cerr << "vertices: " << optimizer.vertices().size() << endl; cerr << "edges: " << optimizer.edges().size() << endl; if (dumpGraphFilename.size() > 0) { cerr << "Writing " << dumpGraphFilename << " ... "; optimizer.save(dumpGraphFilename.c_str()); cerr << "done." << endl; } // optional input of a seperate 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"); const Vector3d& odomCalib = odomParamsVertex->estimate(); 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 measurment 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; } } } if (outputfilename.size() > 0) { Gm2dlIO::updateLaserData(optimizer); cerr << "Writing " << outputfilename << " ... "; bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer); cerr << (writeStatus ? "done." : "failed") << endl; } return 0; }
HyperGraphElementAction* EdgeSE2Landmark_malcolmDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ // std::cout << "DRAW up :D" << std::endl;exit(0); if (typeid(*element).name()!=_typeName){ std::cout << "Wrong name :(" <<std::endl;; return 0; } // refreshPropertyPtrs(params_); // if (! _previousParams) // return this; // // if (_show && !_show->value()) // return this; // // EdgeSE2Prior* e = static_cast<EdgeSE2Prior*>(element); // VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0)); // VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1)); // // EdgeSE2PointXY* e = static_cast<EdgeSE2PointXY*>(element); // // VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0)); // // VertexPointXY* toEdge = static_cast<VertexPointXY*>(e->vertex(1)); // if (! fromEdge) // return this; // Vector3D p3d=e->measurement().toVector(); // Vector2D p; // p << p3d(0), p3d(1); // glPushAttrib(GL_ENABLE_BIT|GL_LIGHTING|GL_COLOR); // glDisable(GL_LIGHTING); // if (!toEdge){ // p=fromEdge->estimate()/*.toVector()*/*p; // glColor3f(LANDMARK_EDGE_GHOST_COLOR); // glPushAttrib(GL_POINT_SIZE); // glPointSize(3); // glBegin(GL_POINTS); // glVertex3f((float)p.x(),(float)p.y(),0.f); // glEnd(); // glPopAttrib(); // } else { // p=toEdge->estimate().toVector(); // glColor3f(LANDMARK_EDGE_COLOR); // } // glBegin(GL_LINES); // glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),0.f); // glVertex3f((float)p.x(),(float)p.y(),0.f); // glEnd(); // glPopAttrib(); // return this; refreshPropertyPtrs(params_); if (! _previousParams) return this; if (_show && !_show->value()) return this; EdgeSE2Landmark_malcolm* e = static_cast<EdgeSE2Landmark_malcolm*>(element); VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0)); VertexSE2* to = static_cast<VertexSE2*>(e->vertex(1)); if (! from && ! to) return this; SE2 fromTransform; SE2 toTransform; glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING | GL_COLOR); glDisable(GL_LIGHTING); if (! from) { glColor3f(POSE_EDGE_LANDMARK_MALCOLM_GHOST_COLOR); toTransform = to->estimate(); fromTransform = to->estimate()*e->measurement().inverse(); // DRAW THE FROM EDGE AS AN ARROW glPushMatrix(); glTranslatef((float)fromTransform.translation().x(), (float)fromTransform.translation().y(),0.f); glRotatef((float)RAD2DEG(fromTransform.rotation().angle()),0.f,0.f,1.f); opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f); glPopMatrix(); } else if (! to){ glColor3f(POSE_EDGE_LANDMARK_MALCOLM_GHOST_COLOR); fromTransform = from->estimate(); toTransform = from->estimate()*e->measurement(); // DRAW THE TO EDGE AS AN ARROW glPushMatrix(); glTranslatef(toTransform.translation().x(),toTransform.translation().y(),0.f); glRotatef((float)RAD2DEG(toTransform.rotation().angle()),0.f,0.f,1.f); opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f); glPopMatrix(); } else { glColor3f(POSE_EDGE_LANDMARK_MALCOLM_COLOR); fromTransform = from->estimate(); toTransform = to->estimate(); } glBegin(GL_LINES); glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),0.f); glVertex3f((float)toTransform.translation().x(),(float)toTransform.translation().y(),0.f); glEnd(); glPopAttrib(); return this; }