本文整理汇总了C++中VertexSE2类的典型用法代码示例。如果您正苦于以下问题:C++ VertexSE2类的具体用法?C++ VertexSE2怎么用?C++ VertexSE2使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了VertexSE2类的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的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());
}
}
}
}*/
}
示例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);
}
示例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;
}
示例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());
}
}
示例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);
}
}
示例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);
}
示例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;
}
示例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;
}
示例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;
}
示例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);
}
}
}
示例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;
}
示例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;
}
示例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());
}
}
}
示例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);
}
示例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());
}