00001 /****************************************************************************** 00002 * * 00003 * pcdfilter_pa_filter.cpp * 00004 * ======================= * 00005 * * 00006 ******************************************************************************* 00007 * * 00008 * github repository * 00009 * https://github.com/TUC-ProAut/ros_pcdfilter * 00010 * * 00011 * Chair of Automation Technology, Technische Universität Chemnitz * 00012 * https://www.tu-chemnitz.de/etit/proaut * 00013 * * 00014 ******************************************************************************* 00015 * * 00016 * New BSD License * 00017 * * 00018 * Copyright (c) 2015-2018, Peter Weissig, Technische Universität Chemnitz * 00019 * All rights reserved. * 00020 * * 00021 * Redistribution and use in source and binary forms, with or without * 00022 * modification, are permitted provided that the following conditions are met: * 00023 * * Redistributions of source code must retain the above copyright * 00024 * notice, this list of conditions and the following disclaimer. * 00025 * * Redistributions in binary form must reproduce the above copyright * 00026 * notice, this list of conditions and the following disclaimer in the * 00027 * documentation and/or other materials provided with the distribution. * 00028 * * Neither the name of the Technische Universität Chemnitz nor the * 00029 * names of its contributors may be used to endorse or promote products * 00030 * derived from this software without specific prior written permission. * 00031 * * 00032 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * 00033 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * 00034 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * 00035 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY * 00036 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * 00037 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR * 00038 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * 00039 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * 00040 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * 00041 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 00042 * DAMAGE. * 00043 * * 00044 ******************************************************************************/ 00045 00046 // local headers 00047 #include "pcdfilter_pa/pcdfilter_pa_filter.h" 00048 00049 // standard headers 00050 #include <sstream> 00051 00052 //**************************[cPcdFilterPaFilter]******************************* 00053 cPcdFilterPaFilter::cPcdFilterPaFilter(void) { 00054 00055 type_ = ftNONE; 00056 inverse_ = false ; 00057 00058 parameter_[0] = 0; 00059 parameter_[1] = 0; 00060 parameter_[2] = 0; 00061 00062 rotation_ = cv::Matx33f::eye(); 00063 //translation_ = cv::Vec3f(); 00064 } 00065 00066 //**************************[cPcdFilterPaFilter]******************************* 00067 cPcdFilterPaFilter::cPcdFilterPaFilter( 00068 const cPcdFilterPaFilter &other) { 00069 00070 *this = other; 00071 } 00072 00073 //**************************[operator = ]************************************** 00074 cPcdFilterPaFilter& cPcdFilterPaFilter::operator = ( 00075 const cPcdFilterPaFilter &other) { 00076 00077 type_ = other.type_ ; 00078 inverse_ = other.inverse_ ; 00079 00080 parameter_[0] = other.parameter_[0]; 00081 parameter_[1] = other.parameter_[1]; 00082 parameter_[2] = other.parameter_[2]; 00083 00084 rotation_ = other.rotation_ ; 00085 translation_ = other.translation_ ; 00086 00087 return *this; 00088 } 00089 00090 //**************************[toString]***************************************** 00091 std::string cPcdFilterPaFilter::toString(void) const { 00092 std::stringstream result; 00093 00094 if (inverse_) { 00095 result << '!'; 00096 } 00097 00098 int count_para = 0; 00099 switch (type_) { 00100 case ftNONE : result << "none" ; break; 00101 case ftCUBE : result << "cube" ; count_para = 1; break; 00102 case ftSPHERE : result << "sphere" ; count_para = 1; break; 00103 case ftBLOCK : result << "block" ; count_para = 3; break; 00104 case ftCYLINDER: result << "cylinder"; count_para = 2; break; 00105 case ftCONE : result << "cone" ; count_para = 2; break; 00106 default : result << "error" ; break; 00107 } 00108 result << ":"; 00109 00110 for (int i = 0; i < count_para; i++) { 00111 result << ' ' << parameter_[i]; 00112 } 00113 00114 result << "; rot=[["; 00115 for (int y = 0; y < 3; y++) { 00116 if (y > 0) { result << "], [";} 00117 for (int x = 0; x < 3; x++) { 00118 if (x > 0) { result << ", ";} 00119 result << rotation_(y,x); 00120 } 00121 } 00122 result << "]]"; 00123 00124 result << "; trans=["; 00125 for (int x = 0; x < 3; x++) { 00126 if (x > 0) { result << ", ";} 00127 result << translation_(x); 00128 } 00129 result << "]"; 00130 00131 return result.str(); 00132 }