Apollo 10.0
自动驾驶开放平台
moving_detection.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2020 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
18
19#include <fftw3.h>
20
21namespace apollo {
22namespace audio {
23
25 const std::vector<std::vector<double>>& signals) {
26 int approaching_count = 0;
27 int departing_count = 0;
28 for (std::size_t i = 0; i < signals.size(); ++i) {
29 while (signal_stats_.size() <= i) {
30 signal_stats_.push_back({});
31 }
32 MovingResult result = DetectSingleChannel(i, signals[i]);
33 if (result == APPROACHING) {
34 ++approaching_count;
35 } else if (result == DEPARTING) {
36 ++departing_count;
37 }
38 }
39 if (approaching_count > departing_count) {
40 return APPROACHING;
41 }
42 if (approaching_count < departing_count) {
43 return DEPARTING;
44 }
45 return UNKNOWN;
46}
47
49 const std::size_t channel_index, const std::vector<double>& signals) {
50 static constexpr int kStartFrequency = 3;
51 static constexpr int kFrameNumStored = 10;
52 std::vector<std::complex<double>> fft_results = fft1d(signals);
53 SignalStat signal_stat = GetSignalStat(fft_results, kStartFrequency);
54 signal_stats_[channel_index].push_back(signal_stat);
55 while (static_cast<int>(signal_stats_[channel_index].size()) >
56 kFrameNumStored) {
57 signal_stats_[channel_index].pop_front();
58 }
59 // TODO(kechxu) refactor the following initial version
60 MovingResult power_result = AnalyzePower(signal_stats_[channel_index]);
61 if (power_result != UNKNOWN) {
62 return power_result;
63 }
64 MovingResult top_frequency_result =
65 AnalyzeTopFrequence(signal_stats_[channel_index]);
66 return top_frequency_result;
67}
68
69MovingResult MovingDetection::AnalyzePower(
70 const std::deque<SignalStat>& signal_stats) {
71 int n = static_cast<int>(signal_stats.size());
72 if (n < 3) {
73 return UNKNOWN;
74 }
75 double first = signal_stats[n - 3].power();
76 double second = signal_stats[n - 2].power();
77 double third = signal_stats[n - 1].power();
78 if (first < second && second < third) {
79 return APPROACHING;
80 }
81 if (first > second && second > third) {
82 return DEPARTING;
83 }
84 return UNKNOWN;
85}
86
87MovingResult MovingDetection::AnalyzeTopFrequence(
88 const std::deque<SignalStat>& signal_stats) {
89 int n = static_cast<int>(signal_stats.size());
90 if (n < 3) {
91 return UNKNOWN;
92 }
93 int first = signal_stats[n - 3].top_frequency();
94 int second = signal_stats[n - 2].top_frequency();
95 int third = signal_stats[n - 1].top_frequency();
96 if (first < second && second < third) {
97 return APPROACHING;
98 }
99 if (first > second && second > third) {
100 return DEPARTING;
101 }
102 return UNKNOWN;
103}
104
105std::vector<std::complex<double>> MovingDetection::fft1d(
106 const std::vector<double>& signal) {
107 int n = static_cast<int>(signal.size());
108 fftw_complex in[n]; // NOLINT
109 fftw_complex out[n]; // NOLINT
110 for (int i = 0; i < n; ++i) {
111 in[i][0] = signal[i];
112 in[i][1] = 0.0;
113 }
114
115 fftw_plan p = fftw_plan_dft_1d(n, in, out, FFTW_FORWARD, FFTW_ESTIMATE);
116 fftw_execute(p);
117
118 std::vector<std::complex<double>> output;
119 output.reserve(n);
120 for (int i = 0; i < n; ++i) {
121 output.emplace_back(out[i][0], out[i][1]);
122 }
123 return output;
124}
125
126MovingDetection::SignalStat MovingDetection::GetSignalStat(
127 const std::vector<std::complex<double>>& fft_results,
128 const int start_frequency) {
129 double total_power = 0.0;
130 int top_frequency = -1;
131 double max_power = -1.0;
132 for (int i = start_frequency; i < static_cast<int>(fft_results.size()); ++i) {
133 double power = std::abs(fft_results[i]);
134 total_power += power;
135 if (power > max_power) {
136 max_power = power;
137 top_frequency = i;
138 }
139 }
140 return {total_power, top_frequency};
141}
142
143} // namespace audio
144} // namespace apollo
std::vector< std::complex< double > > fft1d(const std::vector< double > &signals)
MovingResult DetectSingleChannel(const std::size_t channel_index, const std::vector< double > &signal)
MovingResult Detect(const std::vector< std::vector< double > > &signals)
@description detect if the sound is approaching or departing
class register implement
Definition arena_queue.h:37