Commit 1bca0182 authored by Zhiping Jiang (蒋志平)'s avatar Zhiping Jiang (蒋志平) 💬

Fix bug in phase unwrapping. well tested.

parent 4442e391
......@@ -7,7 +7,6 @@
#include "CvComplex.h"
namespace cve {
namespace complex {
......@@ -29,64 +28,26 @@ namespace cve {
return phase;
}
cv::Mat1d unwrap(const cv::Mat1d & phase) {
#define MAX_LENGTH 10000
static double dp[MAX_LENGTH];
static double dps[MAX_LENGTH];
static double dp_corr[MAX_LENGTH];
static double cumsum[MAX_LENGTH];
int j = 0;
int N = phase.total();
cv::Mat1d unwrapped_phase;
phase.copyTo(unwrapped_phase);
memset(dp, 0, N);
memset(dps, 0, N);
memset(dp_corr, 0, N);
memset(cumsum, 0, N);
assert(N < MAX_LENGTH);
// incremental phase variation
// MATLAB: dp = diff(p, 1, 1);
for (j = 0; j < N-1; j++)
dp[j] = phase(j+1) - phase(j);
// equivalent phase variation in [-pi, pi]
// MATLAB: dps = mod(dp+dp,2*pi) - pi;
for (j = 0; j < N-1; j++)
dps[j] = (dp[j]+M_PI) - floor((dp[j]+M_PI) / (2*M_PI))*(2*M_PI) - M_PI;
// preserve variation sign for +pi vs. -pi
// MATLAB: dps(dps==pi & dp>0,:) = pi;
for (j = 0; j < N-1; j++)
if ((dps[j] == -M_PI) && (dp[j] > 0))
dps[j] = M_PI;
// incremental phase correction
// MATLAB: dp_corr = dps - dp;
for (j = 0; j < N-1; j++)
dp_corr[j] = dps[j] - dp[j];
// Ignore correction when incremental variation is smaller than cutoff
// MATLAB: dp_corr(abs(dp)<cutoff,:) = 0;
for (j = 0; j < N-1; j++)
if (fabs(dp[j]) < M_PI)
dp_corr[j] = 0;
// Find cumulative sum of deltas
// MATLAB: cumsum = cumsum(dp_corr, 1);
cumsum[0] = dp_corr[0];
for (j = 1; j < N-1; j++)
cumsum[j] = cumsum[j-1] + dp_corr[j];
// Integrate corrections and add to P to produce smoothed phase values
// MATLAB: p(2:m,:) = p(2:m,:) + cumsum(dp_corr,1);
for (j = 1; j < N; j++)
unwrapped_phase(j) += cumsum[j-1];
#undef MAX_LENGTH
auto phase_unwrap = [](double prev, double now) -> double
{
constexpr double M_2PI = M_PI * 2;
auto diff = fmod(now - prev, M_PI * 2);
auto diff_abs = fabs(diff);
if (M_PI >= diff_abs)
return prev + diff;
else {
diff = diff > 0 ? -(M_2PI - diff_abs) : (M_2PI - diff_abs);
return prev + diff;
}
};
cv::Mat1d unwrapped_phase = phase.clone();
for(auto i = 1 ; i < unwrapped_phase.total(); i ++) {
unwrapped_phase(i) = phase_unwrap(unwrapped_phase(i-1), unwrapped_phase(i));
}
return unwrapped_phase;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment