本文整理汇总了C++中VertexSE2类的典型用法代码示例。如果您正苦于以下问题：C++ VertexSE2类的具体用法？C++ VertexSE2怎么用？C++ VertexSE2使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了VertexSE2类的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: lockg
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());
}
}
}
}*/
}
开发者ID:droter，项目名称:cg_mrslam，代码行数:57，代码来源:graph_slam.cpp
示例2: assert
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);
}
开发者ID:2maz，项目名称:g2o，代码行数:10，代码来源:edge_se2_pointxy_calib.cpp
示例3:
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;
}
开发者ID:Aerobota，项目名称:c2tam，代码行数:14，代码来源:vertex_se2.cpp
示例4: main
int main(int argc, char** argv) {
CommandArgs arg;
std::string outputFilename;
std::string inputFilename;
arg.param("o", outputFilename, "", "output file name");
arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true);
arg.parseArgs(argc, argv);
OptimizableGraph graph;
if (!graph.load(inputFilename.c_str())){
cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl;
return 0;
}
HyperGraph::EdgeSet removedEdges;
HyperGraph::VertexSet removedVertices;
for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) {
HyperGraph::Edge* e = *it;
EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e);
if (edgePointXY) {
VertexSE2* pose = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0));
VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1));
FeaturePointXYData * feature = new FeaturePointXYData();
feature->setPositionMeasurement(edgePointXY->measurement());
feature->setPositionInformation(edgePointXY->information());
pose->addUserData(feature);
removedEdges.insert(edgePointXY);
removedVertices.insert(landmark);
}
}
for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){
OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it);
graph.removeEdge(e);
}
for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){
OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
graph.removeVertex(v);
}
if (outputFilename.length()){
graph.save(outputFilename.c_str());
}
}
开发者ID:9578577，项目名称:g2o_frontend，代码行数:48，代码来源:g2o_anonymize_observations.cpp
示例5: ce
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);
}
}
开发者ID:droter，项目名称:cg_mrslam，代码行数:44，代码来源:graph_slam.cpp
示例6: assert
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);
}
开发者ID:droter，项目名称:cg_mrslam，代码行数:43，代码来源:graph_ros_publisher.cpp
示例7: refreshPropertyPtrs
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;
}
开发者ID:Aerobota，项目名称:c2tam，代码行数:43，代码来源:vertex_se2.cpp
示例8: refreshPropertyPtrs
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;
}
开发者ID:pangfumin，项目名称:g2o，代码行数:53，代码来源:edge_se2.cpp
示例9:
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;
}
开发者ID:jachu51，项目名称:DiamentowyGrant，代码行数:19，代码来源:edge_se2_distanceOrientation.cpp
示例10: updateVertexIdx
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);
}
}
}
开发者ID:AIS-Bonn，项目名称:humanoid_op_ros，代码行数:42，代码来源:Localization.hpp
示例11: refreshPropertyPtrs
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;
}
开发者ID:jachu51，项目名称:DiamentowyGrant，代码行数:41，代码来源:edge_se2_distanceOrientation.cpp
示例12: refreshPropertyPtrs
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;
}
开发者ID:Florenc，项目名称:g2o，代码行数:25，代码来源:edge_se2.cpp
示例13: transformPointsFromVSet
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());
}
}
}
开发者ID:droter，项目名称:cg_mrslam，代码行数:22，代码来源:scan_matcher.cpp
示例14:
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);
}
开发者ID:pangfumin，项目名称:g2o，代码行数:9，代码来源:edge_se2.cpp
示例15:
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());
}
开发者ID:jachu51，项目名称:DiamentowyGrant，代码行数:11，代码来源:edge_se2_placeVicinity.cpp
示例16: perform
virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e)
{
if (! vParent)
return 0.;
EdgeSE2* odom = static_cast<EdgeSE2*>(e);
VertexSE2* from = static_cast<VertexSE2*>(vParent);
VertexSE2* to = static_cast<VertexSE2*>(v);
assert(to->hessianIndex() >= 0);
double fromTheta = from->hessianIndex() < 0 ? 0. : _thetaGuess[from->hessianIndex()];
bool direct = odom->vertices()[0] == from;
if (direct)
_thetaGuess[to->hessianIndex()] = fromTheta + odom->measurement().rotation().angle();
else
_thetaGuess[to->hessianIndex()] = fromTheta - odom->measurement().rotation().angle();
return 1.;
}
开发者ID:2maz，项目名称:g2o，代码行数:16，代码来源:solver_slam2d_linear.cpp
示例17: tmpMeasurement
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);
}
开发者ID:jachu51，项目名称:DiamentowyGrant，代码行数:17，代码来源:edge_se2_distanceOrientation.cpp
示例18: glPushAttrib
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;
}
开发者ID:ericperko，项目名称:g2o，代码行数:17，代码来源:edge_se2_odom_differential_calib.cpp
示例19: main
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 *
//.........这里部分代码省略.........
开发者ID:lfermin77，项目名称:g2o2ros，代码行数:101，代码来源:g2o2ros.cpp
示例20: resetGrid
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;
}
开发者ID:droter，项目名称:cg_mrslam，代码行数:76，代码来源:scan_matcher.cpp
注：本文中的VertexSE2类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。 |
客服电话
APP下载
官方微信
请发表评论