00001 /* 00002 * Copyright (c) 2013, Willow Garage, Inc. 00003 * Copyright (c) 2016, Orbbec Ltd. 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions are met: 00008 * 00009 * * Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * * Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * * Neither the name of the Willow Garage, Inc. nor the names of its 00015 * contributors may be used to endorse or promote products derived from 00016 * this software without specific prior written permission. 00017 * 00018 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00019 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00020 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00021 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00022 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00023 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00024 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00025 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00026 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00027 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00028 * POSSIBILITY OF SUCH DAMAGE. 00029 * 00030 * Author: Tim Liu (liuhua@orbbec.com) 00031 */ 00032 00033 #include "astra_camera/astra_timer_filter.h" 00034 #include <algorithm> 00035 00036 00037 namespace astra_wrapper 00038 { 00039 00040 AstraTimerFilter::AstraTimerFilter(std::size_t filter_len): 00041 filter_len_(filter_len) 00042 { 00043 } 00044 00045 AstraTimerFilter::~AstraTimerFilter() 00046 { 00047 } 00048 00049 void AstraTimerFilter::addSample(double sample) 00050 { 00051 buffer_.push_back(sample); 00052 if (buffer_.size()>filter_len_) 00053 buffer_.pop_front(); 00054 } 00055 00056 double AstraTimerFilter::getMedian() 00057 { 00058 if (buffer_.size()>0) 00059 { 00060 std::deque<double> sort_buffer = buffer_; 00061 00062 std::sort(sort_buffer.begin(), sort_buffer.end()); 00063 00064 return sort_buffer[sort_buffer.size()/2]; 00065 } else 00066 return 0.0; 00067 } 00068 00069 double AstraTimerFilter::getMovingAvg() 00070 { 00071 if (buffer_.size() > 0) 00072 { 00073 double sum = 0; 00074 00075 std::deque<double>::const_iterator it = buffer_.begin(); 00076 std::deque<double>::const_iterator it_end = buffer_.end(); 00077 00078 while (it != it_end) 00079 { 00080 sum += *(it++); 00081 } 00082 00083 return sum / static_cast<double>(buffer_.size()); 00084 } else 00085 return 0.0; 00086 } 00087 00088 00089 void AstraTimerFilter::clear() 00090 { 00091 buffer_.clear(); 00092 } 00093 00094 00095 } //namespace openni2_wrapper