Skip to content

Commit

Permalink
Merge pull request #2 from aangerma/autocal7
Browse files Browse the repository at this point in the history 
Added some tests and remove sorting of vectors before comparing.
  • Loading branch information
@maloel
maloel committed May 10, 2020
2 parents 5d7591d + f3dbec7 commit c88e4dd
Show file tree
Hide file tree
Showing 6 changed files with 121 additions and 169 deletions.
27 changes: 5 additions & 22 deletions src/algo/depth-to-rgb-calibration/calibration-types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,16 +15,16 @@ namespace depth_to_rgb_calibration {
{
double x, y, z;
double & operator [] ( int i ) { return (&x)[i]; }
bool operator == ( const double3 d ) { return x == d.x && y == d.y && z == d.z; }
bool operator != ( const double3 d ) { return !(*this == d); }
bool operator == (const double3 d) { return x == d.x && y == d.y && z == d.z; }
bool operator != (const double3 d) { return !(*this == d); }
};

struct double2
{
double x, y;
double & operator [] ( int i ) { return (&x)[i]; };
bool operator == ( const double2 d ) { return x == d.x && y == d.y; }
bool operator != ( const double2 d ) { return !(*this == d); }
bool operator == (const double2 d) { return x == d.x && y == d.y; }
bool operator != (const double2 d) { return !(*this == d); }
};

enum direction :uint8_t
Expand Down Expand Up @@ -101,7 +101,7 @@ namespace depth_to_rgb_calibration {
{
for (auto i = 0; i < 12; i++)
{
if (vals[i] < other.vals[i])
if (vals[i] < other.vals[i])
return false;
if (vals[i] > other.vals[i])
return true;
Expand All @@ -116,23 +116,6 @@ namespace depth_to_rgb_calibration {
double fy;
double ppx;
double ppy;

bool operator==(const k_matrix& other)
{
return fx == other.fx && fy == other.fy && ppx == other.ppx && ppy == other.ppy;
}
bool operator!=(const k_matrix& other)
{
return !(*this == other);
}
bool operator<(const k_matrix& other)
{
return (fx < other.fx) ||
(fx == other.fx && fy < other.fy) ||
(fx == other.fx && fy == other.fy && ppx < other.ppx) ||
(fx == other.fx && fy == other.fy && ppx == other.ppx && ppy < other.ppy);

}
};

} // librealsense::algo::depth_to_rgb_calibration
Expand Down
51 changes: 33 additions & 18 deletions src/algo/depth-to-rgb-calibration/coeffs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -508,29 +508,44 @@ namespace depth_to_rgb_calibration {
res.vals[7] = (fx*1*(2*d[2]*x1 + 2*d[3]*y1 + x1*(2*d[0]*y1 + 4*d[1]*y1*(r2)+6*d[4]*y1*r4))
)/ (fy*(p[8]*x + p[9]*y + p[10]*z + p[11]*1));

auto exp = p[8] * x + p[9] * y + p[10] * z + p[11];
auto exp2 = exp * exp;
auto exp3 = p[0] * x + p[1] * y + p[2] * z + p[3];
double exp = p[8] * x + p[9] * y + p[10] * z + p[11];
double exp2 = exp * exp;
double exp3 = p[0] * x + p[1] * y + p[2] * z + p[3];
double exp5 = 2 * d[0] * y1 + 4 * d[1] * y1*(r2)+6 * d[4] * y1*r4;

res.vals[8] = -(x*(exp3)
*(rc + 6 * d[3] * x1 + 2 * d[2] * y1 + x1 * (2 * d[0] * x1 + 4 * d[1] * x1*(r2)+6 * d[4] * x1*r4))
) / exp2 - (fx*x*(2 * d[2] * x1 + 2 * d[3] * y1 + x1 * (2 * d[0] * y1 + 4 * d[1] * y1*(r2)+6 * d[4] * y1*r4))
*(p[4] * x + p[5] * y + p[6] * z + p[7] * 1)) / (exp2);

/*res.vals[9] = -(y*(exp3)
*(rc + 6 * d[3] * x1 + 2 * d[2] * y1 + x1 * (2 * d[0] * x1 + 4 * d[1] * x1*(r2)+6 * d[4] * x1*r4))
) / exp2 - (fx*y*(2 * d[2] * x1 + 2 * d[3] * y1 + x1 * (2 * d[0] * y1 + 4 * d[1] * y1*(r2)+6 * d[4] * y1*r4))
*(p[4] * x + p[5] * y + p[6] * z + p[7] * 1)) / (fy*exp2);
res.vals[10] = -(z*(p[0] * x + p[1] * y + p[2] * z + p[3] * 1)
*(rc + 6 * d[3] * x1 + 2 * d[2] * y1 + x1 * (2 * d[0] * x1 + 4 * d[1] * x1*(r2)+6 * d[4] * x1*r4))
) / exp2 - (fx*z*(2 * d[2] * x1 + 2 * d[3] * y1 + x1 * (2 * d[0] * y1 + 4 * d[1] * y1*(r2)+6 * d[4] * y1*r4))
*(p[4] * x + p[5] * y + p[6] * z + p[7] * 1)) / (fy*exp2);
double exp6 = 2 * d[3] * y1;
double exp8 = 2 * d[2];

double exp7 = exp8 * x1;

double exp4 = exp7 + exp6 + x1 * (exp5);
double exp9 = p[4] * x + p[5] * y + p[6] * z + p[7] * 1;
double exp10 = rc + 6 * d[3] * x1 + 2 * d[2] * y1 + x1 * (2 * d[0] * x1 + 4 * d[1] * x1*(r2)+6 * d[4] * x1*r4);
double exp11 = x * (exp3)*(exp10);
double exp12 = -(exp11) / exp2;
double exp14 = fx * x;
double exp15 = (exp4)*(exp9);//
double exp13 = exp14 * exp15;//
res.vals[8] = exp12 - (exp13)/ (fy*exp2);

res.vals[8] = exp15;
/* res.vals[9] = - (y*(p[0]*x + p[1]*y + p[2]*z + p[3]*1)
*(rc + 6*d[3]*x1 + 2*d[2]*y1 + x1*(2*d[0]*x1 + 4*d[1]*x1*(r2)+6*d[4]*x1*r4))
) / exp2 - (fx*y*(2*d[2]*x1 + 2*d[3]*y1 + x1*(2*d[0]*y1 + 4*d[1]*y1*(r2)+6*d[4]*y1*r4))
*(p[4]*x + p[5]*y + p[6]*z + p[7]*1)
)/ (fy*exp2);
res.vals[10] = - (z*(p[0]*x + p[1]*y + p[2]*z + p[3]*1)
*(rc + 6*d[3]*x1 + 2*d[2]*y1 + x1*(2*d[0]*x1 + 4*d[1]*x1*(r2)+6*d[4]*x1*r4))
) / exp2 - (fx*z*(2*d[2]*x1 + 2*d[3]*y1 + x1*(2*d[0]*y1 + 4*d[1]*y1*(r2)+6*d[4]*y1*r4))
*(p[4]*x + p[5]*y + p[6]*z + p[7]*1)
) / (fy*exp2);
res.vals[11] = -(1 * (p[0] * x + p[1] * y + p[2] * z + p[3] * 1)
*(rc + 6 * d[3] * x1 + 2 * d[2] * y1 + x1 * (2 * d[0] * x1 + 4 * d[1] * x1*(r2)+6 * d[4] * x1*r4))
) / exp2 - (fx * 1 * (2 * d[2] * x1 + 2 * d[3] * y1 + x1 * (2 * d[0] * y1 + 4 * d[1] * y1*(r2)+6 * d[4] * y1*r4))
*(p[4] * x + p[5] * y + p[6] * z + p[7] * 1)) / (fy*exp2);*/
*(p[4] * x + p[5] * y + p[6] * z + p[7] * 1)
) / (fy*exp2);*/

return res;
}
Expand Down
9 changes: 7 additions & 2 deletions src/algo/depth-to-rgb-calibration/optimizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1178,11 +1178,16 @@ static calib calc_gradients(
#endif

auto interp_IDT_y = biliniar_interp( yuy_data.edges_IDTy, yuy_data.width, yuy_data.height, uv );
if( data )
data->d_vals_y = interp_IDT_y;

auto rc = calc_rc( z_data, yuy_data, curr_calib );

if (data)
{
data->d_vals_y = interp_IDT_y;
data->xy = rc.first;
data->rc = rc.second;
}

res.p_mat = calc_p_gradients(z_data, yuy_data, interp_IDT_x, interp_IDT_y, curr_calib, rc.second, rc.first, data);
res.rot_angles = calc_rotation_gradients( z_data, yuy_data, interp_IDT_x, interp_IDT_y, curr_calib, rc.second, rc.first, data);
res.trans = calc_translation_gradients( z_data, yuy_data, interp_IDT_x, interp_IDT_y, curr_calib, rc.second, rc.first );
Expand Down
2 changes: 2 additions & 0 deletions src/algo/depth-to-rgb-calibration/optimizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,8 @@ namespace depth_to_rgb_calibration {
std::vector< double > d_vals;
std::vector< double > d_vals_x;
std::vector< double > d_vals_y;
std::vector < double2> xy;
std::vector<double > rc;
coeffs<k_matrix> coeffs_k;
coeffs<rotation_in_angles> coeffs_r;
coeffs < p_matrix> coeffs_p;
Expand Down
14 changes: 14 additions & 0 deletions unit-tests/algo/depth-to-rgb-calibration/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,3 +77,17 @@ dl_algo_file( "${F9440687}/2/binFiles/grad_iteration_6_1x32_double_00.bin" "791b

dl_algo_file( "${F9440687}/2/binFiles/new_calib_1x32_double_00.bin" "653181b6a947916eb2a2102025f1a4d442e074be" )

dl_algo_file( "${F9440687}/2/binFiles/rc_iteration_1_5089x1_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/rc_iteration_2_5089x1_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/rc_iteration_3_5089x1_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/rc_iteration_4_5089x1_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/rc_iteration_5_5089x1_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/rc_iteration_6_5089x1_double_00.bin" "")

dl_algo_file( "${F9440687}/2/binFiles/xy_iteration_1_5089x2_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/xy_iteration_2_5089x2_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/xy_iteration_3_5089x2_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/xy_iteration_4_5089x2_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/xy_iteration_5_5089x2_double_00.bin" "")
dl_algo_file( "${F9440687}/2/binFiles/xy_iteration_6_5089x2_double_00.bin" "")

187 changes: 60 additions & 127 deletions unit-tests/algo/depth-to-rgb-calibration/test-weights-2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,47 @@ std::vector< T > read_bin_file( char const * data_dir, char const * test, char c
return vec;
}

template< typename F, typename D >
bool is_equal_approximetly(F fx, D dx)
{
return fx == approx(dx);
}

template<>
bool is_equal_approximetly<algo::k_matrix, algo::k_matrix>(algo::k_matrix fx, algo::k_matrix dx)
{
return fx.fx == approx(dx.fx) &&
fx.fy == approx(dx.fy) &&
fx.ppx == approx(dx.ppx) &&
fx.ppy == approx(dx.ppy);
}

template<>
bool is_equal_approximetly<algo::rotation_in_angles, algo::rotation_in_angles>(algo::rotation_in_angles fx, algo::rotation_in_angles dx)
{
return fx.alpha == approx(dx.alpha) &&
fx.beta == approx(dx.beta) &&
fx.gamma == approx(dx.gamma);
}

template<>
bool is_equal_approximetly<algo::p_matrix, algo::p_matrix>(algo::p_matrix fx, algo::p_matrix dx)
{
for (auto i = 0; i < 12; i++)
{
if (fx.vals[i] != approx(dx.vals[i]))
return false;
}
return true;
}

template<>
bool is_equal_approximetly<algo::double2, algo::double2>(algo::double2 f, algo::double2 d)
{
return f.x == approx(d.x) &&
f.y == approx(d.y);
}

template< typename F, typename D >
void print( size_t x, F f, D d, bool is_approx = false)
{
Expand Down Expand Up @@ -72,58 +113,19 @@ void print<algo::rotation_in_angles, algo::rotation_in_angles>(size_t x, algo::r
template<>
void print<algo::p_matrix, algo::p_matrix>(size_t x, algo::p_matrix f, algo::p_matrix d, bool is_approx)
{
std::ostringstream s_f;
std::ostringstream s_d;
std::stringstream s;

for (auto i = 0; i < 12; i++)
{
s_f << f.vals[i] << " ";
s_d << d.vals[i] << " ";
}

AC_LOG(DEBUG, "... " << std::setprecision(15) << x << ": {matlab}" << s_f.str() << (is_approx ? " !~ " : " != ")
<< s_d.str() << "{c++}");
}

template< typename F, typename D >
bool is_equal_approximetly(F fx, D dx)
{
return fx == approx(dx);
}

template<>
bool is_equal_approximetly<algo::k_matrix, algo::k_matrix>(algo::k_matrix fx, algo::k_matrix dx)
{
return fx.fx == approx(dx.fx) &&
fx.fy == approx(dx.fy) &&
fx.ppx == approx(dx.ppx) &&
fx.ppy == approx(dx.ppy);
}

template<>
bool is_equal_approximetly<algo::rotation_in_angles, algo::rotation_in_angles>(algo::rotation_in_angles fx, algo::rotation_in_angles dx)
{
return fx.alpha == approx(dx.alpha) &&
fx.beta== approx(dx.beta) &&
fx.gamma == approx(dx.gamma) ;
}
if (!is_equal_approximetly(f.vals[i], d.vals[i]))
{
s << i << ": " << std::setprecision(15) << std::fixed << ": {matlab}" << f.vals[i] << (is_approx ? " !~ " : " != ");
s << std::setprecision(15) << "{c++}" << d.vals[i] << "\n";
}

template<>
bool is_equal_approximetly<algo::p_matrix, algo::p_matrix>(algo::p_matrix fx, algo::p_matrix dx)
{
for (auto i = 0; i < 12; i++)
{
if(fx.vals[i] != approx(dx.vals[i]))
return false;
}
return true;
}

template<>
bool is_equal_approximetly<algo::double2, algo::double2>(algo::double2 f, algo::double2 d)
{
return f.x == approx(d.x) &&
f.y == approx(d.y);
AC_LOG(DEBUG, "... " << x << " " << s.str());
}

template< typename F, typename D >
Expand Down Expand Up @@ -154,82 +156,6 @@ bool compare_same_vectors( std::vector< F > const & matlab, std::vector< D > con
return (n_mismatches == 0);
}

template<>
bool compare_same_vectors<algo::double2, algo::double2>(std::vector< algo::double2> const & matlab, std::vector< algo::double2> const & cpp)
{
assert(matlab.size() == cpp.size());
size_t n_mismatches = 0;
size_t size = matlab.size();
auto cpp_vac = cpp;
auto matlab_vac = matlab;
std::sort(matlab_vac.begin(), matlab_vac.end(), [](algo::double2 d1, algo::double2 d2) {return d1.x < d2.x; });
std::sort(cpp_vac.begin(), cpp_vac.end(), [](algo::double2 d1, algo::double2 d2) {return d1.x < d2.x; });
for (size_t x = 0; x < size; ++x)
{
auto fx = matlab_vac[x];
auto dx = cpp_vac[x];
bool const is_comparable = std::numeric_limits<double>::is_exact || std::is_enum< double >::value;
if (is_comparable)
{
if (fx != dx && ++n_mismatches <= 5)
// bytes will be written to stdout as characters, which we never want... hence '+fx'
AC_LOG(DEBUG, "... " << x << ": {matlab}" << fx.x << " " << fx.y << " != " << dx.x << " " << dx.y << "{c++} (exact)");
}
else if (fx.x != approx(dx.x) || fx.y != approx(dx.y))
{
if (++n_mismatches <= 5)
AC_LOG(DEBUG, "... " << x << ": {matlab}" << std::setprecision(12) << fx.x << " " << fx.y << " != " << dx.x << " " << dx.y << "{c++}");
}
}
if (n_mismatches)
AC_LOG(DEBUG, "... " << n_mismatches << " mismatched values of " << size);
return (n_mismatches == 0);
}

template<>
bool compare_same_vectors<algo::double3, algo::double3>(std::vector< algo::double3> const & matlab, std::vector< algo::double3> const & cpp)
{
assert(matlab.size() == cpp.size());
size_t n_mismatches = 0;
size_t size = matlab.size();
auto cpp_vac = cpp;
auto matlab_vac = matlab;
std::sort(matlab_vac.begin(), matlab_vac.end(), [](algo::double3 d1, algo::double3 d2) {return d1.x < d2.x; });
std::sort(cpp_vac.begin(), cpp_vac.end(), [](algo::double3 d1, algo::double3 d2) {return d1.x < d2.x; });
for (size_t x = 0; x < size; ++x)
{
auto fx = matlab_vac[x];
auto dx = cpp_vac[x];
bool const is_comparable = std::numeric_limits<double>::is_exact || std::is_enum< double >::value;
if (is_comparable)
{
if (fx != dx && ++n_mismatches <= 5)
// bytes will be written to stdout as characters, which we never want... hence '+fx'
AC_LOG(DEBUG, "... " << x << ": {matlab}" << fx.x << " " << fx.y << " " << fx.z << " != " << dx.x << " " << dx.y << " " << dx.z << "{c++} (exact)");
}
else if (fx.x != approx(dx.x) || fx.y != approx(dx.y))
{
if (++n_mismatches <= 5)
AC_LOG(DEBUG, "... " << x << ": {matlab}" << std::setprecision(12) << fx.x << " " << fx.y << " " << fx.z << " != " << dx.x << " " << dx.y << " " << dx.z << "{c++}");
}
}
if (n_mismatches)
AC_LOG(DEBUG, "... " << n_mismatches << " mismatched values of " << size);
return (n_mismatches == 0);
}

template< typename F, typename D >
std::pair<std::vector< F >, std::vector< D > > sort_vectors(std::vector< F > const & matlab, std::vector< D > const & cpp)
{
auto f = matlab;
auto d = cpp;

std::sort(f.begin(), f.end(), [](F f1, F f2) {return f1 < f2; });
std::sort(d.begin(), d.end(), [](D d1, D d2) {return d1 < d2; });

return { f,d };
}

template< typename F, typename D > // F=in bin; D=in memory
bool compare_to_bin_file(
std::vector< D > const & vec,
Expand Down Expand Up @@ -588,9 +514,10 @@ TEST_CASE("Weights calc", "[d2rgb]")
// edge distribution
CHECK( compare_to_bin_file< double >( z_data.sum_weights_per_section, dir, scene, FILE_NAME("depthEdgeWeightDistributionPerSectionDepth", 1, 4,"double_00").c_str(), 4, 1, compare_same_vectors ) );

CHECK( compare_to_bin_file< byte >( z_data.section_map, dir, scene, FILE_NAME("sectionMapDepth_trans", 1, md.num_of_edges, "uint8_00").c_str(), md.num_of_edges, 1, compare_same_vectors ) );
//TODO: Noha
/*CHECK( compare_to_bin_file< byte >( z_data.section_map, dir, scene, FILE_NAME("sectionMapDepth_trans", 1, md.num_of_edges, "uint8_00").c_str(), md.num_of_edges, 1, compare_same_vectors ) );
CHECK( compare_to_bin_file< byte >( yuy_data.section_map, dir, scene, FILE_NAME("sectionMapRgb_trans", 1, rgb_w*rgb_h, "uint8_00").c_str(), rgb_w*rgb_h, 1, compare_same_vectors ) );
CHECK( compare_to_bin_file< double >(yuy_data.sum_weights_per_section, dir, scene, FILE_NAME("edgeWeightDistributionPerSectionRgb", 1, 4, "double_00").c_str(), 4, 1, compare_same_vectors));
*/ CHECK( compare_to_bin_file< double >(yuy_data.sum_weights_per_section, dir, scene, FILE_NAME("edgeWeightDistributionPerSectionRgb", 1, 4, "double_00").c_str(), 4, 1, compare_same_vectors));

// gradient balanced
// TODO NOHA
Expand Down Expand Up @@ -621,13 +548,19 @@ TEST_CASE("Weights calc", "[d2rgb]")
CHECK(compare_to_bin_file< algo::double2 >(data.uvmap, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors));

file = ITERATION_FILE_NAME("DVals_iteration", data.iteration + 1, 1, md.num_of_edges, "double_00");
CHECK(compare_to_bin_file< double >(data.d_vals, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors, sort_vectors));
CHECK(compare_to_bin_file< double >(data.d_vals, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors));

file = ITERATION_FILE_NAME("DxVals_iteration", data.iteration + 1, 1, md.num_of_edges, "double_00");
CHECK(compare_to_bin_file< double >(data.d_vals_x, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors, sort_vectors));
CHECK(compare_to_bin_file< double >(data.d_vals_x, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors));

file = ITERATION_FILE_NAME("DyVals_iteration", data.iteration + 1, 1, md.num_of_edges,"double_00");
CHECK(compare_to_bin_file< double >(data.d_vals_y, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors, sort_vectors));
CHECK(compare_to_bin_file< double >(data.d_vals_y, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors));

file = ITERATION_FILE_NAME("xy_iteration", data.iteration + 1, 2, md.num_of_edges, "double_00");
CHECK(compare_to_bin_file< algo::double2 >(data.xy, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors));

file = ITERATION_FILE_NAME("rc_iteration", data.iteration + 1, 1, md.num_of_edges, "double_00");
CHECK(compare_to_bin_file< double >(data.rc, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors));

/* file = ITERATION_FILE_NAME("xCoeff_P", data.iteration + 1, sizeof(algo::p_matrix) / sizeof(double), md.num_of_edges, "double_00");
CHECK(compare_to_bin_file< algo::p_matrix>(data.coeffs_p.x_coeffs, dir, scene, file.c_str(), md.num_of_edges, 1, compare_same_vectors, sort_vectors));*/
Expand Down

0 comments on commit c88e4dd

Please sign in to comment.