void generateGraphFile(char* filename, std::vector<RobotPosition *> * poses, std::vector<RobotPosition*>* transformations, std::list<Landmark *> * landmarks){
std::string graphname = basename(filename);
graphname = graphname.substr(0,graphname.length()-4);
std::ofstream g2oout((graphname+(std::string("_graph.g2o"))).c_str());
// poses
for(unsigned int i=0; i<poses->size(); i++){
RobotPosition * pose = (*poses)[i];
RobotPosition * transf = (*transformations)[i];
RobotPosition * prev_pose;
// unsigned int prev_id;
g2oout << "VERTEX_SE2 " << pose->id() << " " << pose->x() << " " << pose->y() << " " << pose->theta() << std::endl;
if(i>0){
prev_pose = (*poses)[i-1];
g2oout << "EDGE_SE2 " << prev_pose->id() << " " << pose->id() << " " << transf->x() << " " << transf->y() << " " << transf->theta() << " 500 0 0 500 0 100" << std::endl;
}
}
// landmarks
std::list<Landmark *>::iterator iter;
for(iter=landmarks->begin(); iter!=landmarks->end(); iter++){
Landmark * lmark = *iter;
if(!lmark->isConfirmed()){
continue;
}
if(!lmark->hasId()){
lmark->setId(next_id++);
lmark->idAssigned();
}
// add the landmark
g2oout << "VERTEX_XY " << lmark->id() << " " << lmark->x() << " " << lmark->y() << std::endl;
// add the associated bearing constraints
for(unsigned int o=0; o<lmark->getObservations()->size(); o++){
Observation * observation= (*(lmark->getObservations()))[o];
RobotPosition * pose = observation->pose;
g2oout << "EDGE_BEARING_SE2_XY " << pose->id() << " " << lmark->id() << " " << observation->bearing << " 200" << std::endl;
}
}
}
void populateGraph(std::vector<RobotPosition *> * poses, std::vector<RobotPosition*> * transformations, std::list<Landmark *> * landmarks){
std::cout << "clearing optimizer..." << std::endl;
// optimizer->clear();
// put poses in the graph
for(unsigned int i=0; i<poses->size(); i++){
if((*poses)[i]->already_in_graph){
continue;
}
// add this pose to the graph
optimizer->addVertex((*poses)[i]);
((*poses)[i])->already_in_graph = true;
if(i>0){
// add the constraint regarding the rototranslation performed from the previous step
g2o::EdgeSE2 * odom_edge = new g2o::EdgeSE2;
odom_edge->vertices()[0] = (*poses)[i-1];
odom_edge->vertices()[1] = (*poses)[i];
g2o::SE2 * measurement = new g2o::SE2((*transformations)[i]->x(), (*transformations)[i]->y(), (*transformations)[i]->theta());
odom_edge->setMeasurement(*measurement);
odom_edge->setInformation(*odom_info);
optimizer->addEdge(odom_edge);
}
}
// put landmarks in the graph
std::list<Landmark *>::iterator iter;
for(iter = landmarks->begin(); iter!=landmarks->end(); iter++){
Landmark * lm = *iter;
if(!lm->isConfirmed()){
continue;
}
if(lm->already_in_graph){
continue;
}
lm->already_in_graph = true;
if(!lm->hasId()){
lm->setId(next_id++);
lm->idAssigned();
}
optimizer->addVertex(lm);
for(unsigned int o=0; o<lm->getObservations()->size(); o++){
Observation * observation= (*(lm->getObservations()))[o];
RobotPosition * pose = observation->pose;
g2o::EdgeSE2PointXYBearing* obs_edge = new g2o::EdgeSE2PointXYBearing;
obs_edge->vertices()[0] = pose;
obs_edge->vertices()[1] = lm;
obs_edge->setMeasurementData(&(observation->bearing));
obs_edge->setInformation(*obs_info);
robust_kernel = new g2o::RobustKernelCauchy();
obs_edge->setRobustKernel(robust_kernel);
optimizer->addEdge(obs_edge);
}
}
optimizer->vertex(0)->setFixed(true);
optimizer->initializeOptimization();
}
void tryToUnderstand1(RobotPosition * pose, std::list<Landmark *>* landmarks,std::vector<Landmark *> * last_observations, std::vector<Landmark *> * propagate_observations){
double rpose[3];
pose->getEstimateData(rpose);
propagate_observations->clear();
std::cout << "Robot Position:\t" << rpose[0] << "\t" << rpose[1] << "\t" << rpose[2] << "\n";
double expected[last_observations->size()]; // this will contain the projections of the previously seen landmarks in the current pose
// I.E. this tells where we expect to see the landmarks propagated from the last step
bool use_general_angle_tolerance[last_observations->size()]; // this will tell if the angle tolerance to be used is the general one or the bearing-only based one
int associations[pose->observations.size()]; // this will contain the candidate associations, that will become effective ONLY IF there will be no ambiguity
for(unsigned int i=0; i<pose->observations.size(); i++){
associations[i] = -1;
}
std::cout << "We expect to see stuff at:\n";
for(unsigned int i = 0; i<last_observations->size(); i++){ // populate the expected array
Landmark * lm = (*last_observations)[i];
if(lm->getObservations()->size() > 1){ // first case: landmark already has an estimated position
expected[i] = computeAngle(lm, pose);
use_general_angle_tolerance[i] = true;
}
else{ // second case: landmark has been seen only once up to now, hence there is no position estimation
double prev_seen = (*(lm->getObservations()))[0]->bearing;
double prev_theta = (*(lm->getObservations()))[0]->pose->theta();
double rotated = computeAnglesDifference(pose->theta(), prev_theta);
expected[i] = prev_seen-rotated;
use_general_angle_tolerance[i] = false;
}
std::cout << expected[i] << "\n";
}
std::cout << "observations:\n";
for(unsigned int obs_counter=0; obs_counter < pose->observations.size(); obs_counter++){ // for every observation in the current position
std::cout << (pose->observations)[obs_counter].bearing << ": ";
// Try to assign it to a previously generated landmark
if(last_observations->size() > 0){
unsigned int closer_index;
unsigned int second_closer_index;
// compute closer_index and second_closer_index.
double distances[last_observations->size()];
for(unsigned int i=0; i < last_observations->size(); i++){
distances[i] = d_abs(computeAnglesDifference(expected[i], pose->observations[obs_counter].bearing));
} // now 'distances' contains the distance between the currently considered observation and the expected values
closer_index = 0;
if(last_observations->size() == 1){
second_closer_index = 0;
}
else{
second_closer_index = 1;
if(distances[1] < distances[0]){
closer_index = 1;
second_closer_index = 0;
}
}
for (unsigned int i = 1; i < last_observations->size(); i++){
if (distances[i] < distances[closer_index]){
second_closer_index = closer_index;
closer_index = i;
}
else{
if(distances[i] < distances[second_closer_index]){
second_closer_index = i;
}
}
}
// if closer is "close enough" and second_closer is "far enough" (I.E. there is no ambiguity)
// associate the current observation to 'closer'.
double tolerance = GENERAL_ANGLE_TOLERANCE;
if(!use_general_angle_tolerance[closer_index]){
tolerance = BEAR_ONLY_ANGLE_TOLERANCE;
}
std::cout << "tolerance is " << tolerance << "\n";
if(distances[closer_index] < tolerance){
if((second_closer_index == closer_index) || (distances[second_closer_index] > SECOND_ANGLE_MIN_DISTANCE)){
associations[obs_counter] = closer_index;
std::cout << "close to " << expected[closer_index] << "\n";
}
else{
std::cout << "close to " << expected[closer_index] << " but also to " << expected[second_closer_index] << "\n";
}
}
else{
std:: cout << "is far from everything\n";
}
// NOTE: Do not add the observation to the set inside the landmark corresponding to
// 'closer' yet, because, if two or more NEW observations are associated to the same
// landmark, none of them will be added.
}
}
// add the computed observations to the landmarks, but only if there is no ambiguity
bool associated[last_observations->size()]; // associated[i] will be true if there is at least 1 new observation associated to the i-th landmark in last_observations
bool ambiguous[last_observations->size()]; // ambiguous[i] will be true if there are at least 2 new observations associated to the i-th landmark in last_observations
// initialize the arrays just created
for(unsigned int i=0; i<last_observations->size(); i++){
associated[i] = false;
ambiguous[i] = false;
}
// tell which ones are ambiguous
for(unsigned int i=0; i < pose->observations.size(); i++){
unsigned int value = associations[i];
if(value!=-1){
if (!associated[value]){
associated[value] = true;
}
else{ // associated is true, hence there is already a new observation (or more) polling for the landmark
ambiguous[value] = true;
}
}
}
// add non-ambiguous observations to the corresponding landmarks
// and create new landmarks for the new observed unassociated observations
std::cout << "adding observations to landmarks\n";
for(unsigned int i=0; i < pose->observations.size(); i++){
std::cout << "obs " << i << " ";
if(associations[i] != -1){
if(!ambiguous[associations[i]]){
std::cout << "tailed to " << associations[i] << "\n";
Landmark * lm = (*last_observations)[associations[i]];
lm->addObservation(&(pose->observations[i]));
lm->estimatePosition();
lm->checkConfirmed(_confirm_obs);
double newpos[2];
(*last_observations)[associations[i]]->getPosition(newpos);
std::cout << "\tnew position estimated:\t" << newpos[0] << "\t" << newpos[1] << "\n";
propagate_observations->push_back((*last_observations)[associations[i]]);
}
else{
std::cout << "should be tailed to " << associations[i] << ", but there is ambiguity\n";
}
}
else{ // unassociated
std::cout << "is a new landmark\n";
Landmark * lm = new Landmark();
lm->addObservation (&(pose->observations[i]));
// lm->estimatePosition();
landmarks->push_back(lm);
propagate_observations->push_back(lm);
}
}
std::cout << "now checking landmarks from previous step\n";
for(unsigned int i = 0; i<last_observations->size(); i++){
std::cout << "prev_obs " << i << " ";
Landmark * lm = (*last_observations)[i];
if(!associated[i]){
// for the landmarks expected to be seen that are not in the current set of observations, we have two cases:
if(lm->isConfirmed()){
// case one: the landmark has been confirmed observations, then it is reliable
std::cout << "was old enough, CONFIRMED\n";
}
else{
// case two: the landmark is pretty unreliable, then it is trashed
std::cout << "was too young, REMOVED\n";
landmarks->remove(lm);
delete lm;
}
}
else{
if(ambiguous[i]){
std::cout << "was ambiguous,";
if(lm->isConfirmed()){
std::cout << " but it was considered reliable, PROPAGATED" << std::endl;
propagate_observations->push_back(lm);
}
else{
std::cout << " and it was pretty young, REMOVED" << std::endl;
landmarks->remove(lm);
delete lm;
}
}
else{
std::cout << "has been REINFORCED\n";
}
}
}
std::cout << "\n\n";
}