void GraphSLAM::init(double resolution, double kernelRadius, int windowLoopClosure_, double maxScore_, double inlierThreshold_, int minInliers_){
boost::mutex::scoped_lock lockg(graphMutex);
//Init graph
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering(false);
SlamBlockSolver* blockSolver = new SlamBlockSolver(linearSolver);
OptimizationAlgorithmGaussNewton* solver = new OptimizationAlgorithmGaussNewton(blockSolver);
_graph->setAlgorithm(solver);
_graph->setVerbose(false);
//Init scan matchers
_closeMatcher.initializeKernel(resolution, kernelRadius);
_closeMatcher.initializeGrid(Vector2f(-15, -15), Vector2f(15, 15), resolution);
_LCMatcher.initializeKernel(0.1, 0.5); //before 0.1, 0.5
_LCMatcher.initializeGrid(Vector2f(-35, -35), Vector2f(35, 35), 0.1); //before 0.1
cerr << "Grids initialized\n";
windowLoopClosure = windowLoopClosure_;
maxScore = maxScore_;
inlierThreshold = inlierThreshold_;
minInliers = minInliers_;
//
_odominf = 100 * Matrix3d::Identity();
_odominf(2,2) = 1000;
_SMinf = 1000 * Matrix3d::Identity();
_SMinf(2,2) = 10000;
}
void GraphSLAM::setInitialData(SE2 initialTruePose, SE2 initialOdom, RobotLaser* laser){
boost::mutex::scoped_lock lockg(graphMutex);
_lastOdom = initialOdom;
//Add initial vertex
_lastVertex = new VertexSE2;
_lastVertex->setEstimate(initialTruePose);
_lastVertex->setId(idRobot() * baseId());
//Add covariance information
//VertexEllipse *ellipse = new VertexEllipse();
//Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
//ellipse->setCovariance(cov);
//_lastVertex->setUserData(ellipse);
_lastVertex->setUserData(laser);
std::cout <<
"Initial vertex: " << _lastVertex->id() <<
" Estimate: "<< _lastVertex->estimate().translation().x() <<
" " << _lastVertex->estimate().translation().y() <<
" " << _lastVertex->estimate().rotation().angle() << std::endl;
_graph->addVertex(_lastVertex);
_firstRobotPose = _lastVertex;
_firstRobotPose->setFixed(true);
}
void SharedMemoryBlock::reallocateBlock(boost::interprocess::managed_shared_memory & segment,
shm_handle & shm, uint32_t size) {
if (!shm.is_valid()) {
LOG_ERROR("Can't reallocateBlock associated to invalid handle");
return;
}
scoped_lock<interprocess_mutex> lock(descriptors[shm.handle].mutex);
LOG_DEBUG("reallocateBlock: locked " << shm.handle << ", checking clients");
descriptors[shm.handle].check_clients(lock);
LOG_DEBUG("reallocateBlock: locked " << shm.handle << ", clients checked");
// First check if the block has been changed since last access, and
// update the pointer if necessary
if (shm.resize_count != descriptors[shm.handle].resize_count_) {
scoped_lock<interprocess_mutex> lockg(mutex);
LOG_DEBUG("reallocateBlock: locked global, checking clients");
check_global_clients(lockg);
LOG_DEBUG("reallocateBlock: locked global, client checked");
std::pair<uint8_t *, std::size_t> ret = segment.find<uint8_t>(descriptors[shm.handle].name_);
shm.resize_count = descriptors[shm.handle].resize_count_;
shm.ptr = ret.first;
LOG_DEBUG("reallocateBlock: unlocking global");
}
// Then check if the size need to be expanded
if (size > descriptors[shm.handle].allocated_) {
scoped_lock<interprocess_mutex> lockg(mutex);
LOG_DEBUG("reallocateBlock2: locked global, checking clients");
check_global_clients(lockg);
LOG_INFO("Reallocating block " << descriptors[shm.handle].name_ << ":" << shm.ptr << " from " <<
descriptors[shm.handle].allocated_ << " to " << size << " bytes");
segment.destroy<uint8_t>(descriptors[shm.handle].name_);
try {
uint8_t * ptr = segment.find_or_construct<uint8_t>(descriptors[shm.handle].name_)[size](0);
shm.ptr = ptr;
descriptors[shm.handle].recordSize(size,size);
shm.resize_count = descriptors[shm.handle].resize_count_;
} catch (boost::interprocess::bad_alloc e) {
LOG_ERROR("Failed to reallocate memory block, resetting block");
descriptors[shm.handle].reset();
shm = shm_handle();
}
LOG_DEBUG("reallocateBlock2: unlocking global");
} else {
descriptors[shm.handle].recordSize(size);
}
LOG_DEBUG("reallocateBlock: unlocking " << shm.handle);
}
void fileManager::saveSurveyMap(){
std::lock_guard<std::mutex> lockg(lock);
std::ofstream output(survDir, trunc);
for(std::map<std::string, surveyData*>::iterator i = surveys->begin(); i != surveys->end(); ++i){
output.write(i->second->save().c_str(), i->second->save().size());
output << '\n';
}
output.close();
}
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());
}
}
}
}*/
}
void SharedMemoryBlock::resetBlock(boost::interprocess::managed_shared_memory & segment, shm_handle & shm) {
scoped_lock<interprocess_mutex> lock(descriptors[shm.handle].mutex);
descriptors[shm.handle].check_clients(lock);
scoped_lock<interprocess_mutex> lockg(mutex);
check_global_clients(lockg);
if (shm.is_valid()) {
assert(shm.handle < MSGTSharedMemoryNumBlock);
segment.destroy<uint8_t>(descriptors[shm.handle].name_);
descriptors[shm.handle].reset();
shm = shm_handle();
}
}
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;
}
//Reads data from a file with saved user data, and parses it into the lsitOfUsers map for memory access
//gets an individual line from the file, then parses tokens from that line delimited by \0 to be stored in a vector
//then, adds the user to the map with the username as the first item, password as second.
//All subsequent items are the surveys and choices made by the user, which are cast into pairs
//as such, the loop iterates over every two items, before directly pushing back the created pair into the vector
//as this method is a friend of user's.
void fileManager::populateUserMap(){
std::lock_guard<std::mutex> lockg(lock);
std::ifstream input(usrDir);
std::string line = "";
user* tmp;
std::vector<std::string> args;
size_t start, end;
while(getline(input, line)){
start = 0; end = 0;
while((end = line.find('\0', start)) != std::string::npos){
args.push_back(line.substr(start, end - start));
start = end + 1;
}
tmp = users->addUser(args[0], args[1]);
for(size_t index = 2; index <= args.size() - 2; index += 2)
tmp->votes.push_back(std::make_pair(args[index], args[index + 1]));
args.clear();
}
input.close();
}
void fileManager::populateSurveyMap(){
std::lock_guard<std::mutex> lockg(lock);
std::ifstream input(survDir, std::ios::in);
std::string line = "";
surveyData* tmp;
std::vector<std::string> args;
size_t start, end;
while(getline(input, line)){
start = 0, end = 0;
while((end = line.find('\0', start)) != std::string::npos){
args.push_back(line.substr(start, end - start));
start = end + 1;
}
surveys->addSurvey(args[1], tmp = new surveyData(args[0], args[1], args[2], args[3], args[4]));
tmp->setNumVoted(std::stoi(args[5]));
tmp->setOption1Votes(std::stof(args[6]));
tmp->setOption2Votes(std::stof(args[7]));
std::cout << "size is: " << args.size() << std::endl;
for(size_t index = 8; index <= args.size() - 2; index += 2)
tmp->usersVoted[args[index]] = args[index + 1];
args.clear();
}
input.close();
}
bool GraphSLAM::loadGraph(const char *filename){
boost::mutex::scoped_lock lockg(graphMutex);
return _graph->load(filename);
}
void GraphSLAM::findConstraints(){
boost::mutex::scoped_lock lockg(graphMutex);
//graph is quickly optimized first so last added edge is satisfied
_graph->initializeOptimization();
_graph->optimize(1);
OptimizableGraph::VertexSet vset;
_vf.findVerticesScanMatching( _lastVertex, vset);
checkCovariance(vset);
addNeighboringVertices(vset, 8);
checkHaveLaser(vset);
std::set<OptimizableGraph::VertexSet> setOfVSet;
_vf.findSetsOfVertices(vset, setOfVSet);
OptimizableGraph::EdgeSet loopClosingEdges;
for (std::set<OptimizableGraph::VertexSet>::iterator it = setOfVSet.begin(); it != setOfVSet.end(); it++) {
OptimizableGraph::VertexSet myvset = *it;
OptimizableGraph::Vertex* closestV = _vf.findClosestVertex(myvset, _lastVertex);
if (closestV->id() == _lastVertex->id() - 1) //Already have this edge
continue;
SE2 transf;
if (!isMyVertex(closestV) || (isMyVertex(closestV) && abs(_lastVertex->id() - closestV->id()) > 10)){
/*VertexEllipse* ellipse = findEllipseData(_lastVertex);
if (ellipse){
for (OptimizableGraph::VertexSet::iterator itv = myvset.begin(); itv != myvset.end(); itv++){
VertexSE2 *vertex = (VertexSE2*) *itv;
SE2 relativetransf = _lastVertex->estimate().inverse() * vertex->estimate();
ellipse->addMatchingVertex(relativetransf.translation().x(), relativetransf.translation().y());
ellipse->addMatchingVertexID(vertex->id());
}
}*/
std::vector<SE2> results;
/*OptimizableGraph::VertexSet referenceVset;
referenceVset.insert(_lastVertex);
int j = 1;
int comm_gap = 5;
while (j <= comm_gap){
VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
if (vj)
referenceVset.insert(vj);
else
break;
j++;
}*/
//Loop Closing Edge
bool shouldIAdd = _LCMatcher.scanMatchingLC(myvset, closestV, _lastVertex, results, maxScore);
//bool shouldIAdd = _mf.scanMatchingLC(myvset, closestV, referenceVset, _lastVertex, results, maxScore);
if (shouldIAdd){
for (unsigned int i =0; i< results.size(); i++){
EdgeSE2 *ne = new EdgeSE2;
ne->setId(++_runningEdgeId + _baseId);
ne->vertices()[0] = closestV;
ne->vertices()[1] = _lastVertex;
ne->setMeasurement(results[i]);
ne->setInformation(_SMinf);
loopClosingEdges.insert(ne);
_SMEdges.insert(ne);
}
}else {
std::cout << "Rejecting LC edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl;
}
}else{
//Edge between close vertices
bool shouldIAdd = _closeMatcher.closeScanMatching(myvset, closestV, _lastVertex, &transf, maxScore);
if (shouldIAdd){
EdgeSE2 *ne = new EdgeSE2;
ne->setId(++_runningEdgeId + _baseId);
ne->vertices()[0] = closestV;
ne->vertices()[1] = _lastVertex;
ne->setMeasurement(transf);
ne->setInformation(_SMinf);
_graph->addEdge(ne);
_SMEdges.insert(ne);
}else {
std::cout << "Rejecting edge between " << closestV->id() << " and " << _lastVertex->id() << " [matching fail] " << std::endl;
}
}
}
if (loopClosingEdges.size())
addClosures(loopClosingEdges);
checkClosures();
updateClosures();
}
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;
}
