///Connect Signals and Slots for the ui control
void ui_connections(QObject* ui, GraphManager* graph_mgr, OpenNIListener* listener)
{
Qt::ConnectionType ctype = Qt::AutoConnection;
if (ParameterServer::instance()->get<bool>("concurrent_io"))
ctype = Qt::DirectConnection;
QObject::connect(ui, SIGNAL(reset()), graph_mgr, SLOT(reset()), ctype);
QObject::connect(ui, SIGNAL(optimizeGraph()), graph_mgr, SLOT(optimizeGraph()), ctype);
QObject::connect(ui, SIGNAL(togglePause()), listener, SLOT(togglePause()), ctype);
QObject::connect(ui, SIGNAL(toggleBagRecording()), listener, SLOT(toggleBagRecording()), ctype);
QObject::connect(ui, SIGNAL(getOneFrame()), listener, SLOT(getOneFrame()), ctype);
QObject::connect(ui, SIGNAL(deleteLastFrame()), graph_mgr, SLOT(deleteLastFrame()), ctype);
QObject::connect(ui, SIGNAL(sendAllClouds()), graph_mgr, SLOT(sendAllClouds()), ctype);
QObject::connect(ui, SIGNAL(saveAllClouds(QString)), graph_mgr, SLOT(saveAllClouds(QString)), ctype);
QObject::connect(ui, SIGNAL(saveOctomapSig(QString)), graph_mgr, SLOT(saveOctomap(QString)), ctype);
QObject::connect(ui, SIGNAL(saveAllFeatures(QString)), graph_mgr, SLOT(saveAllFeatures(QString)), ctype);
QObject::connect(ui, SIGNAL(saveIndividualClouds(QString)), graph_mgr, SLOT(saveIndividualClouds(QString)), ctype);
QObject::connect(ui, SIGNAL(saveTrajectory(QString)), graph_mgr, SLOT(saveTrajectory(QString)), ctype);
QObject::connect(ui, SIGNAL(toggleMapping(bool)), graph_mgr, SLOT(toggleMapping(bool)), ctype);
QObject::connect(ui, SIGNAL(saveG2OGraph(QString)), graph_mgr, SLOT(saveG2OGraph(QString)), ctype);
}
void GraphManager::deleteLastFrame(){
if(graph_.size() <= 1) {
ROS_INFO("Resetting, as the only node is to be deleted");
reset_request_ = true;
Q_EMIT deleteLastNode();
return;
}
deleteCameraFrame(graph_.size()-1);
Q_EMIT deleteLastNode();
optimizeGraph();//s.t. the effect of the removed edge transforms are removed to
ROS_INFO("Removed most recent node");
Q_EMIT setGUIInfo("Removed most recent node");
//Q_EMIT setGraphEdges(getGraphEdges());
//updateTransforms needs to be last, as it triggers a redraw
//Q_EMIT updateTransforms(getAllPosesAsMatrixList());
}
bool RosUi::services(rgbd_slam::rgbdslam_ros_ui::Request &req,
rgbd_slam::rgbdslam_ros_ui::Response &res )
{
ROS_INFO_STREAM("Got Service Request. Command: " << req.command);
if (req.command == "reset" ){ Q_EMIT reset(); }
else if(req.command == "quick_save" ){ Q_EMIT saveAllClouds(filename); }
else if(req.command == "save_g2o_graph" ){ Q_EMIT saveG2OGraph("graph.g2o"); }
else if(req.command == "save_trajectory"){ Q_EMIT saveTrajectory("trajectory"); }
else if(req.command == "send_all" ){ Q_EMIT sendAllClouds();}
else if(req.command == "frame" ){ Q_EMIT getOneFrame(); }
else if(req.command == "delete_frame" ){ Q_EMIT deleteLastFrame(); }
else if(req.command == "optimize" ){ Q_EMIT optimizeGraph(); }
else if(req.command == "reload_config" ){ ParameterServer::instance()->getValues();}
else{
ROS_ERROR("Invalid service call command: %s", req.command.c_str());
ROS_ERROR("RGBDSLAM's services have changed in Feb '13, please revise your service calls");
ROS_INFO("Valid commands are: {\n - reset\n - frame\n - quick_save\n - send_all\n - delete_frame\n - optimize\n - reload_config\n - save_trajectory\n}");
return false;
}
return true;
}
// returns true, iff node could be added to the cloud
bool GraphManager::addNode(Node new_node) {
std::clock_t starttime=std::clock();
if(reset_request_) {
int numLevels = 3;
int nodeDistance = 2;
marker_id =0;
time_of_last_transform_= ros::Time();
last_batch_update_=std::clock();
delete optimizer_;
optimizer_ = new AIS::HCholOptimizer3D(numLevels, nodeDistance);
graph_.clear();
matches_.clear();
freshlyOptimized_= false;
reset_request_ = false;
}
if (new_node.feature_locations_2d_.size() <= 5) {
ROS_DEBUG("found only %i features on image, node is not included",(int)new_node.feature_locations_2d_.size());
return false;
}
//set the node id only if the node is actually added to the graph
//needs to be done here as the graph size can change inside this function
new_node.id_ = graph_.size();
//First Node, so only build its index, insert into storage and add a
//vertex at the origin, of which the position is very certain
if (graph_.size()==0) {
new_node.buildFlannIndex(); // create index so that next nodes can use it
// graph_.insert(make_pair(new_node.id_, new_node)); //store
graph_[new_node.id_] = new_node;
optimizer_->addVertex(0, Transformation3(), 1e9*Matrix6::eye(1.0)); //fix at origin
return true;
}
unsigned int num_edges_before = optimizer_->edges().size();
std::vector<cv::DMatch> initial_matches;
ROS_DEBUG("Graphsize: %d", (int) graph_.size());
marker_id = 0; //overdraw old markers
Eigen::Matrix4f ransac_trafo, final_trafo;
bool edge_added_to_base;
std::vector<int> vertices_to_comp = getPotentialEdgeTargets(new_node, -1); //vernetzungsgrad
int id_of_id = vertices_to_comp.size() -1;
for (; id_of_id >=0; id_of_id--) { //go from the back, so the last comparison is with the first node. The last comparison is visualized.
initial_matches = processNodePair(new_node, graph_[vertices_to_comp[id_of_id]],edge_added_to_base,ransac_trafo, final_trafo);
//initial_matches = processNodePair(new_node, graph_rit->second);
//What if the node has not been added? visualizeFeatureFlow3D(graph_rit->second, new_node, initial_matches, matches_, marker_id++);
}
id_of_id++;//set back to last valid id
if (optimizer_->edges().size() > num_edges_before) {
new_node.buildFlannIndex();
graph_[new_node.id_] = new_node;
ROS_DEBUG("Added Node, new Graphsize: %i", (int) graph_.size());
optimizeGraph();
//make the transform of the last node known
ros::TimerEvent unused;
broadcastTransform(unused);
visualizeGraphEdges();
visualizeGraphNodes();
visualizeFeatureFlow3D(graph_[vertices_to_comp[id_of_id]], new_node, initial_matches, matches_, marker_id++);
} else {
ROS_WARN("##### could not find link for PointCloud!");
matches_.clear();
}
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
return (optimizer_->edges().size() > num_edges_before);
}
void Graphical_UI::optimizeGraphTrig() {
Q_EMIT optimizeGraph();
QString message = tr("Triggering Optimizer");
statusBar()->showMessage(message);
}