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);
}
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());
}
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);
}
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 Localization::InitG2OGraph()
{
optimizer.clear();
LandmarkCount = 0;
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(0, 0, 0));
l->setFixed(true);
l->setId(CenterL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(A2, 0, 0));
l->setFixed(true);
l->setId(FrontL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(-A2, 0, 0));
l->setFixed(true);
l->setId(BackL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(0, -B2, 0));
l->setFixed(true);
l->setId(RightL);
optimizer.addVertex(l);
LandmarkCount++;
}
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(Eigen::Vector3d(0, B2, 0));
l->setFixed(true);
l->setId(LeftL);
optimizer.addVertex(l);
LandmarkCount++;
}
PreviousVertexId = -1;
CurrentVertexId = LandmarkCount;
{
VertexSE2 * l = new VertexSE2;
l->setEstimate(
Eigen::Vector3d(location.x, location.y, 0));
l->setFixed(false);
l->setId(LandmarkCount);
optimizer.addVertex(l);
}
}
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);
}
}
}
int main(int argc, char** argv)
{
bool fixLaser;
int maxIterations;
bool verbose;
string inputFilename;
string outputfilename;
string rawFilename;
string odomTestFilename;
string dumpGraphFilename;
// command line parsing
CommandArgs commandLineArguments;
commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");
commandLineArguments.parseArgs(argc, argv);
SparseOptimizer optimizer;
optimizer.setVerbose(verbose);
allocateSolverForSclam(optimizer);
// loading
DataQueue odometryQueue;
int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue);
if (numLaserOdom == 0) {
cerr << "No raw information read" << endl;
return 0;
}
cerr << "Read " << numLaserOdom << " laser readings from file" << endl;
Eigen::Vector3d odomCalib(1., 1., 1.);
SE2 initialLaserPose;
DataQueue robotLaserQueue;
int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue);
if (numRobotLaser == 0) {
cerr << "No robot laser read" << endl;
return 0;
} else {
RobotLaser* rl = dynamic_cast<RobotLaser*>(robotLaserQueue.buffer().begin()->second);
initialLaserPose = rl->odomPose().inverse() * rl->laserPose();
cerr << PVAR(initialLaserPose.toVector().transpose()) << endl;
}
// adding the measurements
vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
{
std::map<double, RobotData*>::const_iterator it = robotLaserQueue.buffer().begin();
std::map<double, RobotData*>::const_iterator prevIt = it++;
for (; it != robotLaserQueue.buffer().end(); ++it) {
MotionInformation mi;
RobotLaser* prevLaser = dynamic_cast<RobotLaser*>(prevIt->second);
RobotLaser* curLaser = dynamic_cast<RobotLaser*>(it->second);
mi.laserMotion = prevLaser->laserPose().inverse() * curLaser->laserPose();
// get the motion of the robot in that time interval
RobotLaser* prevOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(prevLaser->timestamp()));
RobotLaser* curOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(curLaser->timestamp()));
mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
prevIt = it;
motions.push_back(mi);
}
}
if (1) {
VertexSE2* laserOffset = new VertexSE2;
laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
laserOffset->setEstimate(initialLaserPose);
optimizer.addVertex(laserOffset);
VertexOdomDifferentialParams* odomParamsVertex = new VertexOdomDifferentialParams;
odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
odomParamsVertex->setEstimate(Eigen::Vector3d(1., 1., 1.));
optimizer.addVertex(odomParamsVertex);
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
// add the edge
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
OdomAndLaserMotion meas;
meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
meas.laserMotion = laserMotion;
EdgeSE2PureCalib* calibEdge = new EdgeSE2PureCalib;
calibEdge->setVertex(0, laserOffset);
calibEdge->setVertex(1, odomParamsVertex);
calibEdge->setInformation(Eigen::Matrix3d::Identity());
calibEdge->setMeasurement(meas);
if (! optimizer.addEdge(calibEdge)) {
cerr << "Error adding calib edge" << endl;
delete calibEdge;
}
}
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
cerr << "\nPerforming full non-linear estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(maxIterations);
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
odomCalib = odomParamsVertex->estimate();
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomParamsVertex->estimate().transpose() << endl;
optimizer.clear();
}
// linear least squares for some parameters
{
Eigen::MatrixXd A(motions.size(), 2);
Eigen::VectorXd x(motions.size());
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
A(i, 0) = velMeas.vl() * timeInterval;
A(i, 1) = velMeas.vr() * timeInterval;
x(i) = laserMotion.rotation().angle();
}
//linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
linearSolution = A.colPivHouseholderQr().solve(x);
//cout << PVAR(linearSolution.transpose()) << endl;
}
//constructing non-linear least squares
VertexSE2* laserOffset = new VertexSE2;
laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
laserOffset->setEstimate(initialLaserPose);
optimizer.addVertex(laserOffset);
VertexBaseline* odomParamsVertex = new VertexBaseline;
odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
odomParamsVertex->setEstimate(1.);
optimizer.addVertex(odomParamsVertex);
for (size_t i = 0; i < motions.size(); ++i) {
const SE2& odomMotion = motions[i].odomMotion;
const SE2& laserMotion = motions[i].laserMotion;
const double& timeInterval = motions[i].timeInterval;
// add the edge
MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
OdomAndLaserMotion meas;
meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
meas.laserMotion = laserMotion;
EdgeCalib* calibEdge = new EdgeCalib;
calibEdge->setVertex(0, laserOffset);
calibEdge->setVertex(1, odomParamsVertex);
calibEdge->setInformation(Eigen::Matrix3d::Identity());
calibEdge->setMeasurement(meas);
if (! optimizer.addEdge(calibEdge)) {
cerr << "Error adding calib edge" << endl;
delete calibEdge;
}
}
if (fixLaser) {
cerr << "Fix position of the laser offset" << endl;
laserOffset->setFixed(true);
}
cerr << "\nPerforming partial non-linear estimation" << endl;
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
optimizer.optimize(maxIterations);
cerr << "Calibrated laser offset (x, y, theta):" << laserOffset->estimate().toVector().transpose() << endl;
odomCalib(0) = -1. * linearSolution(0) * odomParamsVertex->estimate();
odomCalib(1) = linearSolution(1) * odomParamsVertex->estimate();
odomCalib(2) = odomParamsVertex->estimate();
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << odomCalib.transpose() << endl;
{
SE2 closedFormLaser;
Eigen::Vector3d closedFormOdom;
ClosedFormCalibration::calibrate(motions, closedFormLaser, closedFormOdom);
cerr << "\nObtaining closed form solution" << endl;
cerr << "Calibrated laser offset (x, y, theta):" << closedFormLaser.toVector().transpose() << endl;
cerr << "Odometry parameters (scaling factors (v_l, v_r, b)): " << closedFormOdom.transpose() << endl;
}
if (dumpGraphFilename.size() > 0) {
cerr << "Writing " << dumpGraphFilename << " ... ";
optimizer.save(dumpGraphFilename.c_str());
cerr << "done." << endl;
}
// optional input of a separate file for applying the odometry calibration
if (odomTestFilename.size() > 0) {
DataQueue testRobotLaserQueue;
int numTestOdom = Gm2dlIO::readRobotLaser(odomTestFilename, testRobotLaserQueue);
if (numTestOdom == 0) {
cerr << "Unable to read test data" << endl;
} else {
ofstream rawStream("odometry_raw.txt");
ofstream calibratedStream("odometry_calibrated.txt");
RobotLaser* prev = dynamic_cast<RobotLaser*>(testRobotLaserQueue.buffer().begin()->second);
SE2 prevCalibratedPose = prev->odomPose();
for (DataQueue::Buffer::const_iterator it = testRobotLaserQueue.buffer().begin(); it != testRobotLaserQueue.buffer().end(); ++it) {
RobotLaser* cur = dynamic_cast<RobotLaser*>(it->second);
assert(cur);
double dt = cur->timestamp() - prev->timestamp();
SE2 motion = prev->odomPose().inverse() * cur->odomPose();
// convert to velocity measurement
MotionMeasurement motionMeasurement(motion.translation().x(), motion.translation().y(), motion.rotation().angle(), dt);
VelocityMeasurement velocityMeasurement = OdomConvert::convertToVelocity(motionMeasurement);
// apply calibration
VelocityMeasurement calibratedVelocityMeasurment = velocityMeasurement;
calibratedVelocityMeasurment.setVl(odomCalib(0) * calibratedVelocityMeasurment.vl());
calibratedVelocityMeasurment.setVr(odomCalib(1) * calibratedVelocityMeasurment.vr());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomCalib(2));
// combine calibrated odometry with the previous pose
SE2 remappedOdom;
remappedOdom.fromVector(mm.measurement());
SE2 calOdomPose = prevCalibratedPose * remappedOdom;
// write output
rawStream << prev->odomPose().translation().x() << " " << prev->odomPose().translation().y() << " " << prev->odomPose().rotation().angle() << endl;
calibratedStream << calOdomPose.translation().x() << " " << calOdomPose.translation().y() << " " << calOdomPose.rotation().angle() << endl;
prevCalibratedPose = calOdomPose;
prev = cur;
}
}
}
return 0;
}
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 *
************************************************************************/
// Create the map
Eigen::Vector2i size;
if(rows != 0 && cols != 0) { size = Eigen::Vector2i(rows, cols); }
else {
size = Eigen::Vector2i((boundingBox(0, 1) - boundingBox(0, 0))/ resolution,
(boundingBox(1, 1) - boundingBox(1, 0))/ resolution);
}
std::cout << "Map size: " << size.transpose() << std::endl;
if(size.x() == 0 || size.y() == 0) {
std::cout << "Zero map size ... quitting!" << std::endl;
return 0;
}
//Eigen::Vector2f offset(-size.x() * resolution / 2.0f, -size.y() * resolution / 2.0f);
Eigen::Vector2f offset(boundingBox(0, 0),boundingBox(1, 0));
FrequencyMapCell unknownCell;
FrequencyMap map = FrequencyMap(resolution, offset, size, unknownCell);
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; }
OptimizableGraph::Data *d = v->userData();
SE2 robotPose = v->estimate();
while(d) {
RobotLaser *robotLaser = dynamic_cast<RobotLaser*>(d);
if(!robotLaser) {
d = d->next();
continue;
}
map.integrateScan(robotLaser, robotPose, max_range, usable_range, gain, square_size);
d = d->next();
}
}
/************************************************************************
* Save map image *
************************************************************************/
cv::Mat mapImage(map.rows(), map.cols(), CV_8UC1);
mapImage.setTo(cv::Scalar(0));
for(int c = 0; c < map.cols(); c++) {
for(int r = 0; r < map.rows(); r++) {
if(map(r, c).misses() == 0 && map(r, c).hits() == 0) {
mapImage.at<unsigned char>(r, c) = 127;
} else {
float fraction = (float)map(r, c).hits()/(float)(map(r, c).hits()+map(r, c).misses());
if (threshold > 0 && fraction > threshold)
mapImage.at<unsigned char>(r, c) = 0;
else if (threshold > 0 && fraction <= threshold)
mapImage.at<unsigned char>(r, c) = 255;
else {
float val = 255*(1-fraction);
mapImage.at<unsigned char>(r, c) = (unsigned char)val;
}
}
// else if(map(r, c).hits() > threshold) {
// mapImage.at<unsigned char>(r, c) = 255;
// }
// else {
// mapImage.at<unsigned char>(r, c) = 0;
// }
}
}
cv::imwrite(mapFilename + ".png", mapImage);
/************************************************************************
* Write yaml file *
************************************************************************/
std::ofstream ofs(string(mapFilename + ".yaml").c_str());
Eigen::Vector3f origin(0.0f, 0.0f, 0.0f);
ofs << "image: " << mapFilename << ".png" << std::endl
<< "resolution: " << resolution << std::endl
<< "origin: [" << origin.x() << ", " << origin.y() << ", " << origin.z() << "]" << std::endl
<< "negate: 0" << std::endl
<< "occupied_thresh: " << 0.65f << std::endl
<< "free_thresh: " << 0.2f << std::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;
}
bool SolverSLAM2DLinear::solveOrientation()
{
assert(_optimizer->indexMapping().size() + 1 == _optimizer->vertices().size() && "Needs to operate on full graph");
assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0");
VectorXD b, x; // will be used for theta and x/y update
b.setZero(_optimizer->indexMapping().size());
x.setZero(_optimizer->indexMapping().size());
typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix;
ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]);
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
blockIndeces[i] = i+1;
SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _optimizer->indexMapping().size(), _optimizer->indexMapping().size());
// building the structure, diagonal for each active vertex
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
int poseIdx = v->hessianIndex();
ScalarMatrix* m = H.block(poseIdx, poseIdx, true);
m->setZero();
}
HyperGraph::VertexSet fixedSet;
// off diagonal for each edge
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
# ifndef NDEBUG
EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
assert(e && "Active edges contain non-odometry edge"); //
# else
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
# endif
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
int ind1 = from->hessianIndex();
int ind2 = to->hessianIndex();
if (ind1 == -1 || ind2 == -1) {
if (ind1 == -1) fixedSet.insert(from); // collect the fixed vertices
if (ind2 == -1) fixedSet.insert(to);
continue;
}
bool transposedBlock = ind1 > ind2;
if (transposedBlock){ // make sure, we allocate the upper triangle block
std::swap(ind1, ind2);
}
ScalarMatrix* m = H.block(ind1, ind2, true);
m->setZero();
}
// walk along the Minimal Spanning Tree to compute the guess for the robot orientation
assert(fixedSet.size() == 1);
VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin());
VectorXD thetaGuess;
thetaGuess.setZero(_optimizer->indexMapping().size());
UniformCostFunction uniformCost;
HyperDijkstra hyperDijkstra(_optimizer);
hyperDijkstra.shortestPaths(root, &uniformCost);
HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
ThetaTreeAction thetaTreeAction(thetaGuess.data());
HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(), &thetaTreeAction);
// construct for the orientation
for (SparseOptimizer::EdgeContainer::const_iterator it = _optimizer->activeEdges().begin(); it != _optimizer->activeEdges().end(); ++it) {
EdgeSE2* e = static_cast<EdgeSE2*>(*it);
VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]);
VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]);
double omega = e->information()(2,2);
double fromThetaGuess = from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()];
double toThetaGuess = to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()];
double error = normalize_theta(-e->measurement().rotation().angle() + toThetaGuess - fromThetaGuess);
bool fromNotFixed = !(from->fixed());
bool toNotFixed = !(to->fixed());
if (fromNotFixed || toNotFixed) {
double omega_r = - omega * error;
if (fromNotFixed) {
b(from->hessianIndex()) -= omega_r;
(*H.block(from->hessianIndex(), from->hessianIndex()))(0,0) += omega;
if (toNotFixed) {
if (from->hessianIndex() > to->hessianIndex())
(*H.block(to->hessianIndex(), from->hessianIndex()))(0,0) -= omega;
else
(*H.block(from->hessianIndex(), to->hessianIndex()))(0,0) -= omega;
}
}
if (toNotFixed ) {
b(to->hessianIndex()) += omega_r;
(*H.block(to->hessianIndex(), to->hessianIndex()))(0,0) += omega;
}
}
}
// solve orientation
typedef LinearSolverCSparse<ScalarMatrix> SystemSolver;
SystemSolver linearSystemSolver;
linearSystemSolver.init();
bool ok = linearSystemSolver.solve(H, x.data(), b.data());
if (!ok) {
cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl;
return false;
}
// update the orientation of the 2D poses and set translation to 0, GN shall solve that
root->setToOrigin();
for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]);
int poseIdx = v->hessianIndex();
SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx)));
v->setEstimate(poseUpdate);
}
return true;
}
