16 size_t num_of_vertices,
22 CHECK( compare_to_bin_file< algo::double3 >(
25 bin_file( time +
"laserIncidentDirection", cycle, 3, 1,
"double_00.bin" ) ) );
27 CHECK( compare_to_bin_file< algo::double3 >(
30 bin_file( time +
"fovexIndicentDirection", cycle, 3, num_of_vertices,
"double_00" )
36 CHECK( compare_to_bin_file< algo::double3 >(
39 bin_file( time +
"mirrorNormalDirection", cycle, 3, num_of_vertices,
"double_00" ) +
".bin",
44 CHECK( compare_to_bin_file< double >(
47 bin_file( time +
"angX", cycle, 1, num_of_vertices,
"double_00" ) +
".bin",
52 CHECK( compare_to_bin_file< double >(
55 bin_file( time +
"angY", cycle, 1, num_of_vertices,
"double_00" ) +
".bin",
60 CHECK( compare_to_bin_file< double >(
63 bin_file( time +
"dsmXcorr", cycle, 1, num_of_vertices,
"double_00" ) +
".bin",
68 CHECK( compare_to_bin_file< double >(
71 bin_file( time +
"dsmYcorr", cycle, 1, num_of_vertices,
"double_00" ) +
".bin",
76 CHECK( compare_to_bin_file< double >(
79 bin_file( time +
"dsmX", cycle, 1, num_of_vertices,
"double_00" ) +
".bin",
84 CHECK( compare_to_bin_file< double >(
87 bin_file( time +
"dsmY", cycle, 1, num_of_vertices,
"double_00" ) +
".bin",
99 auto z_w = depth_data.
width;
100 auto z_h = depth_data.
height;
102 auto rgb_w = yuy_data.
width;
103 auto rgb_h = yuy_data.
height;
134 CHECK( compare_to_bin_file< double >( ir_data.
edges,
141 CHECK( compare_to_bin_file< double >( depth_data.
edges,
157 "validEdgePixelsByIR",
330 CHECK( compare_to_bin_file< double >( depth_data.
local_x,
337 CHECK( compare_to_bin_file< double >( depth_data.
local_y,
365 CHECK( compare_to_bin_file< double >( depth_data.
closest,
367 "zValuesForSubEdges",
389 "validzGradInDirection",
403 "validzValuesForSubEdges",
417 "validsectionMapDepth",
424 "validdirectionIndex",
429 CHECK( compare_to_bin_file< algo::k_matrix >(
432 bin_file(
"k_depth_pinv", 3, 3,
"double_00.bin" ) ) );
438 scene_dir,
"subPoints",
442 CHECK( compare_to_bin_file< algo::double2 >( depth_data.
uvmap,
449 scene_dir,
"isInside",
464 scene_dir,
"directionIndexInside",
470 scene_dir,
"round_xim",
475 scene_dir,
"round_yim",
479 CHECK( compare_to_bin_file< double >( depth_data.
weights,
480 scene_dir,
"weights",
485 scene_dir,
"relevantPixelsImage",
489 CHECK( compare_to_bin_file< algo::double3 >( depth_data.
vertices,
496 scene_dir,
"sectionMapDepthInside",
502 scene_dir,
"YUY2_lum",
507 scene_dir,
"YUY2_edge",
512 scene_dir,
"YUY2_IDT",
517 scene_dir,
"YUY2_IDTx",
522 scene_dir,
"YUY2_IDTy",
530 bool debug_mode =
true,
533 TRACE(
"Loading " << scene_dir <<
" ..." );
545 algo::optimizer::settings settings;
550 settings.hum_temp,
scale))
561 TRACE(
"No thermal data found" );
586 auto num_of_calib_elements = 22;
587 auto num_of_p_matrix_elements =
sizeof(
algo::p_matrix ) /
sizeof(
double );
588 auto num_of_k_matrix_elements =
sizeof(
algo::k_matrix ) /
sizeof(
double );
596 profiler.
section(
"Checking scene validity" );
597 bool const is_scene_valid = cal.
is_scene_valid( debug_mode ? &data :
nullptr );
601 CHECK( is_scene_valid == matlab_scene_valid );
605 = read_from< uint8_t >(
join(
bin_dir( scene_dir ),
"DirSpread_1x1_uint8_00.bin" ) );
606 CHECK( data.edges_dir_spread == spread );
607 bool rgbEdgesSpread = read_from< uint8_t >(
608 join(
bin_dir( scene_dir ),
"rgbEdgesSpread_1x1_uint8_00.bin" ) );
609 CHECK( data.rgb_spatial_spread == rgbEdgesSpread );
610 bool depthEdgesSpread = read_from< uint8_t >(
611 join(
bin_dir( scene_dir ),
"depthEdgesSpread_1x1_uint8_00.bin" ) );
612 CHECK( data.depth_spatial_spread == depthEdgesSpread );
613 bool isMovementFromLastSuccess = read_from< uint8_t >(
614 join(
bin_dir( scene_dir ),
"isMovementFromLastSuccess_1x1_uint8_00.bin" ) );
615 CHECK( data.is_movement_from_last_success == isMovementFromLastSuccess );
619 stats->n_valid_scene = is_scene_valid;
620 stats->n_valid_scene_diff = is_scene_valid != matlab_scene_valid;
624 CHECK( compare_to_bin_file< double >( z_data.sum_weights_per_section,
626 "depthEdgeWeightDistributionPerSectionDepth",
637 CHECK( compare_to_bin_file< double >( yuy_data.sum_weights_per_section,
639 "edgeWeightDistributionPerSectionRgb",
647 CHECK( compare_to_bin_file< double >( z_data.sum_weights_per_direction,
658 CHECK( compare_to_bin_file< uint8_t >( yuy_data.debug.movement_result.logic_edges,
659 scene_dir,
"logicEdges",
664 CHECK( compare_to_bin_file< double >( yuy_data.debug.movement_result.dilated_image,
665 scene_dir,
"dilatedIm",
672 CHECK( compare_to_bin_file< double >( yuy_data.debug.movement_result.yuy_diff,
673 scene_dir,
"diffIm_01",
678 CHECK( compare_to_bin_file< double >( yuy_data.debug.movement_result.gaussian_filtered_image,
686 CHECK( compare_to_bin_file< double >( yuy_data.debug.movement_result.gaussian_diff_masked,
687 scene_dir,
"IDiffMasked",
692 CHECK( compare_to_bin_file< uint8_t >( yuy_data.debug.movement_result.move_suspect,
693 scene_dir,
"ixMoveSuspect",
709 <<
"COMPARING K_TO_DSM DATA " << data.cycle_data_p.cycle << std::endl;
713 CHECK( compare_to_bin_file< double >(
717 data.cycle_data_p.cycle,
718 num_of_k_matrix_elements,
721 num_of_k_matrix_elements,
725 CHECK( compare_to_bin_file< double >(
729 data.cycle_data_p.cycle,
730 num_of_k_matrix_elements,
733 num_of_k_matrix_elements,
737 CHECK( compare_to_bin_file< algo::double3 >( data.k2dsm_data_p.inputs.z.vertices,
740 data.cycle_data_p.cycle,
748 CHECK( compare_to_bin_file< algo::double2 >(
749 std::vector< algo::double2 >(
751 { data.k2dsm_data_p.inputs.previous_dsm_params.h_scale,
752 data.k2dsm_data_p.inputs.previous_dsm_params.v_scale } ),
754 bin_file(
"k2dsm_inpus_acData", data.cycle_data_p.cycle, 2, 1,
"double_00.bin" ),
760 CHECK( compare_to_bin_file< algo::algo_calibration_registers >(
761 data.k2dsm_data_p.inputs.new_dsm_regs,
764 data.cycle_data_p.cycle,
767 "double_00.bin" ) ) );
770 CHECK( compare_to_bin_file< algo::algo_calibration_registers >(
771 data.k2dsm_data_p.dsm_regs_orig,
773 bin_file(
"dsmRegsOrig", data.cycle_data_p.cycle, 4, 1,
"double_00.bin" ) ) );
775 CHECK( compare_to_bin_file< uint8_t >(
776 data.k2dsm_data_p.relevant_pixels_image_rot,
778 bin_file(
"relevantPixelnImage_rot", data.cycle_data_p.cycle, z_w, z_h,
"uint8_00" )
784 CHECK( compare_to_bin_file< algo::los_shift_scaling >(
785 data.k2dsm_data_p.dsm_pre_process_data.last_los_error,
788 data.cycle_data_p.cycle,
791 "double_00.bin" ) ) );
793 CHECK( compare_to_bin_file< algo::double3 >(
794 data.k2dsm_data_p.dsm_pre_process_data.vertices_orig,
797 data.cycle_data_p.cycle,
809 data.cycle_data_p.cycle,
811 data.k2dsm_data_p.first_norm_vertices_to_los_data );
813 CHECK( compare_to_bin_file< algo::double2 >(
814 data.k2dsm_data_p.dsm_pre_process_data.los_orig,
822 CHECK( compare_to_bin_file< double >( data.k2dsm_data_p.errL2,
825 data.cycle_data_p.cycle,
827 data.k2dsm_data_p.errL2.size(),
830 data.k2dsm_data_p.errL2.size(),
834 CHECK( compare_to_bin_file< algo::double2 >(
835 data.k2dsm_data_p.focal_scaling,
837 bin_file(
"focalScaling", data.cycle_data_p.cycle, 2, 1,
"double_00.bin" ) ) );
840 data.k2dsm_data_p.sg_mat,
843 data.cycle_data_p.cycle,
844 data.k2dsm_data_p.sg_mat[0].size(),
845 data.k2dsm_data_p.sg_mat.size(),
848 data.k2dsm_data_p.sg_mat.size(),
849 data.k2dsm_data_p.sg_mat[0].size(),
850 data.k2dsm_data_p.sg_mat.size(),
853 CHECK( compare_to_bin_file< double >(
854 data.k2dsm_data_p.sg_mat_tag_x_sg_mat,
857 data.cycle_data_p.cycle,
859 data.k2dsm_data_p.sg_mat_tag_x_sg_mat.size(),
862 data.k2dsm_data_p.sg_mat_tag_x_sg_mat.size(),
866 CHECK( compare_to_bin_file< double >(
867 data.k2dsm_data_p.sg_mat_tag_x_err_l2,
870 data.cycle_data_p.cycle,
872 data.k2dsm_data_p.sg_mat_tag_x_err_l2.size(),
875 data.k2dsm_data_p.sg_mat_tag_x_err_l2.size(),
879 CHECK( compare_to_bin_file< double >( data.k2dsm_data_p.quad_coef,
882 data.cycle_data_p.cycle,
884 data.k2dsm_data_p.quad_coef.size(),
887 data.k2dsm_data_p.quad_coef.size(),
891 CHECK( compare_to_bin_file< algo::double2 >(
892 data.k2dsm_data_p.opt_scaling_1,
894 bin_file(
"optScaling1", data.cycle_data_p.cycle, 1, 2,
"double_00.bin" ) ) );
896 CHECK( compare_to_bin_file< algo::double2 >(
897 data.k2dsm_data_p.opt_scaling,
899 bin_file(
"optScaling", data.cycle_data_p.cycle, 1, 2,
"double_00.bin" ) ) );
901 CHECK( compare_to_bin_file< algo::double2 >(
902 data.k2dsm_data_p.new_los_scaling,
904 bin_file(
"newlosScaling", data.cycle_data_p.cycle, 1, 2,
"double_00.bin" ) ) );
906 CHECK( compare_to_bin_file< algo::double2 >(
907 std::vector< algo::double2 >( 1,
908 { data.k2dsm_data_p.dsm_params_cand.h_scale,
909 data.k2dsm_data_p.dsm_params_cand.v_scale } ),
911 bin_file(
"acDataCand", data.cycle_data_p.cycle, 2, 1,
"double_00.bin" ),
916 CHECK( compare_to_bin_file< algo::algo_calibration_registers >(
917 data.k2dsm_data_p.dsm_regs_cand,
919 bin_file(
"dsmRegsCand", data.cycle_data_p.cycle, 4, 1,
"double_00.bin" ) ) );
923 data.cycle_data_p.cycle,
925 data.k2dsm_data_p.second_norm_vertices_to_los_data );
927 CHECK( compare_to_bin_file< algo::double2 >(
928 data.k2dsm_data_p.los_orig,
930 bin_file(
"orig_los", data.cycle_data_p.cycle, 2, md.
n_edges,
"double_00" )
936 CHECK( compare_to_bin_file< algo::double2 >(
937 data.k2dsm_data_p.dsm,
939 bin_file(
"dsm", data.cycle_data_p.cycle, 2, md.
n_edges,
"double_00" ) +
".bin",
944 CHECK( compare_to_bin_file< algo::double3 >(
945 data.k2dsm_data_p.vertices,
947 bin_file(
"new_vertices", data.cycle_data_p.cycle, 3, md.
n_edges,
"double_00.bin" ),
952 TRACE(
"\nSet next cycle data from Matlab:" );
957 <<
"COMPARING CYCLE DATA " << data.cycle_data_p.cycle << std::endl;
960 k_depth.
k_mat.
rot[0] = data.cycle_data_p.new_k_depth.fx;
961 k_depth.
k_mat.
rot[2] = data.cycle_data_p.new_k_depth.ppx;
962 k_depth.
k_mat.
rot[4] = data.cycle_data_p.new_k_depth.fy;
963 k_depth.
k_mat.
rot[5] = data.cycle_data_p.new_k_depth.ppy;
965 CHECK( compare_to_bin_file< algo::k_matrix >(
968 bin_file(
"end_cycle_Kdepth", data.cycle_data_p.cycle, 3, 3,
"double_00.bin" ) ) );
970 CHECK( compare_to_bin_file< algo::p_matrix >( data.cycle_data_p.new_params.curr_p_mat,
973 data.cycle_data_p.cycle,
974 num_of_p_matrix_elements,
976 "double_00.bin" ) ) );
1009 catch( std::runtime_error &
e )
1019 <<
"COMPARING ITERATION DATA " << data.cycle_data_p.cycle <<
" " 1020 << data.iteration_data_p.iteration + 1 << std::endl;
1021 CHECK( compare_to_bin_file< algo::p_matrix >(
1022 data.iteration_data_p.params.curr_p_mat,
1025 data.cycle_data_p.cycle,
1026 data.iteration_data_p.iteration + 1,
1027 num_of_p_matrix_elements,
1029 "double_00.bin" ) ) );
1031 CHECK( compare_to_bin_file< double >(
1032 std::vector< double >(
std::begin( data.iteration_data_p.c.k_mat.k_mat.rot ),
1033 std::end( data.iteration_data_p.c.k_mat.k_mat.rot ) ),
1036 data.cycle_data_p.cycle,
1037 data.iteration_data_p.iteration + 1,
1045 CHECK( compare_to_bin_file< double >(
1046 std::vector< double >( 1, data.iteration_data_p.params.cost ),
1049 data.cycle_data_p.cycle,
1050 data.iteration_data_p.iteration + 1,
1059 compare_to_bin_file< algo::double2 >( data.iteration_data_p.uvmap,
1062 data.cycle_data_p.cycle,
1063 data.iteration_data_p.iteration + 1,
1071 CHECK( compare_to_bin_file< double >( data.iteration_data_p.d_vals,
1074 data.cycle_data_p.cycle,
1075 data.iteration_data_p.iteration + 1,
1083 CHECK( compare_to_bin_file< double >( data.iteration_data_p.d_vals_x,
1086 data.cycle_data_p.cycle,
1087 data.iteration_data_p.iteration + 1,
1095 CHECK( compare_to_bin_file< double >( data.iteration_data_p.d_vals_y,
1098 data.cycle_data_p.cycle,
1099 data.iteration_data_p.iteration + 1,
1108 compare_to_bin_file< algo::double2 >( data.iteration_data_p.xy,
1111 data.cycle_data_p.cycle,
1112 data.iteration_data_p.iteration + 1,
1120 CHECK( compare_to_bin_file< double >( data.iteration_data_p.rc,
1123 data.cycle_data_p.cycle,
1124 data.iteration_data_p.iteration + 1,
1132 CHECK( compare_to_bin_file< algo::p_matrix >(
1133 data.iteration_data_p.coeffs_p.x_coeffs,
1136 data.cycle_data_p.cycle,
1137 data.iteration_data_p.iteration + 1,
1138 num_of_p_matrix_elements,
1145 CHECK( compare_to_bin_file< algo::p_matrix >(
1146 data.iteration_data_p.coeffs_p.y_coeffs,
1149 data.cycle_data_p.cycle,
1150 data.iteration_data_p.iteration + 1,
1151 num_of_p_matrix_elements,
1158 CHECK( compare_to_bin_file< double >(
1159 std::vector< double >(
1160 std::begin( data.iteration_data_p.params.calib_gradients.vals ),
1161 std::end( data.iteration_data_p.params.calib_gradients.vals ) ),
1164 data.cycle_data_p.cycle,
1165 data.iteration_data_p.iteration + 1,
1166 num_of_p_matrix_elements,
1169 num_of_p_matrix_elements,
1173 CHECK( compare_to_bin_file< double >(
1174 std::vector< double >( 1, data.iteration_data_p.grads_norma ),
1177 data.cycle_data_p.cycle,
1178 data.iteration_data_p.iteration + 1,
1186 CHECK( compare_to_bin_file< double >(
1187 std::vector< double >(
std::begin( data.iteration_data_p.grads_norm.vals ),
1188 std::end( data.iteration_data_p.grads_norm.vals ) ),
1191 data.cycle_data_p.cycle,
1192 data.iteration_data_p.iteration + 1,
1193 num_of_p_matrix_elements,
1196 num_of_p_matrix_elements,
1201 CHECK( compare_to_bin_file< double >(
1202 std::vector< double >(
std::begin( data.iteration_data_p.normalized_grads.vals ),
1203 std::end( data.iteration_data_p.normalized_grads.vals ) ),
1205 bin_file(
"normalized_grads_iteration",
1206 data.cycle_data_p.cycle,
1207 data.iteration_data_p.iteration + 1,
1208 num_of_p_matrix_elements,
1211 num_of_p_matrix_elements,
1216 CHECK( compare_to_bin_file< double >(
1217 std::vector< double >(
std::begin( data.iteration_data_p.unit_grad.vals ),
1218 std::end( data.iteration_data_p.unit_grad.vals ) ),
1221 data.cycle_data_p.cycle,
1222 data.iteration_data_p.iteration + 1,
1223 num_of_p_matrix_elements,
1226 num_of_p_matrix_elements,
1231 compare_to_bin_file< double >( std::vector< double >( 1, data.iteration_data_p.t ),
1234 data.cycle_data_p.cycle,
1235 data.iteration_data_p.iteration + 1,
1243 CHECK( compare_to_bin_file< double >(
1244 std::vector< double >( 1, data.iteration_data_p.back_tracking_line_search_iters ),
1246 bin_file(
"back_tracking_line_iter_count_iteration",
1247 data.cycle_data_p.cycle,
1248 data.iteration_data_p.iteration + 1,
1256 CHECK( compare_to_bin_file< double >(
1257 std::vector< double >(
1258 std::begin( data.iteration_data_p.next_params.curr_p_mat.vals ),
1259 std::end( data.iteration_data_p.next_params.curr_p_mat.vals ) ),
1261 bin_file(
"next_p_matrix_iteration",
1262 data.cycle_data_p.cycle,
1263 data.iteration_data_p.iteration + 1,
1264 num_of_p_matrix_elements,
1267 num_of_p_matrix_elements,
1271 CHECK( compare_to_bin_file< double >(
1272 std::vector< double >( 1, data.iteration_data_p.next_params.cost ),
1275 data.cycle_data_p.cycle,
1276 data.iteration_data_p.iteration + 1,
1289 profiler.
section(
"Optimizing" );
1297 auto filename =
bin_file(
"new_calib", num_of_calib_elements, 1,
"double_00" ) +
".bin";
1300 double matlab_cost = 0;
1303 new_calibration.copy_coefs( matlab_calib );
1307 stats->d_cost =
cost - matlab_cost;
1312 auto vertices = read_vector_from< algo::double3 >(
1319 new_calibration.calc_p_mat() );
1323 CHECK( our_uvmap.size() == matlab_uvmap.size() );
1324 if( our_uvmap.size() == matlab_uvmap.size() )
1327 stats->d_movement = -1;
1333 auto p_vec = read_vector_from< double >(
bin_file(
join(
bin_dir( scene_dir ),
"end_p_matrix" ),
1334 num_of_p_matrix_elements,
1336 "double_00.bin" ) );
1344 num_of_p_matrix_elements,
1346 "double_00.bin" ) );
1348 std::copy( p_vec_opt.begin(), p_vec_opt.end(), p_mat_opt.vals );
1355 profiler.
section(
"Checking output validity" );
1359 CHECK( is_valid_results == matlab_valid_results );
1363 stats->n_valid_result = is_valid_results;
1364 stats->n_valid_result_diff = is_valid_results != matlab_valid_results;
1366 stats->n_converged = is_valid_results && is_scene_valid;
1367 bool const matlab_converged = matlab_valid_results && matlab_scene_valid;
1368 stats->n_converged_diff = bool( stats->n_converged ) != matlab_converged;
1373 CHECK( movement_in_pixels ==
approx( matlab_movement_in_pixels ) );
1376 stats->movement = movement_in_pixels;
1380 CHECK( compare_to_bin_file< double >( z_data.cost_diff_per_section,
1382 "costDiffPerSection",
1389 auto svm_features_mat = read_vector_from< double >(
1392 svm_features_mat.erase( svm_features_mat.begin() + 7 );
1393 auto svm_mat = svm_features;
1394 svm_mat.erase( svm_mat.begin() + 7 );
1400 CHECK( compare_to_bin_file< double >( decision_params.distribution_per_section_depth,
1402 "svm_edgeWeightDistributionPerSectionDepth",
1407 CHECK( compare_to_bin_file< double >( decision_params.distribution_per_section_rgb,
1409 "svm_edgeWeightDistributionPerSectionRgb",
1414 CHECK( compare_to_bin_file< double >( decision_params.edge_weights_per_dir,
1416 "svm_edgeWeightsPerDir",
1421 CHECK( compare_to_bin_file< double >( decision_params.improvement_per_section,
1423 "svm_improvementPerSection",
std::vector< uint8_t > relevant_pixels_image
std::vector< double3 > vertices
librealsense::algo::depth_to_rgb_calibration::algo_calibration_registers algo_calibration_registers
float memory_consumption_peak
std::vector< byte > is_inside
bool compare_to_bin_file(std::vector< D > const &vec, std::string const &scene_dir, std::string const &filename, size_t width, size_t height, size_t size, bool(*compare_vectors)(std::vector< F > const &, std::vector< D > const &)=nullptr)
bool get_calib_and_cost_from_raw_data(algo::calib &calib, double &cost, std::string const &scene_dir, std::string const &filename)
std::vector< double > valid_location_rc_y
std::string bin_file(std::string const &prefix, size_t cycle, size_t iteration, size_t w, size_t h, std::string const &suffix)
std::string join(const std::string &base, const std::string &path)
librealsense::algo::depth_to_rgb_calibration::algo_calibration_info cal_info
std::vector< double > ang_x
GLenum GLenum GLenum GLenum GLenum scale
std::vector< double > valid_location_rc
std::vector< double > valid_edge_sub_pixel
std::vector< double > local_y
std::vector< double2 > uvmap
std::vector< double > grad_in_direction_valid
void set_final_data(const std::vector< double3 > &vertices, const p_matrix &p_mat, const p_matrix &p_mat_opt=p_matrix())
std::vector< double > local_edges
std::vector< double > edge_sub_pixel
std::vector< double3 > fovex_indicent_direction
std::vector< double > edges
std::vector< double > gradient_x
std::vector< double > local_region[4]
GLsizei const GLchar *const * string
std::vector< double > grad_in_direction
bool read_thermal_data(std::string dir, double hum_temp, double &scale)
std::vector< double > sub_points
bool compare_calib(algo::calib const &calib, double cost, algo::calib calib_from_file, double cost_matlab)
std::vector< double > gradient
std::vector< double > local_region_x[4]
std::vector< double > subpixels_y_round
p_matrix const calc_p_mat() const
librealsense::algo::depth_to_rgb_calibration::algo_calibration_registers cal_regs
size_t optimize(std::function< void(data_collect const &data) > iteration_callback=nullptr)
std::vector< byte > section_map_depth
size_t n_valid_scene_diff
librealsense::algo::depth_to_rgb_calibration::rs2_intrinsics_double z
std::vector< byte > supressed_edges
std::vector< double > subpixels_x
bool compare_same_vectors(std::vector< F > const &matlab, std::vector< D > const &cpp)
std::vector< double3 > mirror_normal_direction
std::vector< byte > is_supressed
std::vector< double > edges_IDTy
float get_baseline() const
std::vector< byte > valid_section_map
std::vector< double > local_values
size_t n_valid_result_diff
std::vector< double > local_region_y[4]
std::vector< double > subpixels_x_round
std::vector< double > valid_direction_per_pixel
std::vector< double > direction_per_pixel
ir_frame_data const & get_ir_data() const
std::vector< double > dsm_x_corr
z_frame_data const & get_z_data() const
std::vector< double > edges
decision_params const & get_decision_params() const
std::vector< double > local_x
std::vector< double > const & get_extracted_features() const
std::vector< double > valid_gradient_y
void init_algo(algo::optimizer &cal, std::string const &dir, std::string const &yuy, std::string const &yuy_prev, std::string const &yuy_last_successful, std::string const &ir, std::string const &z, camera_params const &camera, memory_profiler *profiler=nullptr)
std::vector< double > gradient_y
std::vector< uint8_t > lum_frame
std::vector< byte > section_map
camera_params read_camera_params(std::string const &scene_dir, std::string const &filename)
void compare_preprocessing_data(std::string const &scene_dir, algo::z_frame_data const &depth_data, algo::ir_frame_data const &ir_data, algo::yuy2_frame_data const &yuy_data, scene_metadata const &md)
yuy2_frame_data const & get_yuy_data() const
std::vector< double > dsm_y_corr
std::vector< double > subpixels_y
double calc_correction_in_pixels(std::vector< double2 > const &old_uvmap, std::vector< double2 > const &new_uvmap) const
rs2_dsm_params dsm_params
bool is_scene_valid(input_validity_data *data=nullptr)
static const struct @18 vertices[3]
calib const & get_calibration() const
void section(char const *heading)
uvmap_t get_texture_map(std::vector< double3 > const &points, const calib &cal, const p_matrix &p_mat)
std::vector< double > valid_directions
std::vector< double > valid_location_rc_x
librealsense::algo::depth_to_rgb_calibration::rs2_intrinsics_double rgb
void compare_vertices_to_los_data(std::string const &scene_dir, size_t num_of_vertices, size_t cycle, std::string const &time, algo::convert_norm_vertices_to_los_data const &data)
std::vector< double > valid_gradient_x
std::vector< double > direction_deg
struct librealsense::algo::depth_to_rgb_calibration::yuy2_frame_data::@0 debug
std::vector< double > dsm_y
void compare_scene(std::string const &scene_dir, bool debug_mode=true, scene_stats *stats=nullptr)
std::vector< double > edges_IDT
std::string bin_dir(std::string const &scene_dir)
std::vector< double > local_rc_subpixel
std::vector< double > dsm_x
std::vector< byte > valid_section_map
std::vector< double > ang_y
std::vector< double > closest
std::vector< double > edges
std::vector< double > fraq_step
std::vector< double > values_for_subedges
std::vector< double > edges_IDTx
dsm_params read_dsm_params(std::string const &scene_dir, std::string const &filename)
std::vector< double > gradient_y
std::vector< double > gradient_x
std::vector< double > directions
std::vector< byte > valid_edge_pixels_by_ir
librealsense::algo::depth_to_rgb_calibration::algo_calibration_info regs
std::vector< double > weights
void copy(void *dst, void const *src, size_t size)
void read_data_from(std::string const &filename, T *data)