$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Vit Stancl (stancl@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: dd/mm/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #pragma once 00029 #ifndef OCTOMAP_FILTER_SINGLE_SPECLES_H_ 00030 #define OCTOMAP_FILTER_SINGLE_SPECLES_H_ 00031 00032 #include "octomap_filter_base.h" 00033 00034 namespace srs_env_model 00035 { 00039 class COcFilterSingleSpecles : public COcTreeFilterBase 00040 { 00041 public: 00043 COcFilterSingleSpecles(const std::string & octree_frame_id, ERunMode mode = FILTER_ALLWAYS) : COcTreeFilterBase(octree_frame_id,mode), m_numSpecRemoved(0) {} 00044 00046 virtual void writeLastRunInfo() 00047 { 00048 std::cerr << "COcFilterSingleSpecles: Number of specles removed: " << m_numSpecRemoved << std::endl; 00049 } 00050 00051 protected: 00053 virtual void filterInternal(tButServerOcTree & tree) 00054 { 00055 m_numSpecRemoved = 0; 00056 00057 tButServerOcTree::leaf_iterator it, itEnd(tree.end_leafs()); 00058 for (it = tree.begin_leafs(); it != itEnd; ++it) 00059 { 00060 // Test if node is occupied 00061 if (tree.isNodeOccupied(*it)) 00062 { 00063 octomap::OcTreeKey nKey = it.getKey(); 00064 octomap::OcTreeKey key; 00065 bool neighborFound = false; 00066 00067 // Find neighbours 00068 for (key[2] = nKey[2] - 1; !neighborFound && key[2] <= nKey[2] 00069 + 1; ++key[2]) { 00070 for (key[1] = nKey[1] - 1; !neighborFound && key[1] 00071 <= nKey[1] + 1; ++key[1]) { 00072 for (key[0] = nKey[0] - 1; !neighborFound && key[0] 00073 <= nKey[0] + 1; ++key[0]) { 00074 if (key != nKey) { 00075 tButServerOcTree::NodeType* node = tree.search( 00076 key); 00077 if (node && tree.isNodeOccupied(node)) { 00078 // we have a neighbor => break! 00079 neighborFound = true; 00080 } 00081 } 00082 } 00083 } 00084 } 00085 00086 // done with search, see if found and degrade otherwise: 00087 if (!neighborFound) { 00088 // Remove it... 00089 tree.integrateMissNoTime(&*it); 00090 ++m_numSpecRemoved; 00091 } 00092 00093 } 00094 } 00095 } 00096 00097 protected: 00099 long m_numSpecRemoved; 00100 }; 00101 00102 } // namespace srs_env_model 00103 00104 #endif /* OCTOMAP_FILTER_SINGLE_SPECLES_H_ */