Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include "graph_manager.h"
00017 #include "misc.h"
00018 #include "covariance_estimation.h"
00019 #include "g2o/types/slam3d/edge_se3.h"
00020 #include "scoped_timer.h"
00021
00024
00025 void GraphManager::toggleMapping(bool mappingOn){
00026 QMutexLocker locker(&optimizer_mutex_);
00027 ROS_INFO_COND(mappingOn, "Switching mapping back on");
00028 ROS_INFO_COND(!mappingOn, "Switching mapping off: Localization continues");
00029 localization_only_ = !mappingOn;
00030 ROS_ERROR("Implementation of localization only broken");
00031 #ifdef DO_FEATURE_OPTIMIZATION
00032 optimizer_->setFixed(landmark_vertices, localization_only_);
00033 #endif
00034 optimizer_->setFixed(camera_vertices, localization_only_);
00035 }
00036
00037 bool GraphManager::isBusy(){
00038 return (batch_processing_runs_ || process_node_runs_ );
00039 }
00040
00041 void GraphManager::clearPointClouds() {
00042 ROS_WARN("Clearing PointCloud. They will not be available for save/send, EMM.");
00043 BOOST_REVERSE_FOREACH(GraphNodeType entry, graph_){
00044 entry.second->clearPointCloud();
00045 }
00046 }
00047
00048 void GraphManager::clearPointCloud(pointcloud_type const * pc) {
00049 ROS_DEBUG("Should clear cloud at %p", pc);
00050 BOOST_REVERSE_FOREACH(GraphNodeType entry, graph_){
00051 if(entry.second->pc_col.get() == pc){
00052 entry.second->clearPointCloud();
00053 ROS_WARN("Cleared PointCloud after rendering to openGL list. It will not be available for save/send.");
00054 return;
00055 }
00056 ROS_DEBUG("Compared to node %d (cloud at %p has size %zu)", entry.first, entry.second->pc_col.get(), entry.second->pc_col->points.size());
00057 }
00058 ROS_WARN("Should Clear cloud at %p, but didn't find it in any node.", pc);
00059 }
00060
00061 void GraphManager::deleteLastFrame(){
00062 if(graph_.size() <= 1) {
00063 ROS_INFO("Resetting, as the only node is to be deleted");
00064 reset_request_ = true;
00065 Q_EMIT deleteLastNode();
00066 return;
00067 }
00068
00069 deleteCameraFrame(graph_.size()-1);
00070
00071 Q_EMIT deleteLastNode();
00072 optimizeGraph();
00073 ROS_INFO("Removed most recent node");
00074 Q_EMIT setGUIInfo("Removed most recent node");
00075
00076
00077
00078 }
00079
00080 void GraphManager::reset(){
00081 reset_request_ = true;
00082 }
00083
00084 GraphManager::~GraphManager() {
00085
00086
00087
00088 BOOST_FOREACH(GraphNodeType entry, graph_) {
00089 delete entry.second;
00090 }
00091 graph_.clear();
00092 QMutexLocker locker(&optimizer_mutex_);
00093 QMutexLocker locker2(&optimization_mutex_);
00094
00095 ransac_marker_pub_.shutdown();
00096 whole_cloud_pub_.shutdown();
00097 marker_pub_.shutdown();
00098 batch_cloud_pub_.shutdown();
00099
00100 }
00101
00102
00103 void GraphManager::deleteFeatureInformation() {
00104 ROS_WARN("Clearing out Feature information from nodes");
00105
00106 BOOST_FOREACH(GraphNodeType entry, graph_){
00107 entry.second->clearFeatureInformation();
00108 }
00109 }
00110
00111 void GraphManager::setEmpiricalCovariancesForEdgeSet(EdgeSet& edges){
00112 ScopedTimer s(__FUNCTION__);
00113
00114 typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6Xd;
00115 typedef g2o::BaseEdge<6, Eigen::Isometry3d> BaseEdgeSE3;
00116 typedef g2o::EdgeSE3::InformationType InfoMat;
00117
00118 Matrix6Xd measurements, errors;
00119 edgesToMeasurementMatrix<BaseEdgeSE3>(edges, measurements);
00120 edgesToErrorMatrix<BaseEdgeSE3>(edges, errors);
00121
00122
00123 Vector6d meanMsr = measurements.rowwise().mean();
00124 Vector6d stdDev = (measurements.colwise() - meanMsr).cwiseAbs().rowwise().mean();
00125
00126 EdgeSet::iterator it = edges.begin();
00127 for(int i = 0; it != edges.end(); ++it, ++i)
00128 {
00129 BaseEdgeSE3* edge = dynamic_cast<BaseEdgeSE3*>( *it );
00130 if(edge){
00131 InfoMat infoMat = computeEmpiricalInformationMatrix(measurements, errors, measurements.col(i), stdDev);
00132 edge->setInformation(infoMat);
00133 }
00134 }
00135
00136 }
00137
00138 void GraphManager::setEmpiricalCovariances(){
00139 optimizer_->initializeOptimization();
00140 optimizer_->computeActiveErrors();
00141
00142 setEmpiricalCovariancesForEdgeSet(cam_cam_edges_);
00143
00144 }
00145