HyperGraphElementAction* RobotLaserDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_) { if (typeid(*element).name()!=_typeName) return 0; refreshPropertyPtrs(params_); if (! _previousParams) { return this; } RobotLaser* that = static_cast<RobotLaser*>(element); RawLaser::Point2DVector points=that->cartesian(); glPushMatrix(); const SE2& laserPose = that->laserParams().laserPose; glTranslatef((float)laserPose.translation().x(), (float)laserPose.translation().y(), 0.f); glRotatef((float)RAD2DEG(laserPose.rotation().angle()),0.f,0.f,1.f); glBegin(GL_POINTS); glColor4f(1.f,0.f,0.f,0.5f); int step = 1; if (_beamsDownsampling ) step = _beamsDownsampling->value(); if (_pointSize) { glPointSize(_pointSize->value()); } for (size_t i=0; i<points.size(); i+=step) { glVertex3f((float)points[i].x(), (float)points[i].y(), 0.f); } glEnd(); glPopMatrix(); return this; }
HyperGraphElementAction* RobotLaserDrawAction::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; RobotLaser* that = static_cast<RobotLaser*>(element); RawLaser::Point2DVector points=that->cartesian(); if (_maxRange && _maxRange->value() >=0 ) { // prune the cartesian points; RawLaser::Point2DVector npoints(points.size()); int k = 0; float r2=_maxRange->value(); r2 *= r2; for (size_t i=0; i<points.size(); i++){ double x = points[i].x(); double y = points[i].y(); if (x*x + y*y < r2) npoints[k++] = points[i]; } points = npoints; npoints.resize(k); } glPushMatrix(); const SE2& laserPose = that->laserParams().laserPose; glTranslatef((float)laserPose.translation().x(), (float)laserPose.translation().y(), 0.f); glRotatef((float)RAD2DEG(laserPose.rotation().angle()),0.f,0.f,1.f); glColor4f(1.f,0.f,0.f,0.5f); int step = 1; if (_beamsDownsampling ) step = _beamsDownsampling->value(); if (_pointSize) { glPointSize(_pointSize->value()); } glBegin(GL_POINTS); for (size_t i=0; i<points.size(); i+=step){ glVertex3f((float)points[i].x(), (float)points[i].y(), 0.f); } glEnd(); glPopMatrix(); return this; }
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); }
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()); } } }
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::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; }