ground_truth.h
Go to the documentation of this file.
00001 /***********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
00005  * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
00006  *
00007  * THE BSD LICENSE
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions
00011  * are met:
00012  *
00013  * 1. Redistributions of source code must retain the above copyright
00014  *    notice, this list of conditions and the following disclaimer.
00015  * 2. Redistributions in binary form must reproduce the above copyright
00016  *    notice, this list of conditions and the following disclaimer in the
00017  *    documentation and/or other materials provided with the distribution.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
00020  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
00021  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
00022  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
00023  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
00024  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00025  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00026  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
00028  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *************************************************************************/
00030 
00031 #ifndef RTABMAP_FLANN_GROUND_TRUTH_H_
00032 #define RTABMAP_FLANN_GROUND_TRUTH_H_
00033 
00034 #include "rtflann/algorithms/dist.h"
00035 #include "rtflann/util/matrix.h"
00036 
00037 
00038 namespace rtflann
00039 {
00040 
00041 template <typename Distance>
00042 void find_nearest(const Matrix<typename Distance::ElementType>& dataset, typename Distance::ElementType* query, size_t* matches, size_t nn,
00043                   size_t skip = 0, Distance distance = Distance())
00044 {
00045     //typedef typename Distance::ElementType ElementType;
00046     typedef typename Distance::ResultType DistanceType;
00047     int n = nn + skip;
00048 
00049     int* match = new int[n];
00050     DistanceType* dists = new DistanceType[n];
00051 
00052     dists[0] = distance(dataset[0], query, dataset.cols);
00053     match[0] = 0;
00054     int dcnt = 1;
00055 
00056     for (size_t i=1; i<dataset.rows; ++i) {
00057         DistanceType tmp = distance(dataset[i], query, dataset.cols);
00058 
00059         if (dcnt<n) {
00060             match[dcnt] = i;
00061             dists[dcnt++] = tmp;
00062         }
00063         else if (tmp < dists[dcnt-1]) {
00064             dists[dcnt-1] = tmp;
00065             match[dcnt-1] = i;
00066         }
00067 
00068         int j = dcnt-1;
00069         // bubble up
00070         while (j>=1 && dists[j]<dists[j-1]) {
00071             std::swap(dists[j],dists[j-1]);
00072             std::swap(match[j],match[j-1]);
00073             j--;
00074         }
00075     }
00076 
00077     for (size_t i=0; i<nn; ++i) {
00078         matches[i] = match[i+skip];
00079     }
00080 
00081     delete[] match;
00082     delete[] dists;
00083 }
00084 
00085 
00086 template <typename Distance>
00087 void compute_ground_truth(const Matrix<typename Distance::ElementType>& dataset, const Matrix<typename Distance::ElementType>& testset, Matrix<size_t>& matches,
00088                           int skip=0, Distance d = Distance())
00089 {
00090     for (size_t i=0; i<testset.rows; ++i) {
00091         find_nearest<Distance>(dataset, testset[i], matches[i], matches.cols, skip, d);
00092     }
00093 }
00094 
00095 
00096 }
00097 
00098 #endif //FLANN_GROUND_TRUTH_H_


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:16