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