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;
}
