void GraphSLAM::addData(SE2 currentOdom, RobotLaser* laser){ boost::mutex::scoped_lock lockg(graphMutex); //Add current vertex VertexSE2 *v = new VertexSE2; SE2 displacement = _lastOdom.inverse() * currentOdom; SE2 currEst = _lastVertex->estimate() * displacement; v->setEstimate(currEst); v->setId(++_runningVertexId + idRobot() * baseId()); //Add covariance information //VertexEllipse *ellipse = new VertexEllipse; //Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance //ellipse->setCovariance(cov); //v->setUserData(ellipse); v->addUserData(laser); std::cout << std::endl << "Current vertex: " << v->id() << " Estimate: "<< v->estimate().translation().x() << " " << v->estimate().translation().y() << " " << v->estimate().rotation().angle() << std::endl; _graph->addVertex(v); //Add current odometry edge EdgeSE2 *e = new EdgeSE2; e->setId(++_runningEdgeId + idRobot() * baseId()); e->vertices()[0] = _lastVertex; e->vertices()[1] = v; e->setMeasurement(displacement); // //Computing covariances depending on the displacement // Vector3d dis = displacement.toVector(); // dis.x() = fabs(dis.x()); // dis.y() = fabs(dis.y()); // dis.z() = fabs(dis.z()); // dis += Vector3d(1e-3,1e-3,1e-2); // Matrix3d dis2 = dis*dis.transpose(); // Matrix3d newcov = dis2.cwiseProduct(_odomK); e->setInformation(_odominf); _graph->addEdge(e); _odomEdges.insert(e); _lastOdom = currentOdom; _lastVertex = v; }
void 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::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; }
int main(int argc, char** argv) { string rawFilename; bool use_viso = false; bool use_cfs = false; double init_x, init_y; cout << endl << "\033[32mA demo implementing the method of stereo-odo calibration.\033[0m" << endl << "* * * Author: \033[32mDoom @ ZJU.\033[0m" << endl << "Usage: sclam_odo_stereo_cal USE_VISO INPUT_FILENAME USE_CLOSEFORM" << endl << endl; if(argc < 2){ cout << "\033[33m[Warning]:No input directory name, using the default one : /home/doom/CSO/data/params.yaml\033[0m" << endl << endl; // Read in our param file CYaml::get().parseParamFile("/home/doom/CSO/data/params.yaml"); // Read in params use_viso = CYaml::get().general()["use_viso"].as<bool>(); rawFilename = CYaml::get().general()["data_folder"].as<std::string>(); use_cfs = CYaml::get().general()["use_closed_form"].as<bool>(); init_x = CYaml::get().general()["init_x"].as<double>(); init_y = CYaml::get().general()["init_y"].as<double>(); } else if(argc == 2){ cout << "\033[31mInput params file directory: \033[0m" << argv[1] << endl << endl; // Read in our param file CYaml::get().parseParamFile(argv[1]); // Read in params use_viso = CYaml::get().general()["use_viso"].as<bool>(); rawFilename = CYaml::get().general()["data_folder"].as<std::string>(); use_cfs = CYaml::get().general()["use_closed_form"].as<bool>(); init_x = CYaml::get().general()["init_x"].as<double>(); init_y = CYaml::get().general()["init_y"].as<double>(); } else { cerr << "\033[31m[FATAL]Bad parameters.\033[0m" << endl; return 1; } // construct a new optimizer SparseOptimizer optimizer; initOptimizer(optimizer); // read the times.txt, to determine how many pics in this directory. stringstream ss; ss << rawFilename << "/votimes.txt"; ifstream in(ss.str().c_str()); int Num = 0; vector<double> timeque; if(in.fail()) { cerr << "\033[1;31m[ERROR]Wrong foldername or no times.txt in this directory.\033[0m" << endl; return 1; } else { string stemp; getline(in, stemp, '\n'); while(in.good()){ Num++; getline(in, stemp, '\n'); } cout << "There are " << Num << " pics in this direcotory." << endl << endl; in.close(); in.open(ss.str().c_str()); double temp; for(int i = 0; i < Num; i++) { in >> temp; timeque.push_back(temp); } in.close(); } // loading odom data cerr << "\033[31mNow loading odometry data.\033[0m" << endl; string odomName = rawFilename + "/newodom.txt"; DataQueue odometryQueue; DataQueue stereovoQueue; SE2 init_offset; int numOdom = Gm2dlIO::readRobotOdom(odomName, odometryQueue); if (numOdom == 0) { cerr << "No raw information read" << endl; return 0; } cerr << "Done...Read " << numOdom << " odom readings from file" << endl << endl; // initial first stereo frame pose SE2 initialStereoPose; bool first = true; int numVo = 0; if(use_viso) { cout << "\033[31mUsing libviso...\033[0m" << rawFilename << endl; RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second); // init a libviso2 StereoVo viso(rawFilename, Num, ro, timeque); // get initial odometry pose in robot frame and stereo vo. viso.getInitStereoPose(initialStereoPose); numVo = viso.getMotion(stereovoQueue); cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl; } else { cerr << "\033[31mLoading pose file...\033[0m" << endl; ss.str(""); ss << rawFilename << "/CameraTrajectory.txt"; in.open(ss.str().c_str()); int numVo = 0; Matrix4D posemat; for(int i = 0; i < Num; i++) { for(int j = 0; j < 4; j++) { for(int k = 0; k < 4; k++) in >> posemat(j, k); } if(first){ init_offset.setTranslation(Eigen::Vector2d(init_x, init_y)); init_offset.setRotation(Eigen::Rotation2Dd::Identity()); // SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0))); // RobotOdom* ro = dynamic_cast<RobotOdom*>(odometryQueue.buffer().begin()->second); initialStereoPose = init_offset; first = false; } // save stereo vo as robotOdom node. RobotOdom* tempVo = new RobotOdom; tempVo->setTimestamp(timeque[i]); SE2 x(posemat(2,3),-posemat(0,3),acos(posemat(0,0))); tempVo->setOdomPose(init_offset*x); stereovoQueue.add(tempVo); numVo++; } in.close(); cerr << "Done...There are " << numVo << " vo datas in the queue." << endl << endl; } // adding the measurements vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions; { std::map<double, RobotData*>::const_iterator it = stereovoQueue.buffer().begin(); std::map<double, RobotData*>::const_iterator prevIt = it++; for (; it != stereovoQueue.buffer().end(); ++it) { MotionInformation mi; RobotOdom* prevVo = dynamic_cast<RobotOdom*>(prevIt->second); RobotOdom* curVo = dynamic_cast<RobotOdom*>(it->second); mi.stereoMotion = prevVo->odomPose().inverse() * curVo->odomPose(); // get the motion of the robot in that time interval RobotOdom* prevOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(prevVo->timestamp())); RobotOdom* curOdom = dynamic_cast<RobotOdom*>(odometryQueue.findClosestData(curVo->timestamp())); mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose(); mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp(); prevIt = it; motions.push_back(mi); } } // Construct the graph. cerr << "\033[31mConstruct the graph...\033[0m" << endl; // Vertex offset addVertexSE2(optimizer, initialStereoPose, 0); for(int i = 0; i < motions.size(); i++) { const SE2& odomMotion = motions[i].odomMotion; const SE2& stereoMotion = motions[i].stereoMotion; // add the edge // stereo vo and odom measurement. OdomAndStereoMotion meas; meas.StereoMotion = stereoMotion; meas.odomMotion = odomMotion; addEdgeCalib(optimizer, 0, meas, Eigen::Matrix3d::Identity()); } cerr << "Done..." << endl << endl; // if you want check the component of your graph, uncomment the CHECK_GRAPH #ifdef CHECK_GRAPH for(auto it:optimizer.edges()) { VertexSE2* v = static_cast<VertexSE2*>(it->vertex(0)); cout << v->id() << endl; } #endif // optimize. optimize(optimizer, 10); Eigen::Vector3d result = getEstimation(optimizer); cerr << "\033[1;32mCalibrated stereo offset (x, y, theta):\033[0m" << result[0] << " " << result [1] << " " << result[2] << endl << endl; // If you want see how's its closed form solution, uncomment the CLOSED_FORM if(use_cfs){ cerr << "\033[31mPerforming the closed form solution...\033[0m" << endl; SE2 closedFormStereo; ClosedFormCalibration::calibrate(motions, closedFormStereo); cerr << "\033[1;32mDone... closed form Calibrated stereo offset (x, y, theta):\033[0m" << closedFormStereo.toVector().transpose() << endl; } return 0; }
void GraphSLAM::addDataSM(SE2 currentOdom, RobotLaser* laser){ boost::mutex::scoped_lock lockg(graphMutex); //Add current vertex VertexSE2 *v = new VertexSE2; SE2 displacement = _lastOdom.inverse() * currentOdom; SE2 currEst = _lastVertex->estimate() * displacement; v->setEstimate(currEst); v->setId(++_runningVertexId + idRobot() * baseId()); //Add covariance information //VertexEllipse *ellipse = new VertexEllipse; //Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance //ellipse->setCovariance(cov); //v->setUserData(ellipse); v->addUserData(laser); std::cout << endl << "Current vertex: " << v->id() << " Estimate: "<< v->estimate().translation().x() << " " << v->estimate().translation().y() << " " << v->estimate().rotation().angle() << std::endl; _graph->addVertex(v); //Add current odometry edge EdgeSE2 *e = new EdgeSE2; e->setId(++_runningEdgeId + idRobot() * baseId()); e->vertices()[0] = _lastVertex; e->vertices()[1] = v; OptimizableGraph::VertexSet vset; vset.insert(_lastVertex); int j = 1; int gap = 5; while (j <= gap){ VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j)); if (vj) vset.insert(vj); else break; j++; } SE2 transf; bool shouldIAdd = _closeMatcher.closeScanMatching(vset, _lastVertex, v, &transf, maxScore); if (shouldIAdd){ e->setMeasurement(transf); e->setInformation(_SMinf); }else{ //Trust the odometry e->setMeasurement(displacement); // Vector3d dis = displacement.toVector(); // dis.x() = fabs(dis.x()); // dis.y() = fabs(dis.y()); // dis.z() = fabs(dis.z()); // dis += Vector3d(1e-3,1e-3,1e-2); // Matrix3d dis2 = dis*dis.transpose(); // Matrix3d newcov = dis2.cwiseProduct(_odomK); // e->setInformation(newcov.inverse()); e->setInformation(_odominf); } _graph->addEdge(e); _lastOdom = currentOdom; _lastVertex = v; }