Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "graph_manager.h"
00010
00011 using namespace std;
00012
00013
00014 #ifdef DO_LOOP_CLOSING
00015
00016 void GraphManager::createSearchTree(){
00017
00018 vector<int> all_node_ids;
00019 for (map<int,Node*>::iterator it = graph_.begin(); it != graph_.end(); ++it){
00020 all_node_ids.push_back(it->first);
00021 }
00022
00023 createSearchTree(all_node_ids);
00024 }
00025
00026
00027 int findClosestNeighbour(cv::Mat all, cv::Mat row){
00028
00029 int best = -1;
00030 float dist = 1e10;
00031
00032 for (int i=0; i<all.rows; ++i){
00033 float sum = 0;
00034
00035
00036 for (int j=0; j<row.cols; ++j){
00037 sum += abs(all.at<float>(i,0)-row.at<float>(0,0));
00038 }
00039 if (sum < dist){
00040 dist = sum;
00041 best = i;
00042 }
00043 }
00044
00045 return best;
00046
00047 }
00048
00049
00050 void GraphManager::createSearchTree(const std::vector<int>& node_ids){
00051
00052
00053 descriptor_length = graph_.begin()->second->feature_descriptors_.cols;
00054
00055 total_descriptor_count = 0;
00056
00057
00058 for (uint i=0; i<node_ids.size(); ++i)
00059 total_descriptor_count += graph_.at(node_ids[i])->feature_descriptors_.rows;
00060
00061
00062 descriptor_to_node = new int[total_descriptor_count];
00063
00064
00065 all_descriptors = cv::Mat(total_descriptor_count,descriptor_length, CV_32FC1);
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078 ROS_INFO("Building a tree for %i descriptors of length %i", total_descriptor_count, descriptor_length);
00079
00080 uint pos=0;
00081 for (uint i=0; i<node_ids.size(); ++i){
00082
00083 Node* node = graph_.at(node_ids[i]);
00084 int desc_cnt = node->feature_descriptors_.rows;
00085
00086
00087
00088 for (int l = 0; l<desc_cnt; ++l){
00089 for (uint d = 0; d<descriptor_length; d++){
00090 all_descriptors.at<float>(pos+l,d) = node->feature_descriptors_.at<float>(l,d);
00091 descriptor_to_node[pos+l]= node->id_;
00092 }
00093 }
00094
00095 pos += desc_cnt;
00096 }
00097
00098
00099
00100
00101
00102
00103 tree = new cv::flann::Index(all_descriptors, cv::flann::KDTreeIndexParams(16));
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 ROS_INFO("Tree was build");
00117
00118 }
00119
00120 bool higherScore(const pair<int,float>& A,const pair<int,float>& B){
00121 return A.second > B.second;
00122 }
00123
00124
00125 void GraphManager::loopClosingTest(){
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 createSearchTree();
00147
00148
00149 ROS_INFO("Test: best neighbour is image itself");
00150
00151
00152
00153 for (std::map<int, Node* >::iterator it = graph_.begin(); it!=graph_.end(); ++it){
00154 std::vector<std::pair<int,float> > neighbours;
00155 getNeighbours(it->first, 1, neighbours);
00156
00157 assert(neighbours[0].first == it->first);
00158
00159
00160 }
00161 ROS_INFO("Test: k=1: PASSED");
00162
00163
00164 for (std::map<int, Node* >::iterator it = graph_.begin(); it!=graph_.end(); ++it){
00165 std::vector<std::pair<int,float> > neighbours;
00166 getNeighbours(it->first, 2, neighbours);
00167
00168 assert(neighbours[0].first == it->first);
00169
00170
00171 }
00172 ROS_INFO("Test: k=2: PASSED");
00173
00174
00175 for (std::map<int, Node* >::iterator it = graph_.begin(); it!=graph_.end(); ++it){
00176 std::vector<std::pair<int,float> > neighbours;
00177 getNeighbours(it->first, graph_.size(), neighbours);
00178
00179 assert(neighbours[0].first == it->first);
00180
00181
00182 }
00183 ROS_INFO("Test: k=graph_.size(): PASSED");
00184
00185
00186
00187 }
00188
00189
00190 void GraphManager::getNeighbours(int node_id, uint neighbour_cnt, std::vector<std::pair<int,float> >& neighbours){
00191
00192
00193
00194 assert(tree);
00195 assert(descriptor_to_node);
00196
00197 neighbours.clear();
00198
00199 Node *node = graph_.at(node_id);
00200
00201
00202
00203
00204
00205
00206 cv::Mat indices(node->feature_descriptors_.rows, neighbour_cnt, CV_32S);
00207 cv::Mat dists(node->feature_descriptors_.rows, neighbour_cnt, CV_32FC1);
00208
00209 std::vector<int> index;
00210 std::vector<float> dist;
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223 tree->knnSearch(node->feature_descriptors_, indices, dists, neighbour_cnt);
00224
00225 map<int, float> scores;
00226
00227 for (int desc=0; desc<indices.rows; ++desc){
00228 for (int neighbour = 0; neighbour<indices.cols; neighbour++){
00229
00230 int desc_id = indices.at<int>(desc, neighbour);
00231 int best_node_id = descriptor_to_node[desc_id];
00232
00233
00234
00235
00236
00237
00238 float value = neighbour_cnt-neighbour;
00239
00240 if (scores.find(best_node_id) == scores.end()){
00241 scores[best_node_id] = value;
00242 }else{
00243 scores[best_node_id] += value;
00244 }
00245
00246 }
00247
00248
00249
00250 }
00251
00252
00253
00254
00255 std::vector<pair<int,float> > all_neighbours;
00256 float score_sum = 0;
00257
00258 for (map<int, float>::iterator it = scores.begin(); it != scores.end(); ++it){
00259
00260
00261
00262
00263 all_neighbours.push_back(make_pair(it->first, it->second/graph_.at(it->first)->feature_descriptors_.rows));
00264
00265
00266 score_sum+=it->second;
00267 }
00268
00269 sort(all_neighbours.begin(), all_neighbours.end(),higherScore);
00270
00271 for (uint i=0; i< uint(all_neighbours.size()) && i<neighbour_cnt; ++i){
00272
00273 neighbours.push_back(all_neighbours[i]);
00274 }
00275
00276
00277
00278 }
00279
00280
00281 #endif
00282