Currently there is some Boost code, because pdf/cdf has no direct equivalent in C++14.
|
using boost::math::normal; // typedef provides default type is double. |
|
normal unit_norm_dist(0,1); // (default mean = zero, and standard deviation = unity) |
|
double costFunction5(double measured_depth, double model_disp, double sigma, double floor_proportion) |
|
{ |
|
// NEED TO CONVERT MEASURED TO DISPARITY |
|
double measured_disp = (-0.7253/measured_depth + 1.0360 ); |
|
|
|
// measured_depth = ref_val [m] |
|
// model_disp = depth_val [0-1] |
|
// upper and lower bound on depth buffer: |
|
double lower_bound =0; |
|
double upper_bound =1; |
|
|
|
double gaussian_part = pdf(unit_norm_dist, (measured_disp-model_disp)/sigma)/sigma; |
|
double truncation = 1/cdf(unit_norm_dist,(upper_bound-model_disp)/sigma) - cdf(unit_norm_dist, (lower_bound-model_disp)/sigma); |
|
|
|
double trunc_gaussian_part = truncation*gaussian_part; |
|
|
|
double lhood= (floor_proportion/(upper_bound-lower_bound) + (1-floor_proportion)*trunc_gaussian_part); |
|
|
|
if (measured_depth< 0){ // all images pixels with no range |
|
lhood = 1; // log(1) = 0 ---> has no effect |
|
} |
|
|
|
return log (lhood); |
|
} |
Currently there is some Boost code, because
pdf/cdfhas no direct equivalent in C++14.pcl/simulation/src/range_likelihood.cpp
Lines 559 to 584 in 93f9e72