diff --git a/src/algo/depth-to-rgb-calibration/coeffs.cpp b/src/algo/depth-to-rgb-calibration/coeffs.cpp index 30bc0aa915..9174cf6257 100644 --- a/src/algo/depth-to-rgb-calibration/coeffs.cpp +++ b/src/algo/depth-to-rgb-calibration/coeffs.cpp @@ -620,7 +620,7 @@ namespace depth_to_rgb_calibration { { coeffs res; auto engles = extract_angles_from_rotation( yuy_intrin_extrin.rot.rot ); - auto v = z_data.vertices; + auto v = z_data.vertices_all; res.x_coeffs.resize( v.size() ); res.y_coeffs.resize( v.size() ); @@ -750,7 +750,7 @@ namespace depth_to_rgb_calibration { { coeffs res; - auto v = z_data.vertices; + auto v = z_data.vertices_all; res.y_coeffs.resize(v.size()); res.x_coeffs.resize(v.size()); @@ -767,7 +767,7 @@ namespace depth_to_rgb_calibration { { coeffs res; - auto v = z_data.vertices; + auto v = z_data.vertices_all; res.y_coeffs.resize( v.size() ); res.x_coeffs.resize( v.size() ); @@ -871,7 +871,7 @@ namespace depth_to_rgb_calibration { iteration_data_collect * data) { coeffs res; - auto v = z_data.vertices; + auto v = z_data.vertices_all; res.x_coeffs.resize( v.size() ); res.y_coeffs.resize( v.size() ); diff --git a/src/algo/depth-to-rgb-calibration/debug.h b/src/algo/depth-to-rgb-calibration/debug.h index 7636e952f7..6565f9675f 100644 --- a/src/algo/depth-to-rgb-calibration/debug.h +++ b/src/algo/depth-to-rgb-calibration/debug.h @@ -10,6 +10,6 @@ #define AC_LOG_PREFIX "AC1: " #define AC_LOG(TYPE,MSG) LOG_##TYPE( AC_LOG_PREFIX << (std::string)( to_string() << MSG )) //#define AC_LOG(TYPE,MSG) LOG_ERROR( AC_LOG_PREFIX << MSG ) -//#define AC_LOG(TYPE,MSG) std::cout << (std::string)( to_string() << "-" << #TYPE [0] << "- " << MSG ) << std::endl; //LOG_INFO((std::string)( to_string() << "-" << #TYPE [0] << "- " << MSG )); +//#define AC_LOG(TYPE,MSG) std::cout << (std::string)( to_string() << std::setprecision(6)< ir_frame; - std::vector< double > ir_edges; - std::vector edges; + std::vector< double > edges; + std::vector< double > local_edges; // smearing std::vector gradient_x; std::vector gradient_y; std::vector< byte > section_map_depth; - std::vector valid_edge_pixels_by_ir; + std::vector valid_edge_pixels_by_ir; std::vector valid_section_map; std::vectorvalid_gradient_x; std::vectorvalid_gradient_y; - std::vector edges2; + //std::vector edges; std::vector directions; std::vector direction_deg; std::vector valid_location_rc_x; @@ -51,8 +51,9 @@ namespace depth_to_rgb_calibration { std::vector local_region[4]; std::vector local_region_x[4];//for debug std::vector local_region_y[4]; // for debug - std::vector local_edges; + //std::vector ir_edges; std::vector is_supressed; + std::vector fraq_step; }; @@ -67,16 +68,43 @@ namespace depth_to_rgb_calibration { std::vector edges; std::vector supressed_edges; size_t n_strong_edges; - std::vector directions; + //std::vector valid_directions; std::vector subpixels_x; std::vector subpixels_y; - std::vector< uint16_t> closest; + //std::vector< uint16_t> closest; std::vector weights; std::vector direction_deg; - std::vector vertices; + std::vector vertices_all; //smearing std::vector< byte > section_map_depth; + std::vector< byte > section_map_depth_inside; + std::vector local_x; + std::vector local_y; + std::vector gradient; + std::vector local_values; + std::vector grad_in_direction; + std::vector grad_in_direction_inside; + std::vector values_for_subedges; + std::vector closest; + //std::vector< byte > supressed_edges; + std::vector direction_per_pixel; + std::vector valid_direction_per_pixel; + std::vector valid_section_map; + std::vector local_rc_subpixel; + std::vector edge_sub_pixel; + std::vector valid_directions; + std::vector directions; + std::vector valid_edge_sub_pixel; + std::vector valid_edge_sub_pixel_x; + std::vector valid_edge_sub_pixel_y; + std::vector sub_points; + //std::vector vertices; + std::vector vertices; + std::vector vertices3; + std::vector uvmap; + std::vector is_inside; + // input validation std::vector section_map; bool is_edge_distributed; diff --git a/src/algo/depth-to-rgb-calibration/optimizer.cpp b/src/algo/depth-to-rgb-calibration/optimizer.cpp index 58a46a6648..4694e2eb44 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.cpp +++ b/src/algo/depth-to-rgb-calibration/optimizer.cpp @@ -217,168 +217,168 @@ std::pair< int, int > get_next_index( return { edge_plus_idx, edge_plus_idy }; } -// Return Z edges with weak edges zeroed out -static void suppress_weak_edges( - z_frame_data & z_data, - ir_frame_data const & ir_data, - params const & params ) -{ - std::vector< double > & res = z_data.supressed_edges; - res = z_data.edges; - double const grad_ir_threshold = params.grad_ir_threshold; - double const grad_z_threshold = params.grad_z_threshold; - z_data.n_strong_edges = 0; - for( auto i = 0; i < z_data.height; i++ ) - { - for( auto j = 0; j < z_data.width; j++ ) - { - auto idx = i * z_data.width + j; - - auto edge = z_data.edges[idx]; - - //if (edge == 0) continue; - - auto edge_prev_idx = get_prev_index( z_data.directions[idx], i, j, z_data.width, z_data.height ); - - auto edge_next_idx = get_next_index( z_data.directions[idx], i, j, z_data.width, z_data.height ); - - auto edge_minus_idx = edge_prev_idx.second * z_data.width + edge_prev_idx.first; - - auto edge_plus_idx = edge_next_idx.second * z_data.width + edge_next_idx.first; - - auto z_edge_plus = z_data.edges[edge_plus_idx]; - auto z_edge = z_data.edges[idx]; - auto z_edge_minus = z_data.edges[edge_minus_idx]; - - if( z_edge_minus > z_edge || z_edge_plus > z_edge || ir_data.ir_edges[idx] <= grad_ir_threshold || z_data.edges[idx] <= grad_z_threshold ) - { - res[idx] = 0; - } - else - { - ++z_data.n_strong_edges; - } - } - } -} - -static -std::pair< - std::vector< double >, - std::vector< double > -> -calc_subpixels( - z_frame_data const & z_data, - ir_frame_data const & ir_data, - double const grad_ir_threshold, - double const grad_z_threshold, - size_t const width, size_t const height -) -{ - std::vector< double > subpixels_x; - std::vector< double > subpixels_y; - - subpixels_x.reserve( z_data.edges.size() ); - subpixels_y.reserve( z_data.edges.size() ); - - for( auto i = 0; i < height; i++ ) - { - for( auto j = 0; j < width; j++ ) - { - auto idx = i * width + j; - - auto edge = z_data.edges[idx]; - - //if( edge == 0 ) continue; // TODO commented out elsewhere... - - // Note: - // The original matlab code shifts in the opposite direction: - // Z_plus = circshift( frame.z, -currDir ); - // Z_minus = circshift( frame.z, +currDir ); - // But here we're looking at a specific index and what its value would be - // AFTER the shift. E.g. (with 1 dimension for simplicity, with dir=[1]): - // original: [ 1 2 3 4 ] - // Z_plus : [ 2 3 4 1 ] (shift left [-1]) - // Z_minus : [ 4 1 2 3 ] (shift right [1]) - // At index [2] (0-based) there was a 3 but we now need Z_plus-Z_minus (see below) - // or (4-2). Note that, for Z_plus, the shift was left but the new value was from - // the right! In other words, we do not need to negate currDir! - - auto edge_prev_idx = get_prev_index( z_data.directions[idx], i, j, width, height ); - auto edge_next_idx = get_next_index( z_data.directions[idx], i, j, width, height ); - - auto edge_minus_idx = edge_prev_idx.second * width + edge_prev_idx.first; - auto edge_plus_idx = edge_next_idx.second * width + edge_next_idx.first; - - auto z_plus = z_data.edges[edge_plus_idx]; - auto z_edge = z_data.edges[idx]; - auto z_minus = z_data.edges[edge_minus_idx]; - - auto dir = z_data.directions[idx]; - double x = 0, y = 0; - - if(z_data.supressed_edges[idx]) - { - //% fraqStep = (-0.5*(zEdge_plus-zEdge_minus)./(zEdge_plus+zEdge_minus-2*zEdge)); % The step we need to move to reach the subpixel gradient i nthe gradient direction - //% subGrad_d = fraqStep.*reshape(currDir,1,1,[]); - //% subGrad_d = subGrad_d + cat(3,gridY,gridX);% the location of the subpixel gradient - //% ... - //% zEdgeSubPixel(cat(3,supressedEdges_d,supressedEdges_d)) = subGrad_d(cat(3,supressedEdges_d,supressedEdges_d)); - - double fraq_step = 0; - if( double( z_plus + z_minus - (double)2 * z_edge ) == 0 ) - fraq_step = 0; - else - fraq_step = double( (-0.5f*double( z_plus - z_minus )) / double( z_plus + z_minus - 2 * z_edge ) ); - - // NOTE: - // We adjust by +1 to fit the X/Y to matlab's 1-based index convention - // TODO make sure this fits in with our own USAGE of these coordinates (where we would - // likely have to do -1) - y = i + 1 + fraq_step * (double)dir_map[dir].second; - x = j + 1 + fraq_step * (double)dir_map[dir].first; - } - - subpixels_y.push_back( y ); - subpixels_x.push_back( x ); - } - } - assert( subpixels_x.size() == z_data.edges.size() ); - return { subpixels_x, subpixels_y }; -} - -void optimizer::set_z_data( - std::vector< z_t > && z_data, - rs2_intrinsics_double const & depth_intrinsics, - float depth_units ) -{ - _params.set_depth_resolution( depth_intrinsics.width, depth_intrinsics.height ); - _z.width = depth_intrinsics.width; - _z.height = depth_intrinsics.height; - _z.intrinsics = depth_intrinsics; - _z.depth_units = depth_units; - - _z.frame = std::move( z_data ); - - _z.gradient_x = calc_vertical_gradient( _z.frame, depth_intrinsics.width, depth_intrinsics.height ); - _z.gradient_y = calc_horizontal_gradient( _z.frame, depth_intrinsics.width, depth_intrinsics.height ); - _z.edges = calc_intensity( _z.gradient_x, _z.gradient_y ); - _z.directions = get_direction( _z.gradient_x, _z.gradient_y ); - _z.direction_deg = get_direction_deg( _z.gradient_x, _z.gradient_y ); - suppress_weak_edges( _z, _ir, _params ); - - auto subpixels = calc_subpixels( _z, _ir, - _params.grad_ir_threshold, _params.grad_z_threshold, - depth_intrinsics.width, depth_intrinsics.height ); - _z.subpixels_x = subpixels.first; - _z.subpixels_y = subpixels.second; - - _z.closest = get_closest_edges( _z, _ir, depth_intrinsics.width, depth_intrinsics.height ); - - calculate_weights( _z ); - - auto vertices = subedges2vertices( _z, depth_intrinsics, depth_units ); -} +//// Return Z edges with weak edges zeroed out +//static void suppress_weak_edges( +// z_frame_data & z_data, +// ir_frame_data const & ir_data, +// params const & params ) +//{ +// std::vector< double > & res = z_data.supressed_edges; +// res = z_data.edges; +// double const grad_ir_threshold = params.grad_ir_threshold; +// double const grad_z_threshold = params.grad_z_threshold; +// z_data.n_strong_edges = 0; +// for( auto i = 0; i < z_data.height; i++ ) +// { +// for( auto j = 0; j < z_data.width; j++ ) +// { +// auto idx = i * z_data.width + j; +// +// auto edge = z_data.edges[idx]; +// +// //if (edge == 0) continue; +// +// auto edge_prev_idx = get_prev_index( z_data.valid_directions[idx], i, j, z_data.width, z_data.height ); +// +// auto edge_next_idx = get_next_index( z_data.valid_directions[idx], i, j, z_data.width, z_data.height ); +// +// auto edge_minus_idx = edge_prev_idx.second * z_data.width + edge_prev_idx.first; +// +// auto edge_plus_idx = edge_next_idx.second * z_data.width + edge_next_idx.first; +// +// auto z_edge_plus = z_data.edges[edge_plus_idx]; +// auto z_edge = z_data.edges[idx]; +// auto z_edge_minus = z_data.edges[edge_minus_idx]; +// +// if( z_edge_minus > z_edge || z_edge_plus > z_edge || ir_data.edges[idx] <= grad_ir_threshold || z_data.edges[idx] <= grad_z_threshold ) +// { +// res[idx] = 0; +// } +// else +// { +// ++z_data.n_strong_edges; +// } +// } +// } +//} + +//static +//std::pair< +// std::vector< double >, +// std::vector< double > +//> +//calc_subpixels( +// z_frame_data const & z_data, +// ir_frame_data const & ir_data, +// double const grad_ir_threshold, +// double const grad_z_threshold, +// size_t const width, size_t const height +//) +//{ +// std::vector< double > subpixels_x; +// std::vector< double > subpixels_y; +// +// subpixels_x.reserve( z_data.edges.size() ); +// subpixels_y.reserve( z_data.edges.size() ); +// +// for( auto i = 0; i < height; i++ ) +// { +// for( auto j = 0; j < width; j++ ) +// { +// auto idx = i * width + j; +// +// auto edge = z_data.edges[idx]; +// +// //if( edge == 0 ) continue; // TODO commented out elsewhere... +// +// // Note: +// // The original matlab code shifts in the opposite direction: +// // Z_plus = circshift( frame.z, -currDir ); +// // Z_minus = circshift( frame.z, +currDir ); +// // But here we're looking at a specific index and what its value would be +// // AFTER the shift. E.g. (with 1 dimension for simplicity, with dir=[1]): +// // original: [ 1 2 3 4 ] +// // Z_plus : [ 2 3 4 1 ] (shift left [-1]) +// // Z_minus : [ 4 1 2 3 ] (shift right [1]) +// // At index [2] (0-based) there was a 3 but we now need Z_plus-Z_minus (see below) +// // or (4-2). Note that, for Z_plus, the shift was left but the new value was from +// // the right! In other words, we do not need to negate currDir! +// +// auto edge_prev_idx = get_prev_index( z_data.valid_directions[idx], i, j, width, height ); +// auto edge_next_idx = get_next_index( z_data.valid_directions[idx], i, j, width, height ); +// +// auto edge_minus_idx = edge_prev_idx.second * width + edge_prev_idx.first; +// auto edge_plus_idx = edge_next_idx.second * width + edge_next_idx.first; +// +// auto z_plus = z_data.edges[edge_plus_idx]; +// auto z_edge = z_data.edges[idx]; +// auto z_minus = z_data.edges[edge_minus_idx]; +// +// auto dir = z_data.valid_directions[idx]; +// double x = 0, y = 0; +// +// if(z_data.supressed_edges[idx]) +// { +// //% fraqStep = (-0.5*(zEdge_plus-zEdge_minus)./(zEdge_plus+zEdge_minus-2*zEdge)); % The step we need to move to reach the subpixel gradient i nthe gradient direction +// //% subGrad_d = fraqStep.*reshape(currDir,1,1,[]); +// //% subGrad_d = subGrad_d + cat(3,gridY,gridX);% the location of the subpixel gradient +// //% ... +// //% zEdgeSubPixel(cat(3,supressedEdges_d,supressedEdges_d)) = subGrad_d(cat(3,supressedEdges_d,supressedEdges_d)); +// +// double fraq_step = 0; +// if( double( z_plus + z_minus - (double)2 * z_edge ) == 0 ) +// fraq_step = 0; +// else +// fraq_step = double( (-0.5f*double( z_plus - z_minus )) / double( z_plus + z_minus - 2 * z_edge ) ); +// +// // NOTE: +// // We adjust by +1 to fit the X/Y to matlab's 1-based index convention +// // TODO make sure this fits in with our own USAGE of these coordinates (where we would +// // likely have to do -1) +// y = i + 1 + fraq_step * (double)dir_map[dir].second; +// x = j + 1 + fraq_step * (double)dir_map[dir].first; +// } +// +// subpixels_y.push_back( y ); +// subpixels_x.push_back( x ); +// } +// } +// assert( subpixels_x.size() == z_data.edges.size() ); +// return { subpixels_x, subpixels_y }; +//} + +//void optimizer::set_z_data( +// std::vector< z_t > && z_data, +// rs2_intrinsics_double const & depth_intrinsics, +// float depth_units ) +//{ +// _params.set_depth_resolution( depth_intrinsics.width, depth_intrinsics.height ); +// _z.width = depth_intrinsics.width; +// _z.height = depth_intrinsics.height; +// _z.intrinsics = depth_intrinsics; +// _z.depth_units = depth_units; +// +// _z.frame = std::move( z_data ); +// +// _z.gradient_x = calc_vertical_gradient( _z.frame, depth_intrinsics.width, depth_intrinsics.height ); +// _z.gradient_y = calc_horizontal_gradient( _z.frame, depth_intrinsics.width, depth_intrinsics.height ); +// _z.edges = calc_intensity( _z.gradient_x, _z.gradient_y ); +// _z.directions = get_direction( _z.gradient_x, _z.gradient_y ); +// _z.direction_deg = get_direction_deg( _z.gradient_x, _z.gradient_y ); +// suppress_weak_edges( _z, _ir, _params ); +// +// auto subpixels = calc_subpixels( _z, _ir, +// _params.grad_ir_threshold, _params.grad_z_threshold, +// depth_intrinsics.width, depth_intrinsics.height ); +// _z.subpixels_x = subpixels.first; +// _z.subpixels_y = subpixels.second; +// +// _z.closest = get_closest_edges( _z, _ir, depth_intrinsics.width, depth_intrinsics.height ); +// +// calculate_weights( _z ); +// +// auto vertices = subedges2vertices( _z, depth_intrinsics, depth_units ); +//} void set_margin( std::vector& gradient, double margin, @@ -400,10 +400,10 @@ void set_margin( } } template -void valid_by_ir( +void depth_filter( std::vector& filtered, std::vector& origin, - std::vector& valid_edge_by_ir, + std::vector& valid_edge_by_ir, size_t width, size_t height) { @@ -435,7 +435,118 @@ void grid_xy( } } } +template +std::vector interpolation(std::vector const &grid_points, std::vector x[], std::vector y[], int dim, int valid_size, int valid_width) +{ + // interpolation + std::vector local_interp; + auto iedge_it = grid_points.begin();// iEdge + std::vector::iterator loc_reg_x[4]; + std::vector::iterator loc_reg_y[4]; + for (auto i=0;i is_suppressed(std::vector& local_edges, int valid_size) +{ + std::vector is_supressed; + auto loc_edg_it = local_edges.begin(); + for (auto i = 0; i < valid_size; i++) + { + //isSupressed = localEdges(:,3) >= localEdges(:,2) & localEdges(:,3) >= localEdges(:,4); + auto vec2 = *(loc_edg_it + 1); + auto vec3 = *(loc_edg_it + 2); + auto vec4 = *(loc_edg_it + 3); + loc_edg_it += 4; + bool res = (vec3 >= vec2) && (vec3 >= vec4); + is_supressed.push_back(res); + } + return is_supressed; +} +std::vector depth_mean(std::vector& local_x, std::vector& local_y) +{ + std::vector res; + size_t size = local_x.size() / 2; + auto itx = local_x.begin(); + auto ity = local_y.begin(); + for (auto i = 0; i < size; i++, ity += 2, itx += 2) + { + double valy = (*ity + *(ity + 1)) / 2; + double valx = (*itx + *(itx + 1)) / 2; + res.push_back(valy); + res.push_back(valx); + } + + return res; +} +std::vector sum_gradient_depth(std::vector &gradient, std::vector &direction_per_pixel) +{ + std::vector res; + size_t size = direction_per_pixel.size() / 2; + auto it_dir = direction_per_pixel.begin(); + auto it_grad = gradient.begin(); + for (auto i = 0; i < size; i++, it_dir+=2, it_grad+=2) + { + // normalize : res = val/sqrt(row_sum) + auto rorm_dir1 = *it_dir / sqrt(abs(*it_dir) + abs(*(it_dir + 1))); + auto rorm_dir2 = *(it_dir+1) / sqrt(abs(*it_dir) + abs(*(it_dir + 1))); + auto val = abs(*it_grad * rorm_dir1 + *(it_grad + 1) * rorm_dir2); + res.push_back(val); + } + return res; +} +std::vector< double > find_valid_depth_edges(std::vector grad_in_direction, std::vector is_supressed, std::vectorvalues_for_subedges, int threshold) +{ + std::vector< double > res; + //validEdgePixels = zGradInDirection > params.gradZTh & isSupressed & zValuesForSubEdges > 0; + for (int i = 0; i < grad_in_direction.size(); i++) + { + bool cond1 = *(grad_in_direction.begin() + i) > threshold; + bool cond2 = *(is_supressed.begin() + i) == true; + bool cond3 = *(values_for_subedges.begin() + i) > 0; + if (cond1 && cond2 && cond3) + { + res.push_back(1); + } + else + { + res.push_back(0); + } + } + return res; +} +std::vector find_local_values_min(std::vector& local_values) +{ + std::vector res; + size_t size = local_values.size() / 4; + auto it = local_values.begin(); + for (auto i = 0; i < size; i++) + { + auto val1 = *it; + auto val2 = *(it + 1); + auto val3 = *(it + 2); + auto val4 = *(it + 3); + it += 4; + double res_val = std::min(val1, std::min(val2, std::min(val3, val4))); + res.push_back(res_val); + } + return res; +} void optimizer::set_depth_data( std::vector< z_t >&& depth_data, std::vector< ir_t >&& ir_data, @@ -446,25 +557,25 @@ void optimizer::set_depth_data( [iEdge,Ix,Iy] = OnlineCalibration.aux.edgeSobelXY(uint16(frame.i),2); % Added the second input - margin to zero out validEdgePixelsByIR = iEdge>params.gradITh; */ _params.set_depth_resolution(depth_intrinsics.width, depth_intrinsics.height); - _depth.width = depth_intrinsics.width; - _depth.height = depth_intrinsics.height; - _depth.intrinsics = depth_intrinsics; - _depth.depth_units = depth_units; + _z.width = depth_intrinsics.width; + _z.height = depth_intrinsics.height; + _z.intrinsics = depth_intrinsics; + _z.depth_units = depth_units; - _depth.frame = std::move(depth_data); + _z.frame = std::move(depth_data); - _depth.gradient_x = calc_vertical_gradient(_depth.frame, depth_intrinsics.width, depth_intrinsics.height); - _depth.gradient_y = calc_horizontal_gradient(_depth.frame, depth_intrinsics.width, depth_intrinsics.height); + _z.gradient_x = calc_vertical_gradient(_z.frame, depth_intrinsics.width, depth_intrinsics.height); + _z.gradient_y = calc_horizontal_gradient(_z.frame, depth_intrinsics.width, depth_intrinsics.height); _ir.gradient_x = calc_vertical_gradient(_ir.ir_frame, depth_intrinsics.width, depth_intrinsics.height); _ir.gradient_y = calc_horizontal_gradient(_ir.ir_frame, depth_intrinsics.width, depth_intrinsics.height); // set margin of 2 pixels to 0 - set_margin(_depth.gradient_x, 2, _depth.width, _depth.height); - set_margin(_depth.gradient_y, 2, _depth.width, _depth.height); - set_margin(_ir.gradient_x, 2, _depth.width, _depth.height); - set_margin(_ir.gradient_y, 2, _depth.width, _depth.height); + set_margin(_z.gradient_x, 2, _z.width, _z.height); + set_margin(_z.gradient_y, 2, _z.width, _z.height); + set_margin(_ir.gradient_x, 2, _z.width, _z.height); + set_margin(_ir.gradient_y, 2, _z.width, _z.height); - _depth.edges = calc_intensity(_depth.gradient_x, _depth.gradient_y); + _z.edges = calc_intensity(_z.gradient_x, _z.gradient_y); _ir.edges = calc_intensity(_ir.gradient_x, _ir.gradient_y); for (auto it = _ir.edges.begin(); it < _ir.edges.end(); it++) @@ -483,10 +594,10 @@ void optimizer::set_depth_data( sectionMapDepth = OnlineCalibration.aux.sectionPerPixel(params); */ // Get a map for each pixel to its corresponding section - _depth.section_map_depth.resize(_depth.width * _depth.height); + _z.section_map_depth.resize(_z.width * _z.height); size_t const section_w = _params.num_of_sections_for_edge_distribution_x; //% params.numSectionsH size_t const section_h = _params.num_of_sections_for_edge_distribution_y; //% params.numSectionsH - section_per_pixel(_depth, section_w, section_h, _depth.section_map_depth.data()); + section_per_pixel(_z, section_w, section_h, _z.section_map_depth.data()); /*locRC = [gridY(validEdgePixelsByIR),gridX(validEdgePixelsByIR)]; sectionMapValid = sectionMapDepth(validEdgePixelsByIR); @@ -496,19 +607,19 @@ void optimizer::set_depth_data( directionInDeg(directionInDeg<0) = directionInDeg(directionInDeg<0) + 360; [~,directionIndex] = min(abs(directionInDeg - [0:45:315]),[],2); % Quantize the direction to 4 directions (don't care about the sign) */ - + std::vector grid_x; std::vector grid_y; - grid_xy(grid_x, grid_y, _depth.width, _depth.height); + grid_xy(grid_x, grid_y, _z.width, _z.height); - valid_by_ir(_ir.valid_location_rc_x, grid_x, _ir.valid_edge_pixels_by_ir, _depth.width, _depth.height); - valid_by_ir(_ir.valid_location_rc_y, grid_y,_ir.valid_edge_pixels_by_ir, _depth.width, _depth.height); - valid_by_ir(_ir.valid_section_map, _depth.section_map_depth, _ir.valid_edge_pixels_by_ir, _depth.width, _depth.height); - valid_by_ir(_ir.valid_gradient_x, _ir.gradient_x, _ir.valid_edge_pixels_by_ir, _depth.width, _depth.height); - valid_by_ir(_ir.valid_gradient_y, _ir.gradient_y, _ir.valid_edge_pixels_by_ir, _depth.width, _depth.height); + depth_filter(_ir.valid_location_rc_x, grid_x, _ir.valid_edge_pixels_by_ir, _z.width, _z.height); + depth_filter(_ir.valid_location_rc_y, grid_y, _ir.valid_edge_pixels_by_ir, _z.width, _z.height); + depth_filter(_ir.valid_section_map, _z.section_map_depth, _ir.valid_edge_pixels_by_ir, _z.width, _z.height); + depth_filter(_ir.valid_gradient_x, _ir.gradient_x, _ir.valid_edge_pixels_by_ir, _z.width, _z.height); + depth_filter(_ir.valid_gradient_y, _ir.gradient_y, _ir.valid_edge_pixels_by_ir, _z.width, _z.height); //edges: - _ir.edges2 = calc_intensity(_ir.gradient_x, _ir.gradient_y); + //_ir.edges = calc_intensity(_ir.gradient_x, _ir.gradient_y); auto itx = _ir.valid_location_rc_x.begin(); auto ity = _ir.valid_location_rc_y.begin(); for (auto i = 0; i < _ir.valid_location_rc_x.size(); i++) @@ -536,81 +647,236 @@ void optimizer::set_depth_data( locRCsub = locRC + fraqStep.*dirPerPixel;*/ double directions[8][2] = { {0,1},{1,1},{1,0},{1,-1},{0,-1},{-1,-1},{-1,0},{-1,1} }; + std::vector direction_per_pixel_x; //used later when finding valid direction per pixel for (auto i = 0; i < _ir.directions.size(); i++) { int idx = _ir.directions[i]; _ir.direction_per_pixel.push_back(directions[idx][0]); _ir.direction_per_pixel.push_back(directions[idx][1]); + direction_per_pixel_x.push_back(directions[idx][0]); + } + double vec[4] = { -2,-1,0,1 }; // one pixel along gradient direction, 2 pixels against gradient direction + + auto loc_it = _ir.valid_location_rc.begin(); + auto dir_pp_it = _ir.direction_per_pixel.begin(); + + for (auto k = 0; k < 4; k++) + { + for (auto i = 0; i < _ir.direction_per_pixel.size(); i++) + { + double val = *(loc_it + i) + *(dir_pp_it + i) * vec[k]; + _ir.local_region[k].push_back(val); + } + } + for (auto k = 0; k < 4; k++) + { + for (auto i = 0; i < 2 * _ir.valid_location_rc_x.size(); i++) + { + _ir.local_region_y[k].push_back(*(_ir.local_region[k].begin() + i)); + i++; + _ir.local_region_x[k].push_back(*(_ir.local_region[k].begin() + i)); + } + } + // interpolation + _ir.local_edges = interpolation(_ir.edges, _ir.local_region_x, _ir.local_region_y, 4, _ir.valid_location_rc_x.size(), _ir.width); + + // is suppressed + _ir.is_supressed = is_suppressed(_ir.local_edges, _ir.valid_location_rc_x.size()); + + + /*fraqStep = (-0.5*(localEdges(:,4)-localEdges(:,2))./(localEdges(:,4)+localEdges(:,2)-2*localEdges(:,3))); % The step we need to move to reach the subpixel gradient i nthe gradient direction + fraqStep((localEdges(:,4)+localEdges(:,2)-2*localEdges(:,3))==0) = 0; + + locRCsub = locRC + fraqStep.*dirPerPixel; + + % Calculate the Z gradient for thresholding + localZx = squeeze(interp2(Zx,localRegion(:,2,2:3),localRegion(:,1,2:3))); + localZy = squeeze(interp2(Zy,localRegion(:,2,2:3),localRegion(:,1,2:3))); + zGrad = [mean(localZy,2) ,mean(localZx,2)]; + zGradInDirection = abs(sum(zGrad.*normr(dirPerPixel),2)); + % Take the z value of the closest part of the edge + localZvalues = squeeze(interp2(frame.z,localRegion(:,2,:),localRegion(:,1,:))); + + zValuesForSubEdges = min(localZvalues,[],2); + edgeSubPixel = fliplr(locRCsub);% From Row-Col to XY*/ + + std::vector ::iterator loc_edg_it = _ir.local_edges.begin(); + //std::vector ::iterator loc_rc_sub_it = _depth.local_rc_subpixel.begin(); // locRCsub + auto valid_loc_rc = _ir.valid_location_rc.begin(); // locRC + auto dir_per_pixel_it = _ir.direction_per_pixel.begin(); // dirPerPixel + + std::vector edge_sub_pixel_x; + std::vector edge_sub_pixel_y; + + for (auto i = 0; i < _ir.valid_location_rc_x.size(); i++) + { + double vec2 = *(loc_edg_it + 1); + double vec3 = *(loc_edg_it + 2); + double vec4 = *(loc_edg_it + 3); + loc_edg_it += 4; + + double res = double(-0.5 * (vec4 - vec2) / double(vec4 + vec2 - 2 * vec3)); + + if ((vec4 + vec2 - 2 * vec3) == 0) + { + res = 0; + } + _ir.fraq_step.push_back(res); + auto valx = *valid_loc_rc + *dir_per_pixel_it * res; + valid_loc_rc++; + dir_per_pixel_it++; + auto valy = *valid_loc_rc + *dir_per_pixel_it * res; + valid_loc_rc++; + dir_per_pixel_it++; + _z.local_rc_subpixel.push_back(valx); + _z.local_rc_subpixel.push_back(valy); + + _z.edge_sub_pixel.push_back(valy); + _z.edge_sub_pixel.push_back(valx); + edge_sub_pixel_x.push_back(valy); + edge_sub_pixel_y.push_back(valx); + } + + std::vector local_region_x[2] = { _ir.local_region_x[1] ,_ir.local_region_x[2] }; + std::vector local_region_y[2] = { _ir.local_region_y[1] ,_ir.local_region_y[2] }; + _z.local_x = interpolation(_z.gradient_x, local_region_x, local_region_y, 2, _ir.valid_location_rc_x.size(), _z.width); + _z.local_y = interpolation(_z.gradient_y, local_region_x, local_region_y, 2, _ir.valid_location_rc_x.size(), _z.width); + _z.gradient = depth_mean(_z.local_x, _z.local_y); + _z.grad_in_direction = sum_gradient_depth(_z.gradient, _ir.direction_per_pixel); + _z.local_values = interpolation(_z.frame, _ir.local_region_x, _ir.local_region_y, 4, _ir.valid_location_rc_x.size(), _z.width); + _z.values_for_subedges = find_local_values_min(_z.local_values); + + //_params.alpha; + /* validEdgePixels = zGradInDirection > params.gradZTh & isSupressed & zValuesForSubEdges > 0; + + zGradInDirection = zGradInDirection(validEdgePixels); + edgeSubPixel = edgeSubPixel(validEdgePixels,:); + zValuesForSubEdges = zValuesForSubEdges(validEdgePixels); + dirPerPixel = dirPerPixel(validEdgePixels); + sectionMapDepth = sectionMapValid(validEdgePixels); + directionIndex = directionIndex(validEdgePixels); + directionIndex(directionIndex>4) = directionIndex(directionIndex>4)-4;% Like taking abosoulte value on the direction + */ + _z.supressed_edges = find_valid_depth_edges(_z.grad_in_direction, _ir.is_supressed, _z.values_for_subedges, _params.grad_z_threshold); + std::vector valid_grad_in_direction; + std::vector valid_values_for_subedges; + + + + depth_filter(valid_grad_in_direction, _z.grad_in_direction, _z.supressed_edges, 1, _z.supressed_edges.size()); + depth_filter(_z.valid_edge_sub_pixel_x, edge_sub_pixel_x, _z.supressed_edges, 1, _z.supressed_edges.size()); //edgeSubPixel = edgeSubPixel(validEdgePixels,:); + depth_filter(_z.valid_edge_sub_pixel_y, edge_sub_pixel_y, _z.supressed_edges, 1, _z.supressed_edges.size()); + for (auto i = 0; i < _z.valid_edge_sub_pixel_x.size(); i++) + { + _z.valid_edge_sub_pixel.push_back(*(_z.valid_edge_sub_pixel_x.begin() + i)); + _z.valid_edge_sub_pixel.push_back(*(_z.valid_edge_sub_pixel_y.begin() + i)); + // subPoints : subPoints = [xim,yim,ones(size(yim))]; + _z.sub_points.push_back(*(_z.valid_edge_sub_pixel_x.begin() + i)-1); + _z.sub_points.push_back(*(_z.valid_edge_sub_pixel_y.begin() + i)-1); + _z.sub_points.push_back(1); + } + depth_filter(valid_values_for_subedges, _z.values_for_subedges, _z.supressed_edges, 1, _z.supressed_edges.size()); + depth_filter(_z.valid_direction_per_pixel, direction_per_pixel_x, _z.supressed_edges, 1, _z.supressed_edges.size()); + depth_filter(_z.valid_section_map, _ir.valid_section_map, _z.supressed_edges, 1, _z.supressed_edges.size()); + std::vector edited_ir_directions; + + for (auto i = 0; i < _ir.directions.size(); i++) + { + auto val = double(*(_ir.directions.begin() + i)); + val = val + 1;// +1 to align with matlab + val = val > 4 ? val - 4 : val; + edited_ir_directions.push_back(val); + } + depth_filter(_z.valid_directions, edited_ir_directions, _z.supressed_edges, 1, _z.supressed_edges.size()); + + _z.grad_in_direction = valid_grad_in_direction; + _z.values_for_subedges = valid_values_for_subedges; + + /* weights = min(max(zGradInDirection - params.gradZTh,0),params.gradZMax - params.gradZTh); + if params.constantWeights + weights(:) = params.constantWeightsValue; + end + xim = edgeSubPixel(:,1)-1; + yim = edgeSubPixel(:,2)-1; + + subPoints = [xim,yim,ones(size(yim))]; + vertices = subPoints*(pinv(params.Kdepth)').*zValuesForSubEdges/single(params.zMaxSubMM); + + [uv,~,~] = OnlineCalibration.aux.projectVToRGB(vertices,params.rgbPmat,params.Krgb,params.rgbDistort); + isInside = OnlineCalibration.aux.isInsideImage(uv,params.rgbRes); + + xim = xim(isInside); + yim = yim(isInside); + zValuesForSubEdges = zValuesForSubEdges(isInside); + zGradInDirection = zGradInDirection(isInside); + directionIndex = directionIndex(isInside); + weights = weights(isInside); + vertices = vertices(isInside,:); + sectionMapDepth = sectionMapDepth(isInside);*/ + _params.constant_weights; + transform(_z.valid_edge_sub_pixel_x.begin(), _z.valid_edge_sub_pixel_x.end(), _z.valid_edge_sub_pixel_x.begin(), bind2nd(std::plus(), -1.0)); + transform(_z.valid_edge_sub_pixel_y.begin(), _z.valid_edge_sub_pixel_y.end(), _z.valid_edge_sub_pixel_y.begin(), bind2nd(std::plus(), -1.0)); + for (auto i = 0; i < _z.sub_points.size(); i += 3) + { + double sub_points_mult[3] = { 0,0,0 }; + double vec1 = *(_z.sub_points.begin() + i); + double vec2 = *(_z.sub_points.begin() + i + 1); + double vec3 = *(_z.sub_points.begin() + i + 2); + for (auto jj = 0; jj < 3; jj++) + { + sub_points_mult[jj] = vec1 * _params.k_depth_pinv_trans[0][jj] + vec2 * _params.k_depth_pinv_trans[1][jj] + vec3 * _params.k_depth_pinv_trans[2][jj]; + } + + auto val1 = sub_points_mult[0] * *(_z.values_for_subedges.begin() + i / 3) / _params.max_sub_mm_z; + auto val2 = sub_points_mult[1] * *(_z.values_for_subedges.begin() + i / 3) / _params.max_sub_mm_z; + auto val3 = sub_points_mult[2] * *(_z.values_for_subedges.begin() + i / 3) / _params.max_sub_mm_z; + _z.vertices_all.push_back({val1, val2, val3}); + } + + _z.uvmap = get_texture_map(_z.vertices_all, _original_calibration); + // + /*function isInside = isInsideImage(xy,res) + isInside = xy(:,1) >= 0 & ... + xy(:,1) <= res(2)-1 & ... + xy(:,2) >= 0 & ... + xy(:,2) <= res(1)-1; +end*/ + for (auto i = 0; i < _z.uvmap.size(); i++) + { + bool cond_x = (_z.uvmap[i].x >= 0) && (_z.uvmap[i].x <= _yuy.height-1); + bool cond_y = (_z.uvmap[i].y >= 0) && (_z.uvmap[i].y <= _yuy.width-1); + double res = 0; + + if (cond_x && cond_y) + { + res = 1; + } + _z.is_inside.push_back(res); } - double vec[4] = {-2,-1,0,1}; // one pixel along gradient direction, 2 pixels against gradient direction - - auto loc_it = _ir.valid_location_rc.begin(); - auto dir_pp_it = _ir.direction_per_pixel.begin(); - - for (auto k = 0; k < 4; k++) - { - for (auto i = 0; i < _ir.direction_per_pixel.size(); i++) - { - double val = *(loc_it + i) +*(dir_pp_it + i) * vec[k]; - _ir.local_region[k].push_back(val); - } - } - - // interpolation - for debug - for (auto k = 0; k < 4; k++) - { - for (auto i = 0; i < 2*_ir.valid_location_rc_x.size(); i++) - { - _ir.local_region_y[k].push_back(*(_ir.local_region[k].begin()+i)); - i++; - _ir.local_region_x[k].push_back(*(_ir.local_region[k].begin() + i)); - } - } - - auto iedge_it = _ir.edges2.begin();// iEdge - std::vector::iterator loc_reg_x[4] = { _ir.local_region_x[0].begin() ,_ir.local_region_x[1].begin() ,_ir.local_region_x[2].begin() ,_ir.local_region_x[3].begin() }; - std::vector::iterator loc_reg_y[4] = { _ir.local_region_y[0].begin() ,_ir.local_region_y[1].begin(),_ir.local_region_y[2].begin() ,_ir.local_region_y[3].begin() }; - - for (auto i = 0; i < _ir.valid_location_rc_x.size(); i++) - { - for (auto k = 0; k < 4; k++) - { - auto idx = *(loc_reg_x[k]+i) - 1; - auto idy = *(loc_reg_y[k]+i) - 1; - assert(_ir.width * idy + idx <= _ir.width * _ir.height); - auto val = *(iedge_it + _ir.width * idy + idx);// find value in iEdge - _ir.local_edges.push_back(val); - } - } - - auto loc_edg_it = _ir.local_edges.begin(); - for (auto i = 0; i < _ir.valid_location_rc_x.size(); i++) - { - //isSupressed = localEdges(:,3) >= localEdges(:,2) & localEdges(:,3) >= localEdges(:,4); - auto vec2 = *(loc_edg_it+1); - auto vec3 = *(loc_edg_it+2); - auto vec4 = *(loc_edg_it+3); - loc_edg_it += 4; - bool res = (vec3 >= vec2) && (vec3 >= vec4); - _ir.is_supressed.push_back(res); - } - // old code : - /* - suppress_weak_edges(_depth, _ir, _params); - - auto subpixels = calc_subpixels(_depth, _ir, - _params.grad_ir_threshold, _params.grad_z_threshold, - depth_intrinsics.width, depth_intrinsics.height); - _depth.subpixels_x = subpixels.first; - _depth.subpixels_y = subpixels.second; - - _depth.closest = get_closest_edges(_depth, _ir, depth_intrinsics.width, depth_intrinsics.height); - - calculate_weights(_depth); - - auto vertices = subedges2vertices(_depth, depth_intrinsics, depth_units);*/ + + /*xim = xim(isInside); + yim = yim(isInside); + zValuesForSubEdges = zValuesForSubEdges(isInside); + zGradInDirection = zGradInDirection(isInside); + directionIndex = directionIndex(isInside); + weights = weights(isInside); + vertices = vertices(isInside,:); + sectionMapDepth = sectionMapDepth(isInside);*/ + depth_filter(_z.subpixels_x, _z.valid_edge_sub_pixel_x, _z.is_inside, 1, _z.is_inside.size()); + depth_filter(_z.subpixels_y, _z.valid_edge_sub_pixel_y, _z.is_inside, 1, _z.is_inside.size()); + depth_filter(_z.closest, _z.values_for_subedges, _z.is_inside, 1, _z.is_inside.size()); + depth_filter(_z.grad_in_direction_inside, _z.grad_in_direction, _z.is_inside, 1, _z.is_inside.size()); + depth_filter(_z.directions, _z.valid_directions, _z.is_inside, 1, _z.is_inside.size()); + depth_filter(_z.vertices, _z.vertices_all, _z.is_inside, 1, _z.is_inside.size()); + depth_filter(_z.section_map_depth_inside, _z.section_map_depth, _z.is_inside, 1, _z.is_inside.size()); + + for (auto i = 0; i < _z.is_inside.size(); i++) { + _z.weights.push_back(_params.constant_weights); + } + } + void optimizer::set_yuy_data( std::vector< yuy_t > && yuy_data, std::vector< yuy_t > && prev_yuy_data, @@ -649,15 +915,15 @@ void optimizer::set_ir_data( _ir.height = height; _ir.ir_frame = std::move( ir_data ); - _ir.ir_edges = calc_edges( _ir.ir_frame, width, height ); + _ir.edges = calc_edges( _ir.ir_frame, width, height ); } void optimizer::zero_invalid_edges( z_frame_data & z_data, ir_frame_data const & ir_data ) { - for( auto i = 0; i < ir_data.ir_edges.size(); i++ ) + for( auto i = 0; i < ir_data.edges.size(); i++ ) { - if( ir_data.ir_edges[i] <= _params.grad_ir_threshold || z_data.edges[i] <= _params.grad_z_threshold ) + if( ir_data.edges[i] <= _params.grad_ir_threshold || z_data.edges[i] <= _params.grad_z_threshold ) { z_data.supressed_edges[i] = 0; z_data.subpixels_x[i] = 0; @@ -713,49 +979,49 @@ std::vector< direction > optimizer::get_direction2(std::vector gradient_ } return res; } -std::vector< uint16_t > optimizer::get_closest_edges( - z_frame_data const & z_data, - ir_frame_data const & ir_data, - size_t width, size_t height ) -{ - std::vector< uint16_t > z_closest; - z_closest.reserve( z_data.edges.size() ); - - for( auto i = 0; i < int(height); i++ ) - { - for( auto j = 0; j < int(width); j++ ) - { - auto idx = i * width + j; - - auto edge = z_data.edges[idx]; - - //if (edge == 0) continue; - - auto edge_prev_idx = get_prev_index( z_data.directions[idx], i, j, width, height ); - - auto edge_next_idx = get_next_index( z_data.directions[idx], i, j, width, height ); - - auto edge_minus_idx = edge_prev_idx.second * width + edge_prev_idx.first; - - auto edge_plus_idx = edge_next_idx.second * width + edge_next_idx.first; - - auto z_edge_plus = z_data.edges[edge_plus_idx]; - auto z_edge = z_data.edges[idx]; - auto z_edge_minus = z_data.edges[edge_minus_idx]; - - - if (z_data.supressed_edges[idx]) - { - z_closest.push_back(std::min(z_data.frame[edge_minus_idx], z_data.frame[edge_plus_idx])); - } - else - { - z_closest.push_back(0); - } - } - } - return z_closest; -} +//std::vector< uint16_t > optimizer::get_closest_edges( +// z_frame_data const & z_data, +// ir_frame_data const & ir_data, +// size_t width, size_t height ) +//{ +// std::vector< uint16_t > z_closest; +// z_closest.reserve( z_data.edges.size() ); +// +// for( auto i = 0; i < int(height); i++ ) +// { +// for( auto j = 0; j < int(width); j++ ) +// { +// auto idx = i * width + j; +// +// auto edge = z_data.edges[idx]; +// +// //if (edge == 0) continue; +// +// auto edge_prev_idx = get_prev_index( z_data.valid_directions[idx], i, j, width, height ); +// +// auto edge_next_idx = get_next_index( z_data.valid_directions[idx], i, j, width, height ); +// +// auto edge_minus_idx = edge_prev_idx.second * width + edge_prev_idx.first; +// +// auto edge_plus_idx = edge_next_idx.second * width + edge_next_idx.first; +// +// auto z_edge_plus = z_data.edges[edge_plus_idx]; +// auto z_edge = z_data.edges[idx]; +// auto z_edge_minus = z_data.edges[edge_minus_idx]; +// +// +// if (z_data.supressed_edges[idx]) +// { +// z_closest.push_back(std::min(z_data.frame[edge_minus_idx], z_data.frame[edge_plus_idx])); +// } +// else +// { +// z_closest.push_back(0); +// } +// } +// } +// return z_closest; +//} /* Given pixel coordinates and depth in an image with no distortion or inverse distortion coefficients, compute the corresponding point in 3D space relative to the same camera */ static void deproject_pixel_to_point(double point[3], const struct rs2_intrinsics_double * intrin, const double pixel[2], double depth) @@ -897,20 +1163,20 @@ double get_min(double x, double y) return x < y ? x : y; } -std::vector optimizer::calculate_weights(z_frame_data& z_data) -{ - std::vector res; - - for (auto i = 0; i < z_data.supressed_edges.size(); i++) - { - if (z_data.supressed_edges[i]) - z_data.weights.push_back( - get_min(get_max(z_data.supressed_edges[i] - _params.grad_z_min, (double)0), - _params.grad_z_max - _params.grad_z_min)); - } - - return res; -} +//std::vector optimizer::calculate_weights(z_frame_data& z_data) +//{ +// std::vector res; +// +// for (auto i = 0; i < z_data.supressed_edges.size(); i++) +// { +// if (z_data.supressed_edges[i]) +// z_data.weights.push_back( +// get_min(get_max(z_data.supressed_edges[i] - _params.grad_z_min, (double)0), +// _params.grad_z_max - _params.grad_z_min)); +// } +// +// return res; +//} void deproject_sub_pixel( std::vector& points, @@ -918,7 +1184,7 @@ void deproject_sub_pixel( std::vector< double > const & edges, const double* x, const double* y, - const uint16_t* depth, double depth_units + const double* depth, double depth_units ) { auto ptr = (double*)points.data(); @@ -938,7 +1204,7 @@ std::vector optimizer::subedges2vertices(z_frame_data& z_data, const rs { std::vector res(z_data.n_strong_edges); deproject_sub_pixel(res, intrin, z_data.supressed_edges, z_data.subpixels_x.data(), z_data.subpixels_y.data(), z_data.closest.data(), depth_units); - z_data.vertices = res; + z_data.vertices_all = res; return res; } @@ -1105,11 +1371,11 @@ std::pair< std::vector, std::vector> calc_rc( const calib& curr_calib ) { - auto v = z_data.vertices; + auto v = z_data.vertices_all; - std::vector f1( z_data.vertices.size() ); - std::vector r2( z_data.vertices.size() ); - std::vector rc( z_data.vertices.size() ); + std::vector f1( z_data.vertices_all.size() ); + std::vector r2( z_data.vertices_all.size() ); + std::vector rc( z_data.vertices_all.size() ); auto yuy_intrin = curr_calib.get_intrinsics(); auto yuy_extrin = curr_calib.get_extrinsics(); @@ -1127,7 +1393,7 @@ std::pair< std::vector, std::vector> calc_rc( fy*(double)r[1] + ppy * (double)r[2], fy*(double)r[4] + ppy * (double)r[5], fy*(double)r[7] + ppy * (double)r[8], fy*(double)t[1] + ppy * (double)t[2], r[2], r[5], r[8], t[2] }; - for( auto i = 0; i < z_data.vertices.size(); ++i ) + for( auto i = 0; i < z_data.vertices_all.size(); ++i ) { double x = v[i].x; double y = v[i].y; @@ -1203,7 +1469,7 @@ std::pair calc_cost_and_grad( iteration_data_collect * data = nullptr ) { - auto uvmap = get_texture_map(z_data.vertices, curr_calib); + auto uvmap = get_texture_map(z_data.vertices_all, curr_calib); if( data ) data->uvmap = uvmap; @@ -1400,10 +1666,10 @@ optimaization_params optimizer::back_tracking_line_search( const z_frame_data & new_params.curr_calib.rot = extract_rotation_from_angles( new_params.curr_calib.rot_angles ); new_params.curr_calib.calc_p_mat(); - auto uvmap = get_texture_map( z_data.vertices, curr_params.curr_calib ); + auto uvmap = get_texture_map( z_data.vertices_all, curr_params.curr_calib ); curr_params.cost = calc_cost( z_data, yuy_data, uvmap ); - uvmap = get_texture_map( z_data.vertices, new_params.curr_calib ); + uvmap = get_texture_map( z_data.vertices_all, new_params.curr_calib ); new_params.cost = calc_cost( z_data, yuy_data, uvmap ); auto iter_count = 0; @@ -1416,7 +1682,7 @@ optimaization_params optimizer::back_tracking_line_search( const z_frame_data & new_params.curr_calib.rot = extract_rotation_from_angles( new_params.curr_calib.rot_angles ); new_params.curr_calib.calc_p_mat(); - uvmap = get_texture_map( z_data.vertices, new_params.curr_calib ); + uvmap = get_texture_map( z_data.vertices_all, new_params.curr_calib ); new_params.cost = calc_cost( z_data, yuy_data, uvmap ); } diff --git a/src/algo/depth-to-rgb-calibration/optimizer.h b/src/algo/depth-to-rgb-calibration/optimizer.h index f572366b92..3a997a2aa3 100644 --- a/src/algo/depth-to-rgb-calibration/optimizer.h +++ b/src/algo/depth-to-rgb-calibration/optimizer.h @@ -29,9 +29,9 @@ namespace depth_to_rgb_calibration { struct params { params(); - - void set_depth_resolution( size_t width, size_t height ); - void set_rgb_resolution( size_t width, size_t height ); + + void set_depth_resolution(size_t width, size_t height); + void set_rgb_resolution(size_t width, size_t height); double gamma = 0.9; double alpha = (double)1 / (double)3; @@ -63,6 +63,17 @@ namespace depth_to_rgb_calibration { double move_thresh_pix_val = 20; double move_threshold_pix_num = 62.2080; + //smearing + double max_sub_mm_z = 4; + double constant_weights = 1000; + double k_depth[3][3] = { {731.27344,0,529.27344 },{0,731.97656,402.32031},{0,0,1} }; + //double k_depth_pinv_trans[3][3] = { { 0.0013674778, -1.1641532e-10, 2.3283064e-10 }, //pinv(params.Kdepth)'; + // { 8.7311491e-11,0.0013661643 ,-2.9103830e-11}, + // { -0.72376943, -0.54963547,0.99999988 } }; + + double k_depth_pinv_trans[3][3] = { { 0.0013674775381131,5.42101086242752e-20,1.0842021724855e-19 }, //pinv(params.Kdepth)'; + { 2.16840434497101e-19,0.00136616396102163,-2.71050543121376e-19}, + { -0.723769537301155,-0.549635511724462,1 } }; // output validation double const max_xy_movement_per_calibration[3] = { 10, 2, 2 }; double const max_xy_movement_from_origin = 20; @@ -96,10 +107,10 @@ namespace depth_to_rgb_calibration { void set_ir_data( std::vector< ir_t > && ir_data, size_t width, size_t height ); - void set_z_data( + /*void set_z_data( std::vector< z_t > && z_data, rs2_intrinsics_double const & depth_intrinsics, - float depth_units ); + float depth_units );*/ void set_depth_data( std::vector< z_t >&& z_data, std::vector< ir_t >&& ir_data, @@ -130,7 +141,7 @@ namespace depth_to_rgb_calibration { z_frame_data const & get_z_data() const { return _z; } yuy2_frame_data const & get_yuy_data() const { return _yuy; } ir_frame_data const & get_ir_data() const { return _ir; } - z_frame_data const& get_depth_data() const { return _depth; } + z_frame_data const& get_depth_data() const { return _z; } // impl private: void zero_invalid_edges( z_frame_data& z_data, ir_frame_data const & ir_data ); @@ -141,7 +152,7 @@ namespace depth_to_rgb_calibration { std::vector get_luminance_from_yuy2( std::vector yuy2_imagh ); std::vector get_logic_edges( std::vector edges ); - std::vector calculate_weights( z_frame_data& z_data ); + //std::vector calculate_weights( z_frame_data& z_data ); std::vector subedges2vertices(z_frame_data& z_data, const rs2_intrinsics_double& intrin, double depth_units); optimaization_params back_tracking_line_search( const z_frame_data & z_data, const yuy2_frame_data& yuy_data, optimaization_params opt_params ); @@ -167,8 +178,8 @@ namespace depth_to_rgb_calibration { params _params; yuy2_frame_data _yuy; ir_frame_data _ir; + //z_frame_data _z; z_frame_data _z; - z_frame_data _depth; calib _original_calibration; // starting state of auto-calibration calib _factory_calibration; // factory default calibration of the camera optimaization_params _params_curr; // last-known setting diff --git a/src/algo/depth-to-rgb-calibration/valid-results.cpp b/src/algo/depth-to-rgb-calibration/valid-results.cpp index bf9b2ee71e..d026be72bb 100644 --- a/src/algo/depth-to-rgb-calibration/valid-results.cpp +++ b/src/algo/depth-to-rgb-calibration/valid-results.cpp @@ -13,8 +13,8 @@ double optimizer::calc_correction_in_pixels( calib const & from_calibration ) co { //% [uvMap,~,~] = OnlineCalibration.aux.projectVToRGB(frame.vertices,params.rgbPmat,params.Krgb,params.rgbDistort); //% [uvMapNew,~,~] = OnlineCalibration.aux.projectVToRGB(frame.vertices,newParams.rgbPmat,newParams.Krgb,newParams.rgbDistort); - auto old_uvmap = get_texture_map( _z.vertices, from_calibration ); - auto new_uvmap = get_texture_map( _z.vertices, _params_curr.curr_calib ); + auto old_uvmap = get_texture_map( _z.vertices_all, from_calibration ); + auto new_uvmap = get_texture_map( _z.vertices_all, _params_curr.curr_calib ); if( old_uvmap.size() != new_uvmap.size() ) throw std::runtime_error( to_string() << "did not expect different uvmap sizes (" << old_uvmap.size() << " vs " << new_uvmap.size() << ")" ); // uvmap is Nx[x,y] @@ -85,8 +85,8 @@ std::vector< double > optimizer::cost_per_section_diff(calib const & old_calib, if (_z.section_map.size() != _z.weights.size()) throw std::runtime_error("section_map has not been initialized"); - auto uvmap_old = get_texture_map(_z.vertices, old_calib); - auto uvmap_new = get_texture_map(_z.vertices, new_calib); + auto uvmap_old = get_texture_map(_z.vertices_all, old_calib); + auto uvmap_new = get_texture_map(_z.vertices_all, new_calib); size_t const n_sections_x = _params.num_of_sections_for_edge_distribution_x; size_t const n_sections_y = _params.num_of_sections_for_edge_distribution_y; diff --git a/src/algo/depth-to-rgb-calibration/valid-scene.cpp b/src/algo/depth-to-rgb-calibration/valid-scene.cpp index 6c5214c428..50b17fea64 100644 --- a/src/algo/depth-to-rgb-calibration/valid-scene.cpp +++ b/src/algo/depth-to-rgb-calibration/valid-scene.cpp @@ -386,12 +386,12 @@ weightsPerDir = [sum(weightIm(frame.dirI == 1));sum(weightIm(frame.dirI == 2));s } } std::vector weights_per_dir(deg_none); // deg_non is number of directions - auto directions_iter = z_data.directions.begin(); + auto directions_iter = z_data.valid_directions.begin(); auto weights_per_dir_iter = weights_per_dir.begin(); for (auto i = 0; i < deg_none; ++i) { *(weights_per_dir_iter + i) = 0; // init sum per direction - for (auto ii = 0; ii < z_data.directions.size(); ++ii) // directions size = z_data size = weights_im size + for (auto ii = 0; ii < z_data.valid_directions.size(); ++ii) // directions size = z_data size = weights_im size { if (*(directions_iter + ii) == i) { @@ -702,13 +702,14 @@ bool optimizer::is_scene_valid() // remove pixels in section map that were removed in weights AC_LOG(DEBUG, "... " << _z.supressed_edges.size() << " total edges"); - for (auto i = 0; i < _z.supressed_edges.size(); i++) + /*for (auto i = 0; i < _z.supressed_edges.size(); i++) { if (_z.supressed_edges[i]) { _z.section_map.push_back(section_map_depth[i]); } - } + }*/ + _z.section_map = _z.section_map_depth_inside; // NOHA :: taken from preprocessDepth AC_LOG(DEBUG, "... " << _z.section_map.size() << " not suppressed"); // remove pixels in section map where edges_IDT > 0 diff --git a/src/depth-to-rgb-calibration.cpp b/src/depth-to-rgb-calibration.cpp index f50da3c80f..31f72283de 100644 --- a/src/depth-to-rgb-calibration.cpp +++ b/src/depth-to-rgb-calibration.cpp @@ -70,8 +70,9 @@ depth_to_rgb_calibration::depth_to_rgb_calibration( AC_LOG( DEBUG, "... setting z data" ); auto z_profile = depth.get_profile().as< rs2::video_stream_profile >(); auto z_data = (impl::z_t const *) depth.get_data(); - _algo.set_z_data( + _algo.set_depth_data( std::vector< impl::z_t >( z_data, z_data + depth.get_data_size() / sizeof( impl::z_t ) ), + std::vector< impl::ir_t >(ir_data, ir_data + ir.get_data_size() / sizeof(impl::ir_t)), z_profile.get_intrinsics(), depth.as< rs2::depth_frame >().get_units() * 1000.f // same scaling as for extrinsics! ); diff --git a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h index 3df131308d..399aadb5f1 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h +++ b/unit-tests/algo/depth-to-rgb-calibration/compare-scene.h @@ -29,13 +29,14 @@ void compare_scene( std::string const & scene_dir ) auto z_w = ci.z.width; auto num_of_calib_elements = 32; +#if 0 CHECK( compare_to_bin_file< double >( yuy_data.edges, scene_dir, "YUY2_edge", rgb_w, rgb_h, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< double >( yuy_data.edges_IDT, scene_dir, "YUY2_IDT", rgb_w, rgb_h, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< double >( yuy_data.edges_IDTx, scene_dir, "YUY2_IDTx", rgb_w, rgb_h, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< double >( yuy_data.edges_IDTy, scene_dir, "YUY2_IDTy", rgb_w, rgb_h, "double_00", compare_same_vectors ) ); //--- - CHECK( compare_to_bin_file< double >( ir_data.ir_edges, scene_dir, "I_edge", z_w, z_h, "double_00", compare_same_vectors ) ); + CHECK( compare_to_bin_file< double >( ir_data.edges, scene_dir, "I_edge", z_w, z_h, "double_00", compare_same_vectors ) ); //--- CHECK( compare_to_bin_file< double >( z_data.edges, scene_dir, "Z_edge", z_w, z_h, "double_00", compare_same_vectors ) ); @@ -48,45 +49,74 @@ void compare_scene( std::string const & scene_dir ) CHECK( compare_to_bin_file< double >( z_data.weights, scene_dir, "weightsT", 1, md.n_edges, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< double >( z_data.closest, scene_dir, "Z_valuesForSubEdges", z_w, z_h, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< algo::double3 >( z_data.vertices, scene_dir, bin_file( "vertices", 3, md.n_edges, "double_00" ) + ".bin", md.n_edges, 1, compare_same_vectors ) ); - -#if 0 +#endif +#if 1 // smearing - CHECK( compare_to_bin_file< double >( depth_data.gradient_x, scene_dir, "Zx", z_w, z_h, "double_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< double >( depth_data.gradient_y, scene_dir, "Zy", z_w, z_h, "double_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< double >( ir_data.gradient_x, scene_dir, "Ix", z_w, z_h, "double_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< double >( ir_data.gradient_y, scene_dir, "Iy", z_w, z_h, "double_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< double >( ir_data.edges2, scene_dir, "iEdge", z_w, z_h, "double_00", compare_same_vectors ) ); - //CHECK(compare_to_bin_file< double >(depth_data.edges2, scene_dir, "zedge", z_w, z_h, "double_00", compare_same_vectors)); - CHECK( compare_to_bin_file< uint8_t >( depth_data.section_map_depth, scene_dir, "sectionMapDepth", z_w, z_h, "uint8_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< uint8_t >( ir_data.valid_edge_pixels_by_ir, scene_dir, "validEdgePixelsByIR", z_w, z_h, "uint8_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< double >( ir_data.valid_location_rc_x, scene_dir, "gridXValid", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.valid_location_rc_y, scene_dir, "gridYValid", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.valid_location_rc, scene_dir, "locRC", 2, 105794, "double_00" ), 105794, 2, compare_same_vectors )); - CHECK( compare_to_bin_file< uint8_t >( ir_data.valid_section_map, scene_dir, "sectionMapValid", 1, 105794, "uint8_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.valid_gradient_x, scene_dir, "IxValid", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.valid_gradient_y, scene_dir, "IyValid", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.direction_deg, scene_dir, "directionInDeg", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - //CHECK(compare_to_bin_file< double >(ir_data.directions, scene_dir, "directionIndex", 1, 105794, "double_00"), 105794, 1, compare_same_vectors)); // it passed, depends on index definition - CHECK( compare_to_bin_file< double >( ir_data.direction_per_pixel, scene_dir, "dirPerPixel", 2, 105794, "double_00" ), 105794, 2, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region[0], scene_dir, "localRegion", 2, 105794, "double_00" ), 105794, 2, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region[1], scene_dir, "localRegion", 2, 105794, "double_01" ), 105794, 2, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region[2], scene_dir, "localRegion", 2, 105794, "double_02" ), 105794, 2, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region[3], scene_dir, "localRegion", 2, 105794, "double_03" ), 105794, 2, compare_same_vectors )); - - CHECK( compare_to_bin_file< double >( ir_data.local_region_x[0], scene_dir, "localRegion_x", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region_x[1], scene_dir, "localRegion_x", 1, 105794, "double_01" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region_x[2], scene_dir, "localRegion_x", 1, 105794, "double_02" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region_x[3], scene_dir, "localRegion_x", 1, 105794, "double_03" ), 105794, 1, compare_same_vectors )); - - CHECK( compare_to_bin_file< double >( ir_data.local_region_y[0], scene_dir, "localRegion_y", 1, 105794, "double_00" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region_y[1], scene_dir, "localRegion_y", 1, 105794, "double_01" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region_y[2], scene_dir, "localRegion_y", 1, 105794, "double_02" ), 105794, 1, compare_same_vectors )); - CHECK( compare_to_bin_file< double >( ir_data.local_region_y[3], scene_dir, "localRegion_y", 1, 105794, "double_03" ), 105794, 1, compare_same_vectors )); - - CHECK( compare_to_bin_file< double >( ir_data.local_edges, scene_dir, "localEdges", 4, 105794, "double_00" ), 105794, 4, compare_same_vectors )); - CHECK( compare_to_bin_file< uint8_t >( ir_data.is_supressed, scene_dir, "isSupressed", 1, 105794, "uint8_00" ), 105794, 1, compare_same_vectors )); + CHECK(compare_to_bin_file< double >(depth_data.gradient_x, scene_dir, "Zx", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.gradient_y, scene_dir, "Zy", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.gradient_x, scene_dir, "Ix", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.gradient_y, scene_dir, "Iy", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.edges, scene_dir, "iEdge", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.edges, scene_dir, "zedge", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< uint8_t >(depth_data.section_map_depth, scene_dir, "sectionMapDepth", z_w, z_h, "uint8_00",compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.valid_edge_pixels_by_ir, scene_dir, "validEdgePixelsByIR", z_w, z_h, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.valid_location_rc_x, scene_dir, "gridXValid", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.valid_location_rc_y, scene_dir, "gridYValid", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.valid_location_rc, scene_dir, "locRC", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< uint8_t >(ir_data.valid_section_map, scene_dir, "sectionMapValid", 1, 105794, "uint8_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.valid_gradient_x, scene_dir, "IxValid", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.valid_gradient_y, scene_dir, "IyValid", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.direction_deg, scene_dir, "directionInDeg", 1, 105794, "double_00", compare_same_vectors)); + //CHECK(compare_to_bin_file< double >(ir_data.directions, scene_dir, "directionIndex", 105794, 1,"double_00", compare_same_vectors)); // it passed, depends on index definition + CHECK(compare_to_bin_file< double >(ir_data.direction_per_pixel, scene_dir, "dirPerPixel", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.valid_direction_per_pixel, scene_dir, "validdirPerPixel", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region[0], scene_dir, "localRegion", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region[1], scene_dir, "localRegion", 2, 105794, "double_01", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region[2], scene_dir, "localRegion", 2, 105794, "double_02", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region[3], scene_dir, "localRegion", 2, 105794, "double_03", compare_same_vectors)); + + CHECK(compare_to_bin_file< double >(ir_data.local_region_x[0], scene_dir, "localRegion_x", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region_x[1], scene_dir, "localRegion_x", 1, 105794, "double_01", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region_x[2], scene_dir, "localRegion_x", 1, 105794, "double_02", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region_x[3], scene_dir, "localRegion_x", 1, 105794, "double_03", compare_same_vectors)); + + CHECK(compare_to_bin_file< double >(ir_data.local_region_y[0], scene_dir, "localRegion_y", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region_y[1], scene_dir, "localRegion_y", 1, 105794, "double_01", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region_y[2], scene_dir, "localRegion_y", 1, 105794, "double_02", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.local_region_y[3], scene_dir, "localRegion_y", 1, 105794, "double_03", compare_same_vectors)); + + CHECK(compare_to_bin_file< double >(ir_data.local_edges, scene_dir, "localEdges", 4, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< uint8_t >(ir_data.is_supressed, scene_dir, "isSupressed", 1, 105794, "uint8_00", compare_same_vectors)); + //CHECK(compare_to_bin_file< double >(ir_data.fraq_step, scene_dir, "fraqStep2", 1, 105794, "single_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(ir_data.fraq_step, scene_dir, "fraqStep", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.local_rc_subpixel, scene_dir, "locRCsub", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.local_x, scene_dir, "localZx", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.local_y, scene_dir, "localZy", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.gradient, scene_dir, "zGrad", 2, 105794, "double_00", compare_same_vectors)); + //CHECK(compare_to_bin_file< double >(depth_data.grad_in_direction, scene_dir, "zGradInDirection", 105794, 1,"double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.grad_in_direction, scene_dir, "validzGradInDirection", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.local_values, scene_dir, "localZvalues", 4, 105794, "double_00", compare_same_vectors)); + //CHECK(compare_to_bin_file< double >(depth_data.values_for_subedges, scene_dir, "zValuesForSubEdges", 105794, 1,"double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.edge_sub_pixel, scene_dir, "edgeSubPixel", 2, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.values_for_subedges, scene_dir, "validzValuesForSubEdges", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.supressed_edges, scene_dir, "validEdgePixels", 1, 105794, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< uint8_t >(depth_data.valid_section_map, scene_dir, "validsectionMapDepth", 1, 4259, "uint8_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.valid_directions, scene_dir, "validdirectionIndex", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.valid_edge_sub_pixel, scene_dir, "validedgeSubPixel", 2, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.valid_edge_sub_pixel_x, scene_dir, "xim", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.valid_edge_sub_pixel_y, scene_dir, "yim", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double>(depth_data.sub_points, scene_dir, "subPoints", 3, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< algo::double2 >(depth_data.uvmap, scene_dir, bin_file("uv", 2, 4259, "double_00") + ".bin", 4259, 1, compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.is_inside, scene_dir, "isInside", 1, 4259, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.subpixels_x, scene_dir, "Z_xim", 1, 2506, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.subpixels_y, scene_dir, "Z_yim", 1, 2506, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.closest, scene_dir, "Z_valuesForSubEdges", 1, 2506, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.grad_in_direction_inside, scene_dir, "zGradInDirection", 1, 2506, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< double >(depth_data.directions, scene_dir, "directionIndexInside", 1, 2506, "double_00", compare_same_vectors)); + CHECK(compare_to_bin_file< algo::double3 >(depth_data.vertices, scene_dir, bin_file("vertices", 3, 2506, "double_00") + ".bin", 2506, 1, compare_same_vectors)); + //CHECK(compare_to_bin_file< uint8_t >(depth_data.section_map_depth_inside, scene_dir, "sectionMapDepthInside", 1, 2506, "uint8_00", compare_same_vectors)); #endif - +#if 0 // --- TRACE( "\nChecking scene validity:" ); @@ -95,8 +125,8 @@ void compare_scene( std::string const & scene_dir ) // edge distribution CHECK( compare_to_bin_file< double >( z_data.sum_weights_per_section, scene_dir, "depthEdgeWeightDistributionPerSectionDepth", 1, 4, "double_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< byte >( z_data.section_map, scene_dir, "sectionMapDepth_trans", 1, md.n_edges, "uint8_00", compare_same_vectors ) ); - CHECK( compare_to_bin_file< byte >( yuy_data.section_map, scene_dir, "sectionMapRgb_trans", 1, rgb_w*rgb_h, "uint8_00", compare_same_vectors ) ); + //CHECK( compare_to_bin_file< byte >( z_data.section_map, scene_dir, "sectionMapDepth_trans", 1, md.n_edges, "uint8_00", compare_same_vectors ) ); + //CHECK( compare_to_bin_file< byte >( yuy_data.section_map, scene_dir, "sectionMapRgb_trans", 1, rgb_w*rgb_h, "uint8_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< double >( yuy_data.sum_weights_per_section, scene_dir, "edgeWeightDistributionPerSectionRgb", 1, 4, "double_00", compare_same_vectors ) ); @@ -116,8 +146,8 @@ void compare_scene( std::string const & scene_dir ) // 3. movement CHECK( compare_to_bin_file< double >( yuy_data.gaussian_diff_masked, scene_dir, "IDiffMasked", rgb_w, rgb_h, "double_00", compare_same_vectors ) ); CHECK( compare_to_bin_file< uint8_t >( yuy_data.move_suspect, scene_dir, "ixMoveSuspect", rgb_w, rgb_h, "uint8_00", compare_same_vectors ) ); - - +#endif +#if 1 //-- TRACE( "\nOptimizing:" ); auto cb = [&]( algo::iteration_data_collect const & data ) @@ -170,8 +200,8 @@ void compare_scene( std::string const & scene_dir ) auto cost = cal.get_cost(); CHECK( compare_calib_to_bin_file( new_calibration, cost, scene_dir, "new_calib", num_of_calib_elements, 1, "double_00" ) ); - - +#endif +#if 0 //-- TRACE( "\nChecking output validity:" ); // Pixel movement is OK, but some sections have negative cost @@ -180,4 +210,5 @@ void compare_scene( std::string const & scene_dir ) CHECK( cal.calc_correction_in_pixels() == approx( md.correction_in_pixels ) ); CHECK( compare_to_bin_file< double >( z_data.cost_diff_per_section, scene_dir, "costDiffPerSection", 4, 1, "double_00", compare_same_vectors ) ); +#endif } diff --git a/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h b/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h index 274faf4223..abaf8068fd 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h +++ b/unit-tests/algo/depth-to-rgb-calibration/d2rgb-common.h @@ -38,15 +38,15 @@ void init_algo( algo::optimizer & cal, camera.z.width, camera.z.height ); - cal.set_z_data( + /*cal.set_z_data( read_image_file< algo::z_t >( dir + z, camera.z.width, camera.z.height ), camera.z, float(camera.z_units) - ); + );*/ // TODO NOHA - /*cal.set_depth_data( + cal.set_depth_data( read_image_file< algo::z_t >(dir + z, camera.z.width, camera.z.height), read_image_file< algo::ir_t >(dir + ir, camera.z.width, camera.z.height), camera.z, float(camera.z_units) - );*/ + ); } diff --git a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp index f530b7300d..5f8c92dcda 100644 --- a/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp +++ b/unit-tests/algo/depth-to-rgb-calibration/test-scene-2.cpp @@ -10,9 +10,9 @@ TEST_CASE("Scene 2", "[d2rgb]") { - std::string scene_dir( "..\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20" ); + std::string scene_dir("C:\\Users\\nyassin\\Documents\\realsense_all\\debug_scene");// "..\\unit-tests\\algo\\depth-to-rgb-calibration\\19.2.20" ); // std::string scene_dir( "C:\\work\\autocal" ); - scene_dir += "\\F9440687\\LongRange_D_768x1024_RGB_1920x1080\\2\\"; + scene_dir += "\\LongRange 768X1024 (RGB 1920X1080)\\2\\";// "\\F9440687\\LongRange_D_768x1024_RGB_1920x1080\\2\\"; compare_scene( scene_dir ); } diff --git a/unit-tests/approx.h b/unit-tests/approx.h index c8135622d8..82397f95dc 100644 --- a/unit-tests/approx.h +++ b/unit-tests/approx.h @@ -17,7 +17,7 @@ // REQUIRE( == Approx(2.1).epsilon(0.01) ); // allow up to 1% diff // So we want a macro that does this automatically: #if ! defined( __EPSILON ) -#define __EPSILON (std::numeric_limits::epsilon()*1) +#define __EPSILON (std::numeric_limits::epsilon()) #endif #define approx(X) Approx(X).epsilon(__EPSILON) // Because we have our own macro (and because it's more verbose) we do NOT want