d400-auto-calibration.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2016 Intel Corporation. All Rights Reserved.
3 
4 #include <numeric>
5 #include <rsutils/json.h>
6 #include "d400-device.h"
7 #include "d400-private.h"
8 #include "d400-thermal-monitor.h"
9 #include "d400-auto-calibration.h"
10 #include "librealsense2/rsutil.h"
11 #include "algo.h"
12 #include <src/core/video-frame.h>
13 
14 #include <rsutils/string/from.h>
15 
16 
17 #undef UCAL_PROFILE
18 #ifdef UCAL_PROFILE
19 #define LOG_OCC_WARN(...) do { CLOG(WARNING ,"librealsense") << __VA_ARGS__; } while(false)
20 #else
21 #define LOG_OCC_WARN(...)
22 #endif //UCAL_PROFILE
23 
24 namespace librealsense
25 {
26 #pragma pack(push, 1)
27 #pragma pack(1)
28  struct TareCalibrationResult
29  {
30  uint16_t status; // DscStatus
31  uint32_t tareDepth; // Tare depth in 1/100 of depth unit
32  uint32_t aveDepth; // Average depth in 1/100 of depth unit
33  int32_t curPx; // Current Px in 1/1000000 of normalized unit
34  int32_t calPx; // Calibrated Px in 1/1000000 of normalized unit
35  float curRightRotation[9]; // Current right rotation
36  float calRightRotation[9]; // Calibrated right rotation
37  uint16_t accuracyLevel; // [0-3] (Very High/High/Medium/Low)
38  uint16_t iterations; // Number of iterations it took to converge
39  };
40 
41  struct FocalLengthCalibrationResult
42  {
43  uint16_t status; // DscStatus
44  uint16_t stepCount;
45  uint16_t scanRange; // 1/1000 of a pixel
46  float rightFy;
47  float rightFx;
48  float leftFy;
49  float leftFx;
50  float FL_healthCheck;
51  uint16_t fillFactor0; // first of the setCount number of fill factor, 1/100 of a percent
52  };
53 
54  struct DscPyRxFLCalibrationTableResult
55  {
56  uint16_t headerSize; // 10 bytes for status & health numbers
57  uint16_t status; // DscStatus
58  float healthCheck;
59  float FL_heathCheck;
60  uint16_t tableSize; // 512 bytes
61  };
62 
63  struct DscResultParams
64  {
65  uint16_t m_status;
66  float m_healthCheck;
67  };
68 
69  struct DscResultBuffer
70  {
71  uint16_t m_paramSize;
72  DscResultParams m_dscResultParams;
73  uint16_t m_tableSize;
74  };
75 
76  enum rs2_dsc_status : uint16_t
77  {
78  RS2_DSC_STATUS_SUCCESS = 0,
79  RS2_DSC_STATUS_RESULT_NOT_READY = 1,
80  RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW = 2,
81  RS2_DSC_STATUS_EDGE_TOO_CLOSE = 3,
82  RS2_DSC_STATUS_NOT_CONVERGE = 4,
83  RS2_DSC_STATUS_BURN_SUCCESS = 5,
84  RS2_DSC_STATUS_BURN_ERROR = 6,
85  RS2_DSC_STATUS_NO_DEPTH_AVERAGE = 7
86  };
87 
88 #pragma pack(pop)
89 
90  enum auto_calib_sub_cmd : uint8_t
91  {
92  py_rx_calib_check_status = 0x03,
93  interactive_scan_control = 0x05,
94  py_rx_calib_begin = 0x08,
95  tare_calib_begin = 0x0b,
96  tare_calib_check_status = 0x0c,
97  get_calibration_result = 0x0d,
98  focal_length_calib_begin = 0x11,
99  get_focal_legth_calib_result = 0x12,
100  py_rx_plus_fl_calib_begin = 0x13,
101  get_py_rx_plus_fl_calib_result = 0x14,
102  set_coefficients = 0x19
103  };
104 
105  enum auto_calib_speed : uint8_t
106  {
107  speed_very_fast = 0,
108  speed_fast = 1,
109  speed_medium = 2,
110  speed_slow = 3,
111  speed_white_wall = 4
112  };
113 
114  enum class host_assistance_type
115  {
116  no_assistance = 0,
117  assistance_start,
118  assistance_first_feed,
119  assistance_second_feed,
120  };
121 
122  enum subpixel_accuracy
123  {
124  very_high = 0, //(0.025%)
125  high = 1, //(0.05%)
126  medium = 2, //(0.1%)
127  low = 3 //(0.2%)
128  };
129 
130  enum data_sampling
131  {
132  polling = 0,
133  interrupt = 1
134  };
135 
136  enum scan_parameter
137  {
138  py_scan = 0,
139  rx_scan = 1
140  };
141 
142  struct tare_params3
143  {
144  uint8_t average_step_count;
145  uint8_t step_count;
146  uint8_t accuracy;
147  uint8_t reserved;
148  };
149 
150  struct params4
151  {
152  int scan_parameter : 1;
153  int reserved : 2;
154  int data_sampling : 1;
155  };
156 
157  union tare_calibration_params
158  {
159  tare_params3 param3_struct;
160  uint32_t param3;
161  };
162 
163  union param4
164  {
165  params4 param4_struct;
166  uint32_t param_4;
167  };
168 
169  const int DEFAULT_CALIB_TYPE = 0;
170 
171  const int DEFAULT_AVERAGE_STEP_COUNT = 20;
172  const int DEFAULT_STEP_COUNT = 20;
173  const int DEFAULT_ACCURACY = subpixel_accuracy::medium;
174  const auto_calib_speed DEFAULT_SPEED = auto_calib_speed::speed_slow;
175  const int DEFAULT_SCAN = scan_parameter::py_scan;
176  const int DEFAULT_SAMPLING = data_sampling::interrupt;
177 
178  const int DEFAULT_FL_STEP_COUNT = 100;
179  const int DEFAULT_FY_SCAN_RANGE = 40;
180  const int DEFAULT_KEEP_NEW_VALUE_AFTER_SUCESSFUL_SCAN = 1;
181  const int DEFAULT_FL_SAMPLING = data_sampling::interrupt;
182  const int DEFAULT_ADJUST_BOTH_SIDES = 0;
183 
184  const int DEFAULT_TARE_SAMPLING = data_sampling::polling;
185 
186  const int DEFAULT_OCC_FL_SCAN_LOCATION = 0;
187  const int DEFAULT_FY_SCAN_DIRECTION = 0;
188  const int DEFAULT_WHITE_WALL_MODE = 0;
189 
190  auto_calibrated::auto_calibrated()
191  : _interactive_state(interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE),
192  _interactive_scan(false),
193  _action(auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB),
194  _average_step_count(-1),
195  _collected_counter(-1),
196  _collected_frame_num(-1),
197  _collected_sum(-1.0),
198  _min_valid_depth(0),
199  _max_valid_depth(uint16_t(-1)),
200  _resize_factor(5),
201  _skipped_frames(0)
202  {}
203 
204  std::map<std::string, int> auto_calibrated::parse_json(std::string json_content)
205  {
206  auto j = rsutils::json::parse(json_content);
207 
208  std::map<std::string, int> values;
209 
210  for (auto it = j.begin(); it != j.end(); ++it)
211  {
212  values[it.key()] = it.value();
213  }
214 
215  return values;
216  }
217 
218  void try_fetch(std::map<std::string, int> jsn, std::string key, int* value)
219  {
220  std::replace(key.begin(), key.end(), '_', ' '); // Treat _ as space
221  if (jsn.find(key) != jsn.end())
222  {
223  *value = jsn[key];
224  }
225  }
226 
227  // RAII to handle auto-calibration with the thermal compensation disabled
228  class thermal_compensation_guard
229  {
230  public:
231  thermal_compensation_guard(auto_calibrated_interface* handle) :
232  restart_tl(false), snr(nullptr)
233  {
234  if (Is<librealsense::device>(handle))
235  {
236  try
237  {
238  // The depth sensor is assigned first by design
239  snr = &(As<librealsense::device>(handle)->get_sensor(0));
240 
241  if (snr->supports_option(RS2_OPTION_THERMAL_COMPENSATION))
242  restart_tl = static_cast<bool>(snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).query() != 0);
243  if (restart_tl)
244  {
245  snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).set(0.f);
246  // Allow for FW changes to propagate
247  std::this_thread::sleep_for(std::chrono::milliseconds(100));
248  }
249  }
250  catch(...) {
251  LOG_WARNING("Thermal Compensation guard failed to invoke");
252  }
253  }
254  }
255  virtual ~thermal_compensation_guard()
256  {
257  try
258  {
259  if (snr && restart_tl)
260  snr->get_option(RS2_OPTION_THERMAL_COMPENSATION).set(1.f);
261  }
262  catch (...) {
263  LOG_WARNING("Thermal Compensation guard failed to complete");
264  }
265  }
266 
267  protected:
268  bool restart_tl;
269  librealsense::sensor_interface* snr;
270 
271  private:
272  // Disable copy/assignment ctors
273  thermal_compensation_guard(const thermal_compensation_guard&);
274  thermal_compensation_guard& operator=(const thermal_compensation_guard&);
275  };
276 
277  DirectSearchCalibrationResult auto_calibrated::get_calibration_status(int timeout_ms, std::function<void(const int count)> progress_func, bool wait_for_final_results)
278  {
279  DirectSearchCalibrationResult result{};
280 
281  int count = 0;
282  int retries = 0;
283  bool done = false;
284 
285  auto start = std::chrono::high_resolution_clock::now();
286  auto now = start;
287 
288  // While not ready...
289  do
290  {
291  std::this_thread::sleep_for(std::chrono::milliseconds(200));
292  try
293  {
294  // Check calibration status
295  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_check_status });
296  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " check occ status, res size = " << res.size()));
297  if (res.size() < sizeof(DirectSearchCalibrationResult))
298  {
299  if (!((retries++) % 5)) // Add log debug once in a sec
300  {
301  LOG_DEBUG("Not enough data from CALIB_STATUS!");
302  }
303  }
304  else
305  {
306  result = *reinterpret_cast<DirectSearchCalibrationResult*>(res.data());
307  done = !wait_for_final_results || result.status != RS2_DSC_STATUS_RESULT_NOT_READY;
308  }
309  }
310  catch (const invalid_value_exception& e)
311  {
312  LOG_DEBUG("error: " << e.what());
313  // Asked for status while firmware is still in progress.
314  }
315 
316  if (progress_func)
317  {
318  progress_func(count);
319  }
320 
321  now = std::chrono::high_resolution_clock::now();
322 
323  } while (now - start < std::chrono::milliseconds(timeout_ms) && !done);
324 
325 
326  // If we exit due to timeout, report timeout
327  if (!done)
328  {
329  throw std::runtime_error("Operation timed-out!\n"
330  "Calibration state did not converge on time");
331  }
332  return result;
333  }
334 
335  std::vector<uint8_t> auto_calibrated::run_on_chip_calibration(int timeout_ms, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback)
336  {
337  int calib_type = DEFAULT_CALIB_TYPE;
338 
339  auto_calib_speed speed(DEFAULT_SPEED);
340  int speed_fl = auto_calib_speed::speed_slow;
341  int scan_parameter = DEFAULT_SCAN;
342  int data_sampling = DEFAULT_SAMPLING;
343  int apply_preset = 1;
344 
345  int fl_step_count = DEFAULT_FL_STEP_COUNT;
346  int fy_scan_range = DEFAULT_FY_SCAN_RANGE;
347  int keep_new_value_after_sucessful_scan = DEFAULT_KEEP_NEW_VALUE_AFTER_SUCESSFUL_SCAN;
348  int fl_data_sampling = DEFAULT_FL_SAMPLING;
349  int adjust_both_sides = DEFAULT_ADJUST_BOTH_SIDES;
350 
351  int fl_scan_location = DEFAULT_OCC_FL_SCAN_LOCATION;
352  int fy_scan_direction = DEFAULT_FY_SCAN_DIRECTION;
353  int white_wall_mode = DEFAULT_WHITE_WALL_MODE;
354 
355  host_assistance_type host_assistance(host_assistance_type::no_assistance);
356  int scan_only_v3 = 0;
357  int interactive_scan_v3 = 0;
358  uint16_t step_count_v3 = 0;
359  uint16_t fill_factor[256] = { 0 };
360 
361  float h_1 = 0.0f;
362  float h_2 = 0.0f;
363 
364  //Enforce Thermal Compensation off during OCC
365  volatile thermal_compensation_guard grd(this);
366 
367  if (json.size() > 0)
368  {
369  int tmp_speed(DEFAULT_SPEED);
370  int tmp_host_assistance(0);
371  auto jsn = parse_json(json);
372  try_fetch(jsn, "calib type", &calib_type);
373  if (calib_type == 0)
374  {
375  try_fetch(jsn, "speed", &tmp_speed);
376  if (tmp_speed < speed_very_fast || tmp_speed > speed_white_wall)
377  throw invalid_value_exception( rsutils::string::from()
378  << "Auto calibration failed! Given value of 'speed' " << speed
379  << " is out of range (0 - 4)." );
380  speed = auto_calib_speed(tmp_speed);
381  }
382  else
383  try_fetch(jsn, "speed", &speed_fl);
384 
385  try_fetch(jsn, "host assistance", &tmp_host_assistance);
386  if (tmp_host_assistance < (int)host_assistance_type::no_assistance || tmp_host_assistance > (int)host_assistance_type::assistance_second_feed)
387  throw invalid_value_exception( rsutils::string::from()
388  << "Auto calibration failed! Given value of 'host assistance' "
389  << tmp_host_assistance << " is out of range (0 - "
390  << (int)host_assistance_type::assistance_second_feed << ")." );
391  host_assistance = host_assistance_type(tmp_host_assistance);
392 
393  try_fetch(jsn, "scan parameter", &scan_parameter);
394  try_fetch(jsn, "data sampling", &data_sampling);
395  try_fetch(jsn, "apply preset", &apply_preset);
396 
397  try_fetch(jsn, "fl step count", &fl_step_count);
398  try_fetch(jsn, "fy scan range", &fy_scan_range);
399  try_fetch(jsn, "keep new value after sucessful scan", &keep_new_value_after_sucessful_scan);
400  try_fetch(jsn, "fl data sampling", &fl_data_sampling);
401  try_fetch(jsn, "adjust both sides", &adjust_both_sides);
402 
403  try_fetch(jsn, "fl scan location", &fl_scan_location);
404  try_fetch(jsn, "fy scan direction", &fy_scan_direction);
405  try_fetch(jsn, "white wall mode", &white_wall_mode);
406 
407  try_fetch(jsn, "scan only", &scan_only_v3);
408  try_fetch(jsn, "interactive scan", &interactive_scan_v3);
409 
410  int val = 0;
411  try_fetch(jsn, "step count v3", &val);
412 
413  step_count_v3 = static_cast<uint16_t>(val);
414  if (step_count_v3 > 0)
415  {
416  for (int i = 0; i < step_count_v3; ++i)
417  {
418  val = 0;
419  std::stringstream ss;
420  ss << "fill factor " << i;
421  try_fetch(jsn, ss.str(), &val);
422  fill_factor[i] = static_cast<uint16_t>(val);
423  }
424  }
425  try_fetch(jsn, "resize factor", &_resize_factor);
426  }
427 
428  std::vector<uint8_t> res;
429 
430  if (host_assistance != host_assistance_type::no_assistance && _interactive_state == interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE)
431  {
432  _json = json;
433  _action = auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB;
434  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START;
435  _interactive_scan = false; // Production code must enforce non-interactive runs. Interactive scans for development only
436  switch (speed)
437  {
438  case auto_calib_speed::speed_very_fast:
439  _total_frames = 60;
440  break;
441  case auto_calib_speed::speed_fast:
442  _total_frames = 120;
443  break;
444  case auto_calib_speed::speed_medium:
445  _total_frames = 256;
446  break;
447  case auto_calib_speed::speed_slow:
448  _total_frames = 256;
449  break;
450  case auto_calib_speed::speed_white_wall:
451  _total_frames = 120;
452  break;
453  }
454  std::fill_n(_fill_factor, 256, 0);
455  DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count)
456  {
457  if (progress_callback)
458  {
459  if (host_assistance != host_assistance_type::no_assistance)
460  {
461  if (count < 20)
462  progress_callback->on_update_progress(static_cast<float>(80 + count++));
463  else
464  progress_callback->on_update_progress(count++ * (2.f * static_cast<int>(speed))); //currently this number does not reflect the actual progress
465  }
466  }
467  }, false);
468  // Handle errors from firmware
469  rs2_dsc_status status = (rs2_dsc_status)result.status;
470 
471  if (result.maxDepth == 0)
472  {
473  throw std::runtime_error("Firmware calibration values are not yet set.");
474  }
475  _min_valid_depth = result.minDepth;
476  _max_valid_depth = result.maxDepth;
477  return res;
478  }
479 
480  std::shared_ptr<ds_advanced_mode_base> preset_recover;
481  if (calib_type == 0)
482  {
483  LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling);
484  check_params(speed, scan_parameter, data_sampling);
485 
486  uint32_t p4 = 0;
487  if (scan_parameter)
488  p4 |= 1;
489  if (host_assistance != host_assistance_type::no_assistance)
490  p4 |= (1 << 1);
491  if (data_sampling)
492  p4 |= (1 << 3);
493  if (scan_only_v3)
494  p4 |= (1 << 8);
495  if (interactive_scan_v3)
496  p4 |= (1 << 9);
497 
498  if (speed == speed_white_wall && apply_preset)
499  {
500  preset_recover = change_preset();
501  std::this_thread::sleep_for(std::chrono::milliseconds(200));
502  }
503 
504  // Begin auto-calibration
505  if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start)
506  {
507  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, py_rx_calib_begin, speed, 0, p4 });
508  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__
509  << " send occ py_rx_calib_begin, speed = " << speed << ", p4 = " << p4 << " res size = " << res.size()));
510  }
511 
512  if (host_assistance != host_assistance_type::assistance_start)
513  {
514  if (host_assistance == host_assistance_type::assistance_first_feed)
515  {
516  command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0);
517  LOG_OCC_WARN(" occ interactive_scan_control 0,0 - save statistics ");
518  uint8_t* p = reinterpret_cast<uint8_t*>(&step_count_v3);
519  cmd.data.push_back(p[0]);
520  cmd.data.push_back(p[1]);
521  for (uint16_t i = 0; i < step_count_v3; ++i)
522  {
523  p = reinterpret_cast<uint8_t*>(fill_factor + i);
524  cmd.data.push_back(p[0]);
525  cmd.data.push_back(p[1]);
526  }
527  bool success = false;
528  int iter =0;
529  do // Retries are needed to overcome MIPI SKU occasionaly reporting busy state
530  {
531  try
532  {
533  if (iter==0) // apply only in the first iteration
534  std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Sending shorter request may fail with MIPI SKU
535  auto res = _hw_monitor->send(cmd);
536  success = true;
537  }
538  catch(...)
539  {
540  LOG_OCC_WARN("occ Save Statistics result failed");
541  std::this_thread::sleep_for(std::chrono::milliseconds(100)); // For the FW to recuperate
542  };
543  }
544  while(( ++iter < 3) && (!success));
545  }
546 
547  DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count)
548  {
549  if( progress_callback )
550  {
551  if( host_assistance != host_assistance_type::no_assistance )
552  {
553  if( count < 20 )
554  progress_callback->on_update_progress( static_cast< float >( 80 + count++ ) );
555  else
556  progress_callback->on_update_progress( count++ * ( 2.f * static_cast< int >( speed ) ) ); // currently this number does not reflect the actual progress
557  }
558  }
559  });
560  std::this_thread::sleep_for(std::chrono::milliseconds(100));
561 
562  auto status = (rs2_dsc_status)result.status;
563 
564  // Handle errors from firmware
565  if (status != RS2_DSC_STATUS_SUCCESS)
566  {
567  handle_calibration_error(status);
568  }
569  if (progress_callback)
570  progress_callback->on_update_progress(static_cast<float>(100));
571  res = get_calibration_results(health);
572  LOG_OCC_WARN(std::string(rsutils::string::from() << "Py: " << result.rightPy));
573  }
574  }
575  else if (calib_type == 1)
576  {
577  LOG_DEBUG("run_on_chip_focal_length_calibration with parameters: step count = " << fl_step_count
578  << ", fy scan range = " << fy_scan_range << ", keep new value after sucessful scan = " << keep_new_value_after_sucessful_scan
579  << ", interrrupt data sampling " << fl_data_sampling << ", adjust both sides = " << adjust_both_sides
580  << ", fl scan location = " << fl_scan_location << ", fy scan direction = " << fy_scan_direction << ", white wall mode = " << white_wall_mode);
581  check_focal_length_params(fl_step_count, fy_scan_range, keep_new_value_after_sucessful_scan, fl_data_sampling, adjust_both_sides, fl_scan_location, fy_scan_direction, white_wall_mode);
582 
583  // Begin auto-calibration
584  uint32_t p4 = 0;
585  if (keep_new_value_after_sucessful_scan)
586  p4 |= (1 << 1);
587  if (fl_data_sampling)
588  p4 |= (1 << 3);
589  if (adjust_both_sides)
590  p4 |= (1 << 4);
591  if (fl_scan_location)
592  p4 |= (1 << 5);
593  if (fy_scan_direction)
594  p4 |= (1 << 6);
595  if (white_wall_mode)
596  p4 |= (1 << 7);
597  if (scan_only_v3)
598  p4 |= (1 << 8);
599  if (interactive_scan_v3)
600  p4 |= (1 << 9);
601 
602  if (speed == speed_white_wall && apply_preset)
603  {
604  preset_recover = change_preset();
605  std::this_thread::sleep_for(std::chrono::milliseconds(200));
606  }
607 
608  if( host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start )
609  {
610  _hw_monitor->send( command{ ds::AUTO_CALIB,
611  focal_length_calib_begin,
612  static_cast< uint32_t >( fl_step_count ),
613  static_cast< uint32_t >( fy_scan_range ),
614  p4 } );
615  }
616 
617  if (host_assistance != host_assistance_type::assistance_start)
618  {
619  if (host_assistance == host_assistance_type::assistance_first_feed)
620  {
621  command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0);
622  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ interactive_scan_control 0,0 " << " res size = " << res.size()));
623  uint8_t* p = reinterpret_cast<uint8_t*>(&step_count_v3);
624  cmd.data.push_back(p[0]);
625  cmd.data.push_back(p[1]);
626  for (uint16_t i = 0; i < step_count_v3; ++i)
627  {
628  p = reinterpret_cast<uint8_t*>(fill_factor + i);
629  cmd.data.push_back(p[0]);
630  cmd.data.push_back(p[1]);
631  }
632 
633  _hw_monitor->send(cmd);
634  }
635 
636  FocalLengthCalibrationResult result{};
637 
638  int count = 0;
639  bool done = false;
640 
641  auto start = std::chrono::high_resolution_clock::now();
642  auto now = start;
643 
644  // While not ready...
645  do
646  {
647  std::this_thread::sleep_for(std::chrono::milliseconds(200));
648 
649  // Check calibration status
650  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_focal_legth_calib_result });
651 
652  if (res.size() < sizeof(FocalLengthCalibrationResult))
653  LOG_WARNING("Not enough data from CALIB_STATUS!");
654  else
655  {
656  result = *reinterpret_cast<FocalLengthCalibrationResult*>(res.data());
657  done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY;
658  }
659 
660  if( progress_callback )
661  {
662  if( host_assistance != host_assistance_type::no_assistance )
663  {
664  if( count < 20 )
665  progress_callback->on_update_progress( static_cast< float >( 80 + count++ ) );
666  else
667  progress_callback->on_update_progress( count++ * ( 2.f * 3 ) ); // currently this number does not reflect the actual progress
668  }
669  }
670 
671  now = std::chrono::high_resolution_clock::now();
672 
673  } while (now - start < std::chrono::milliseconds(timeout_ms) && !done);
674 
675 
676  // If we exit due to timeout, report timeout
677  if (!done)
678  {
679  throw std::runtime_error("Operation timed-out!\n"
680  "Calibration did not converge on time");
681  }
682 
683  std::this_thread::sleep_for(std::chrono::milliseconds(100));
684 
685  auto status = (rs2_dsc_status)result.status;
686 
687  // Handle errors from firmware
688  if (status != RS2_DSC_STATUS_SUCCESS)
689  {
690  handle_calibration_error(status);
691  }
692 
693  res = get_calibration_results(health);
694  }
695  }
696  else if (calib_type == 2)
697  {
698  LOG_DEBUG("run_on_chip_calibration with parameters: speed = " << speed_fl
699  << ", keep new value after sucessful scan = " << keep_new_value_after_sucessful_scan
700  << " data_sampling = " << data_sampling << ", adjust both sides = " << adjust_both_sides
701  << ", fl scan location = " << fl_scan_location << ", fy scan direction = " << fy_scan_direction << ", white wall mode = " << white_wall_mode);
702  check_one_button_params(speed, keep_new_value_after_sucessful_scan, data_sampling, adjust_both_sides, fl_scan_location, fy_scan_direction, white_wall_mode);
703 
704  uint32_t p4 = 0;
705  if (scan_parameter)
706  p4 |= 1;
707  if (keep_new_value_after_sucessful_scan)
708  p4 |= (1 << 1);
709  if (fl_data_sampling)
710  p4 |= (1 << 3);
711  if (adjust_both_sides)
712  p4 |= (1 << 4);
713  if (fl_scan_location)
714  p4 |= (1 << 5);
715  if (fy_scan_direction)
716  p4 |= (1 << 6);
717  if (white_wall_mode)
718  p4 |= (1 << 7);
719  if (scan_only_v3)
720  p4 |= (1 << 8);
721  if (interactive_scan_v3)
722  p4 |= (1 << 9);
723 
724  if (speed == speed_white_wall && apply_preset)
725  {
726  preset_recover = change_preset();
727  std::this_thread::sleep_for(std::chrono::milliseconds(200));
728  }
729 
730  // Begin auto-calibration
731  if (host_assistance == host_assistance_type::no_assistance || host_assistance == host_assistance_type::assistance_start)
732  {
733  _hw_monitor->send( command{ ds::AUTO_CALIB,
734  py_rx_plus_fl_calib_begin,
735  static_cast< uint32_t >( speed_fl ),
736  0,
737  p4 } );
738  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ py_rx_plus_fl_calib_begin speed_fl = " << speed_fl << " res size = " << res.size()));
739  }
740 
741  if (host_assistance != host_assistance_type::assistance_start)
742  {
743  if ((host_assistance == host_assistance_type::assistance_first_feed) || (host_assistance == host_assistance_type::assistance_second_feed))
744  {
745  command cmd(ds::AUTO_CALIB, interactive_scan_control, 0, 0);
746  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ interactive_scan_control 0,0"));
747  uint8_t* p = reinterpret_cast<uint8_t*>(&step_count_v3);
748  cmd.data.push_back(p[0]);
749  cmd.data.push_back(p[1]);
750  for (uint16_t i = 0; i < step_count_v3; ++i)
751  {
752  p = reinterpret_cast<uint8_t*>(fill_factor + i);
753  cmd.data.push_back(p[0]);
754  cmd.data.push_back(p[1]);
755  }
756 
757  _hw_monitor->send(cmd);
758  }
759 
760  if (host_assistance != host_assistance_type::assistance_first_feed)
761  {
762  DscPyRxFLCalibrationTableResult result{};
763 
764  int count = 0;
765  bool done = false;
766 
767  auto start = std::chrono::high_resolution_clock::now();
768  auto now = start;
769  float progress = 0.0f;
770 
771  // While not ready...
772  do
773  {
774  std::this_thread::sleep_for(std::chrono::milliseconds(200));
775 
776  // Check calibration status
777  try
778  {
779  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result });
780  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "occ get_py_rx_plus_fl_calib_result res size = " << res.size() ));
781 
782  if (res.size() < sizeof(DscPyRxFLCalibrationTableResult))
783  throw std::runtime_error("Not enough data from CALIB_STATUS!");
784 
785  result = *reinterpret_cast<DscPyRxFLCalibrationTableResult*>(res.data());
786  done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY;
787  }
788  catch (const std::exception& ex)
789  {
790  LOG_WARNING(ex.what());
791  }
792 
793  if (progress_callback)
794  {
795  if (host_assistance != host_assistance_type::no_assistance)
796  {
797  if( count < 20 )
798  progress_callback->on_update_progress( static_cast< float >( 80 + count++ ) );
799  else
800  progress_callback->on_update_progress(count++* (2.f * static_cast<int>(speed))); //curently this number does not reflect the actual progress
801  }
802  }
803 
804  now = std::chrono::high_resolution_clock::now();
805 
806  } while (now - start < std::chrono::milliseconds(timeout_ms) && !done);
807 
808  // If we exit due to timeout, report timeout
809  if (!done)
810  {
811  throw std::runtime_error("Operation timed-out!\n"
812  "Calibration state did not converge on time");
813  }
814 
815  std::this_thread::sleep_for(std::chrono::milliseconds(100));
816 
817  auto status = (rs2_dsc_status)result.status;
818 
819  // Handle errors from firmware
820  if (status != RS2_DSC_STATUS_SUCCESS)
821  handle_calibration_error(status);
822 
823  res = get_PyRxFL_calibration_results(&h_1, &h_2);
824 
825  int health_1 = static_cast<int>(std::abs(h_1) * 1000.0f + 0.5f);
826  health_1 &= 0xFFF;
827 
828  int health_2 = static_cast<int>(std::abs(h_2) * 1000.0f + 0.5f);
829  health_2 &= 0xFFF;
830 
831  int sign = 0;
832  if (h_1 < 0.0f)
833  sign = 1;
834  if (h_2 < 0.0f)
835  sign |= 2;
836 
837  int h = health_1;
838  h |= health_2 << 12;
839  h |= sign << 24;
840  *health = static_cast<float>(h);
841  }
842  }
843  }
844 
845  return res;
846  }
847 
848  std::vector<uint8_t> auto_calibrated::run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float* const health, rs2_update_progress_callback_sptr progress_callback)
849  {
850  int average_step_count = DEFAULT_AVERAGE_STEP_COUNT;
851  int step_count = DEFAULT_STEP_COUNT;
852  int accuracy = DEFAULT_ACCURACY;
853  int speed = DEFAULT_SPEED;
854  int scan_parameter = DEFAULT_SCAN;
855  int data_sampling = DEFAULT_TARE_SAMPLING;
856  int apply_preset = 1;
857  int depth = 0;
858  host_assistance_type host_assistance(host_assistance_type::no_assistance);
859  std::vector<uint8_t> res;
860 
861  //Enforce Thermal Compensation off during Tare calibration
862  volatile thermal_compensation_guard grd(this);
863 
864  if (json.size() > 0)
865  {
866  auto jsn = parse_json(json);
867  try_fetch(jsn, "speed", &speed);
868  try_fetch(jsn, "average step count", &average_step_count);
869  try_fetch(jsn, "step count", &step_count);
870  try_fetch(jsn, "accuracy", &accuracy);
871  try_fetch(jsn, "scan parameter", &scan_parameter);
872  try_fetch(jsn, "data sampling", &data_sampling);
873  try_fetch(jsn, "apply preset", &apply_preset);
874  int tmp_host_assistance(0);
875  try_fetch(jsn, "host assistance", &tmp_host_assistance);
876  if (tmp_host_assistance < (int)host_assistance_type::no_assistance || tmp_host_assistance >(int)host_assistance_type::assistance_second_feed)
877  throw invalid_value_exception( rsutils::string::from()
878  << "Auto calibration failed! Given value of 'host assistance' "
879  << tmp_host_assistance << " is out of range (0 - "
880  << (int)host_assistance_type::assistance_second_feed << ")." );
881  host_assistance = host_assistance_type(tmp_host_assistance);
882  try_fetch(jsn, "depth", &depth);
883  try_fetch(jsn, "resize factor", &_resize_factor);
884  }
885 
886  if (host_assistance != host_assistance_type::no_assistance && _interactive_state == interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE)
887  {
888  _json = json;
889  _ground_truth_mm = ground_truth_mm;
890  _total_frames = step_count;
891  _average_step_count = average_step_count;
892  _action = auto_calib_action::RS2_OCC_ACTION_TARE_CALIB;
893  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START;
894 
895  DirectSearchCalibrationResult result = get_calibration_status(timeout_ms, [progress_callback, host_assistance, speed](int count)
896  {
897  if (progress_callback)
898  {
899  if (host_assistance != host_assistance_type::no_assistance)
900  if (count < 20) progress_callback->on_update_progress(static_cast<float>(80 + count++));
901  else
902  progress_callback->on_update_progress(count++ * (2.f * speed)); //curently this number does not reflect the actual progress
903  }
904  }, false);
905  // Handle errors from firmware
906  rs2_dsc_status status = (rs2_dsc_status)result.status;
907 
908  if (result.maxDepth == 0)
909  {
910  throw std::runtime_error("Firmware calibration values are not yet set.");
911  }
912  _min_valid_depth = result.minDepth;
913  _max_valid_depth = result.maxDepth;
914 
915  return res;
916  }
917 
918  if (depth > 0)
919  {
920  LOG_OCC_WARN("run_tare_calibration interactive control (2) with parameters: depth = " << depth);
921  _hw_monitor->send( command{ ds::AUTO_CALIB,
922  interactive_scan_control,
923  2,
924  static_cast< uint32_t >( depth ) } );
925  }
926  else
927  {
928  std::shared_ptr<ds_advanced_mode_base> preset_recover;
929  if (depth == 0)
930  {
931  if (apply_preset)
932  {
933  if (host_assistance != host_assistance_type::no_assistance)
934  change_preset_and_stay();
935  else
936  preset_recover = change_preset();
937  std::this_thread::sleep_for(std::chrono::milliseconds(200));
938  }
939 
940  LOG_DEBUG("run_tare_calibration with parameters: speed = " << speed << " average_step_count = " << average_step_count << " step_count = " << step_count << " accuracy = " << accuracy << " scan_parameter = " << scan_parameter << " data_sampling = " << data_sampling);
941  check_tare_params(speed, scan_parameter, data_sampling, average_step_count, step_count, accuracy);
942 
943  auto param2 = static_cast< uint32_t >( ground_truth_mm ) * 100;
944 
945  tare_calibration_params param3{ static_cast< uint8_t >( average_step_count ),
946  static_cast< uint8_t >( step_count ),
947  static_cast< uint8_t >( accuracy ),
948  0 };
949 
950  param4 param{ static_cast< uint8_t >( scan_parameter ),
951  0,
952  static_cast< uint8_t >( data_sampling ) };
953 
954  if (host_assistance != host_assistance_type::no_assistance)
955  param.param_4 |= (1 << 8);
956 
957  // Log the current preset
958  auto advanced_mode = dynamic_cast<ds_advanced_mode_base*>(this);
959  if (advanced_mode)
960  {
961  auto cur_preset = (rs2_rs400_visual_preset)(int)advanced_mode->_preset_opt->query();
962  LOG_DEBUG("run_tare_calibration with preset: " << rs2_rs400_visual_preset_to_string(cur_preset));
963  }
964 
965  if (depth == 0)
966  _hw_monitor->send(command{ ds::AUTO_CALIB, tare_calib_begin, param2, param3.param3, param.param_4 });
967  }
968 
969  if (host_assistance == host_assistance_type::no_assistance || depth < 0)
970  {
971  TareCalibrationResult result;
972 
973  // While not ready...
974  int count = 0;
975  bool done = false;
976 
977  auto start = std::chrono::high_resolution_clock::now();
978  auto now = start;
979  do
980  {
981  memset(&result, 0, sizeof(TareCalibrationResult));
982  std::this_thread::sleep_for(std::chrono::milliseconds(200));
983 
984  // Check calibration status
985  try
986  {
987  res = _hw_monitor->send(command{ ds::AUTO_CALIB, tare_calib_check_status });
988  if (res.size() < sizeof(TareCalibrationResult))
989  {
990  if (depth < 0)
991  restore_preset();
992  throw std::runtime_error("Not enough data from CALIB_STATUS!");
993  }
994 
995  result = *reinterpret_cast<TareCalibrationResult*>(res.data());
996  done = result.status != RS2_DSC_STATUS_RESULT_NOT_READY;
997  }
998  catch (const std::exception& ex)
999  {
1000  LOG_INFO(ex.what());
1001  }
1002 
1003  if (progress_callback)
1004  {
1005  if (depth < 0 && count < 20)
1006  progress_callback->on_update_progress(static_cast<float>(80 + count++));
1007  else if (depth == 0)
1008  progress_callback->on_update_progress(count++ * (2.f * speed)); //curently this number does not reflect the actual progress
1009  }
1010 
1011  now = std::chrono::high_resolution_clock::now();
1012 
1013  } while (now - start < std::chrono::milliseconds(timeout_ms) && !done);
1014 
1015  // If we exit due to timeout, report timeout
1016  if (!done)
1017  {
1018  if (depth < 0)
1019  restore_preset();
1020 
1021  throw std::runtime_error("Operation timed-out!\n"
1022  "Calibration did not converge on time");
1023  }
1024 
1025  auto status = (rs2_dsc_status)result.status;
1026 
1027  uint8_t* p = res.data() + sizeof(TareCalibrationResult) + 2 * result.iterations * sizeof(uint32_t);
1028  float* ph = reinterpret_cast<float*>(p);
1029  health[0] = ph[0];
1030  health[1] = ph[1];
1031 
1032  LOG_INFO("Ground truth: " << ground_truth_mm << "mm");
1033  LOG_INFO("Health check numbers from TareCalibrationResult(0x0C): before=" << ph[0] << ", after=" << ph[1]);
1034  LOG_INFO("Z calculated from health check numbers : before=" << (ph[0] + 1) * ground_truth_mm << ", after=" << (ph[1] + 1) * ground_truth_mm);
1035 
1036  // Handle errors from firmware
1037  if (status != RS2_DSC_STATUS_SUCCESS)
1038  handle_calibration_error(status);
1039 
1040  res = get_calibration_results();
1041 
1042  if (depth < 0)
1043  {
1044  restore_preset();
1045  std::this_thread::sleep_for(std::chrono::milliseconds(200));
1046  }
1047  if (progress_callback)
1048  progress_callback->on_update_progress(static_cast<float>(100));
1049  }
1050  }
1051 
1052  return res;
1053  }
1054 
1055  uint16_t auto_calibrated::calc_fill_rate(const rs2_frame* f)
1056  {
1057  auto frame = ((video_frame*)f);
1058  int width = frame->get_width();
1059  int height = frame->get_height();
1060  int roi_w = width / _resize_factor;
1061  int roi_h = height / _resize_factor;
1062  int roi_start_w = (width - roi_w) / 2;
1063  int roi_start_h = (height - roi_h) / 2;
1064  int from = roi_start_h;
1065  int to = roi_start_h + roi_h;
1066  int roi_size = roi_w * roi_h;
1067  int data_size = roi_size;
1068  const uint16_t* p = reinterpret_cast<const uint16_t*>(frame->get_frame_data());
1069 
1070 #ifdef SAVE_RAW_IMAGE
1071  std::vector<uint16_t> cropped_image(width * height, 0);
1072  int cropped_idx(0);
1073  cropped_idx += from * width + roi_start_w;
1074 #endif
1075 
1076  p += from * width + roi_start_w;
1077 
1078  int counter(0);
1079  for (int j = from; j < to; ++j)
1080  {
1081  for (int i = 0; i < roi_w; ++i)
1082  {
1083 #ifdef SAVE_RAW_IMAGE
1084  cropped_image[cropped_idx] = (*p);
1085  cropped_idx++;
1086 #endif
1087  if ((*p) >= _min_valid_depth && (*p) <= _max_valid_depth)
1088  ++counter;
1089  ++p;
1090  }
1091  p += (width - roi_w);
1092 #ifdef SAVE_RAW_IMAGE
1093  cropped_idx += (width - roi_w);
1094 #endif
1095  }
1096 #ifdef SAVE_RAW_IMAGE
1097  {
1098  unsigned long milliseconds_since_epoch =
1099  std::chrono::duration_cast<std::chrono::milliseconds>
1100  (std::chrono::system_clock::now().time_since_epoch()).count();
1101 
1102  std::stringstream name_s;
1103  name_s << "cropped_image_" << std::setfill('0') << std::setw(4) << milliseconds_since_epoch << "_" << frame->get_frame_number() << ".raw";
1104  std::ofstream fout(name_s.str(), std::ios::out | std::ios::binary);
1105  fout.write((char*)&cropped_image[0], cropped_image.size() * sizeof(uint16_t));
1106  fout.close();
1107  }
1108 #endif
1109  double tmp = static_cast<double>(counter) / static_cast<double>(data_size) * 10000.0;
1110  return static_cast<uint16_t>(tmp + 0.5f);
1111 
1112  }
1113 
1114  // fill_missing_data:
1115  // Fill every zeros section linearly based on the section's edges.
1116  void auto_calibrated::fill_missing_data(uint16_t data[256], int size)
1117  {
1118  int counter = 0;
1119  int start = 0;
1120  while (data[start++] == 0)
1121  ++counter;
1122 
1123  if (start + 2 > size)
1124  throw std::runtime_error( rsutils::string::from() << "There is no enought valid data in the array!" );
1125 
1126  for (int i = 0; i < counter; ++i)
1127  data[i] = data[counter];
1128 
1129  start = 0;
1130  int end = 0;
1131  float tmp = 0;
1132  for (int i = 0; i < size; ++i)
1133  {
1134  if (data[i] == 0)
1135  start = i;
1136 
1137  if (start != 0 && data[i] != 0)
1138  end = i;
1139 
1140  if (start != 0 && end != 0)
1141  {
1142  tmp = static_cast<float>(data[end] - data[start - 1]);
1143  tmp /= end - start + 1;
1144  for (int j = start; j < end; ++j)
1145  data[j] = static_cast<uint16_t>(tmp * (j - start + 1) + data[start - 1] + 0.5f);
1146  start = 0;
1147  end = 0;
1148  }
1149  }
1150 
1151  if (start != 0 && end == 0)
1152  {
1153  for (int i = start; i < size; ++i)
1154  data[i] = data[start - 1];
1155  }
1156  }
1157 
1158  void auto_calibrated::remove_outliers(uint16_t data[256], int size)
1159  {
1160  //Due to the async between the preset activation and the flow, the initial frames of the sample may include unrelated data.
1161  // the purpose of the function is to eliminate those by replacing them with a single value.
1162  // This assumes that the fill_rate is contiguous during scan (i.e. grows or shrinks monotonically)
1163  // Additionally, this function rectifies singular sporadic outliers which are in the top 5% that may skew the results
1164  uint16_t base_fr = 0;
1165  for (int i = 255; i >= 0; i--)
1166  {
1167  // Initialize reference value
1168  if (!base_fr)
1169  {
1170  if (data[i])
1171  base_fr = data[i];
1172  continue;
1173  }
1174 
1175  // Rectify missing values
1176  if (!data[i])
1177  data[i] = base_fr;
1178  }
1179 
1180  static const int _outlier_percentile = 9500; // The outlier value is expected to be significantly above this value
1181  for (int i = 0; i <= 253; i++) // Check for single outliers by assessing triples
1182  {
1183  auto val1 = data[i];
1184  auto val2 = data[i+1];
1185  auto val3 = data[i+2];
1186 
1187  // Check for rectification candidate
1188  if ((val2 > val1) && (val2 > val3))
1189  {
1190  auto diff = val3-val1;
1191  auto delta = std::max(std::abs(val2-val1),std::abs(val2-val3));
1192  // Actual outlier is a
1193  // - spike 3 times or more than the expected gap
1194  // - in the 5 top percentile
1195  // - with neighbour values being smaller by at least 500 points to avoid clamping around the peak
1196  if ((delta > 500) && (delta > (std::abs(diff) * 3)) && (val2 > _outlier_percentile))
1197  {
1198  data[i+1] = data[i] + diff/2;
1199  LOG_OCC_WARN(std::string(rsutils::string::from() << "Outlier with value " << val2 << " was changed to be " << data[i+1] ));
1200  }
1201  }
1202  }
1203  }
1204 
1205  // get_depth_frame_sum:
1206  // Function sums the pixels in the image ROI - Add the collected avarage parameters to _collected_counter, _collected_sum.
1207  void auto_calibrated::collect_depth_frame_sum(const rs2_frame* f)
1208  {
1209  auto frame = ((video_frame*)f);
1210  int width = frame->get_width();
1211  int height = frame->get_height();
1212  int roi_w = width / _resize_factor;
1213  int roi_h = height / _resize_factor;
1214  int roi_start_w = (width - roi_w) / 2;
1215  int roi_start_h = (height - roi_h) / 2;
1216 
1217  const uint16_t* p = reinterpret_cast<const uint16_t*>(frame->get_frame_data());
1218 
1219 #ifdef SAVE_RAW_IMAGE
1220  std::vector<uint16_t> origin_image(width * height, 0);
1221  for (int ii = 0; ii < width * height; ii++)
1222  origin_image[ii] = *(p + ii);
1223 
1224  {
1225  unsigned long milliseconds_since_epoch =
1226  std::chrono::duration_cast<std::chrono::milliseconds>
1227  (std::chrono::system_clock::now().time_since_epoch()).count();
1228 
1229  std::stringstream name_s;
1230  name_s << "origin_tare_image_" << std::setfill('0') << std::setw(4) << milliseconds_since_epoch << "_" << frame->get_frame_number() << ".raw";
1231 
1232  std::ofstream fout(name_s.str(), std::ios::out | std::ios::binary);
1233  fout.write((char*)&origin_image[0], origin_image.size() * sizeof(uint16_t));
1234  fout.close();
1235  }
1236  std::vector<uint16_t> cropped_image(width * height, 0);
1237  int cropped_idx(0);
1238  cropped_idx += roi_start_h * width + roi_start_w;
1239 #endif
1240 
1241  p += roi_start_h * width + roi_start_w;
1242 
1243  for (int j = 0; j < roi_h; ++j)
1244  {
1245  for (int i = 0; i < roi_w; ++i)
1246  {
1247 #ifdef SAVE_RAW_IMAGE
1248  cropped_image[cropped_idx] = (*p);
1249  cropped_idx++;
1250 #endif
1251  if ((*p) >= _min_valid_depth && (*p) <= _max_valid_depth)
1252  {
1253  ++_collected_counter;
1254  _collected_sum += *p;
1255  }
1256  ++p;
1257  }
1258  p += (width- roi_w);
1259 #ifdef SAVE_RAW_IMAGE
1260  cropped_idx += (width - roi_w);
1261 #endif
1262  }
1263 #ifdef SAVE_RAW_IMAGE
1264  {
1265  unsigned long milliseconds_since_epoch =
1266  std::chrono::duration_cast<std::chrono::milliseconds>
1267  (std::chrono::system_clock::now().time_since_epoch()).count();
1268 
1269  std::stringstream name_s;
1270  name_s << "cropped_tare_image_" << std::setfill('0') << std::setw(4) << milliseconds_since_epoch << "_" << frame->get_frame_number() << ".raw";
1271 
1272  std::ofstream fout(name_s.str(), std::ios::out | std::ios::binary);
1273  fout.write((char*)&cropped_image[0], cropped_image.size() * sizeof(uint16_t));
1274  fout.close();
1275  }
1276 #endif
1277  }
1278 
1279  std::vector<uint8_t> auto_calibrated::process_calibration_frame(int timeout_ms, const rs2_frame* f, float* const health, rs2_update_progress_callback_sptr progress_callback)
1280  {
1281  try
1282  {
1283  auto fi = (frame_interface *)f;
1284  std::vector<uint8_t> res;
1285  rs2_metadata_type frame_counter;
1286  if( ! fi->find_metadata( RS2_FRAME_METADATA_FRAME_COUNTER, &frame_counter ) )
1287  throw invalid_value_exception( "missing FRAME_COUNTER" );
1288  rs2_metadata_type frame_ts;
1289  if( ! fi->find_metadata( RS2_FRAME_METADATA_FRAME_TIMESTAMP, &frame_ts ) )
1290  throw invalid_value_exception( "missing FRAME_TIMESTAMP" );
1291  bool tare_fc_workaround = (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB); //Tare calib shall use rolling frame counter
1292  bool mipi_sku = fi->find_metadata( RS2_FRAME_METADATA_CALIB_INFO, &frame_counter );
1293 
1294  if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START)
1295  {
1296  if (frame_counter <= 2)
1297  {
1298  return res;
1299  }
1300  if (progress_callback)
1301  {
1302  progress_callback->on_update_progress(static_cast<float>(10));
1303  }
1304  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_INITIAL_FW_CALL;
1305  }
1306  if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_INITIAL_FW_CALL)
1307  {
1308  if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB)
1309  {
1310  res = run_tare_calibration(timeout_ms, _ground_truth_mm, _json, health, progress_callback);
1311  }
1312  else if (_action == auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB)
1313  {
1314  res = run_on_chip_calibration(timeout_ms, _json, health, progress_callback);
1315  }
1316  _prev_frame_counter = frame_counter;
1317  if ((!tare_fc_workaround) && mipi_sku) _prev_frame_counter +=10; // Compensate for MIPI OCC calib. invoke delay
1318  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START;
1319  LOG_OCC_WARN(std::string(rsutils::string::from() << "switch INITIAL_FW_CALL=>WAIT_TO_CALIB_START, prev_fc is reset to " << _prev_frame_counter));
1320  return res;
1321  }
1322  if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START)
1323  {
1324  bool still_waiting(frame_counter >= _prev_frame_counter || frame_counter >= _total_frames);
1325  _prev_frame_counter = frame_counter;
1326  if (still_waiting)
1327  {
1328  if (progress_callback)
1329  {
1330  progress_callback->on_update_progress(static_cast<float>(15));
1331  }
1332  return res;
1333  }
1334  if (progress_callback)
1335  {
1336  progress_callback->on_update_progress(static_cast<float>(20));
1337  }
1338  _collected_counter = 0;
1339  _collected_sum = 0;
1340  _collected_frame_num = 0;
1341  _skipped_frames = 0;
1342  _prev_frame_counter = -1;
1343  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT;
1344  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " switch to RS2_OCC_STATE_DATA_COLLECT"));
1345  }
1346  if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT)
1347  {
1348  if (_action == auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB)
1349  {
1350 #ifdef SAVE_RAW_IMAGE
1351  {
1352  unsigned long milliseconds_since_epoch =
1353  std::chrono::duration_cast<std::chrono::milliseconds>
1354  (std::chrono::system_clock::now().time_since_epoch()).count();
1355 
1356  std::stringstream name_s;
1357  name_s << "all_frame_numbers.txt";
1358  std::ofstream fout(name_s.str(), std::ios::app);
1359  fout << frame_counter << ", " << _prev_frame_counter << ", " << _total_frames << ", "
1360  << ((frame_interface*)f)->get_frame_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP) << ", " << milliseconds_since_epoch << std::endl;
1361  fout.close();
1362  }
1363 #endif
1364 #ifdef SAVE_RAW_IMAGE
1365  {
1366  auto frame = ((video_frame*)f);
1367  int width = frame->get_width();
1368  int height = frame->get_height();
1369  const uint16_t* p = reinterpret_cast<const uint16_t*>(frame->get_frame_data());
1370  {
1371  unsigned long milliseconds_since_epoch =
1372  std::chrono::duration_cast<std::chrono::milliseconds>
1373  (std::chrono::system_clock::now().time_since_epoch()).count();
1374 
1375  std::stringstream name_s;
1376  name_s << "origin_image_" << std::setfill('0') << std::setw(4) << milliseconds_since_epoch << "_" << frame->get_frame_number() << ".raw";
1377 
1378  std::ofstream fout(name_s.str(), std::ios::out | std::ios::binary);
1379  fout.write((char*)p, width * height * sizeof(uint16_t));
1380  fout.close();
1381  }
1382  }
1383 #endif
1384 
1385  static const int FRAMES_TO_SKIP(_interactive_scan ? 1 : 0);
1386  int fw_host_offset = (_interactive_scan ? 0 : 1);
1387 
1388  if (frame_counter + fw_host_offset < _total_frames)
1389  {
1390  if (frame_counter < _prev_frame_counter)
1391  {
1392  throw std::runtime_error("Frames arrived in a wrong order!");
1393  }
1394  if (_skipped_frames < FRAMES_TO_SKIP || frame_counter == _prev_frame_counter)
1395  {
1396  _skipped_frames++;
1397  }
1398  else
1399  {
1400  if (progress_callback)
1401  {
1402  progress_callback->on_update_progress(static_cast<float>(20 + static_cast<int>(frame_counter * 60.0 / _total_frames)));
1403  }
1404  auto fill_rate = calc_fill_rate(f);
1405  if (frame_counter < 10) // handle discrepancy on stream/preset activation
1406  fill_rate = 0;
1407  if (frame_counter + fw_host_offset < 256)
1408  {
1409  _fill_factor[frame_counter + fw_host_offset] = fill_rate;
1410  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " fc = " << frame_counter << ", _fill_factor[" << frame_counter + fw_host_offset << "] = " << fill_rate));
1411  }
1412 
1413  if (_interactive_scan)
1414  {
1415  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1});
1416  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " occ interactive_scan_control 1,"));
1417  }
1418  _skipped_frames = 0;
1419  _prev_frame_counter = frame_counter;
1420  }
1421  }
1422  else
1423  {
1424  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL;
1425  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " go to final FW call"));
1426  }
1427  }
1428  else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB)
1429  {
1430  static const int FRAMES_TO_SKIP(1);
1431  if (frame_counter != _prev_frame_counter)
1432  {
1433  _collected_counter = 0;
1434  _collected_sum = 0.0;
1435  _collected_frame_num = 0;
1436  _skipped_frames = 0;
1437  if (progress_callback)
1438  {
1439  double progress_rate = std::min(1.0, static_cast<double>(frame_counter) / _total_frames);
1440  progress_callback->on_update_progress(static_cast<float>(20 + static_cast<int>(progress_rate * 60.0)));
1441  }
1442  }
1443  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " fr_c = " << frame_counter
1444  << " fr_ts = " << frame_ts << " _c_f_num = " << _collected_frame_num));
1445 
1446  if (frame_counter < _total_frames)
1447  {
1448  if (_skipped_frames < FRAMES_TO_SKIP)
1449  {
1450  _skipped_frames++;
1451  }
1452  else
1453  {
1454  if (_collected_frame_num < _average_step_count)
1455  {
1456  collect_depth_frame_sum(f);
1457  if (_collected_counter && (_collected_frame_num + 1) == _average_step_count)
1458  {
1459  _collected_sum = (_collected_sum / _collected_counter) * 10000;
1460  int depth = static_cast<int>(_collected_sum + 0.5);
1461 
1462  std::stringstream ss;
1463  ss << "{\n \"depth\":" << depth << "}";
1464 
1465  std::string json = ss.str();
1466  run_tare_calibration(timeout_ms, _ground_truth_mm, json, health, progress_callback);
1467  }
1468  }
1469  ++_collected_frame_num;
1470  }
1471  _prev_frame_counter = frame_counter;
1472  }
1473  else
1474  {
1475  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL;
1476  }
1477  }
1478  }
1479  if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL)
1480  {
1481  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " :RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL"));
1482  if (frame_counter > _total_frames)
1483  {
1484  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL;
1485  }
1486  else if (_interactive_scan)
1487  {
1488  _hw_monitor->send(command{ ds::AUTO_CALIB, interactive_scan_control, 1 });
1489  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << "Call for interactive_scan_control, 1"));
1490  }
1491  }
1492  if (_interactive_state == interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL)
1493  {
1494  LOG_OCC_WARN(std::string(rsutils::string::from() << __LINE__ << " :RS2_OCC_STATE_FINAL_FW_CALL"));
1495  if (progress_callback)
1496  {
1497  progress_callback->on_update_progress(static_cast<float>(80));
1498  }
1499  if (_action == auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB)
1500  {
1501 #ifdef SAVE_RAW_IMAGE
1502  {
1503  std::stringstream ss;
1504  ss << "{\n \"calib type\":" << 0 <<
1505  ",\n \"host assistance\":" << 2 <<
1506  ",\n \"step count v3\":" << _total_frames;
1507  for (int i = 0; i < _total_frames; ++i)
1508  ss << ",\n \"fill factor " << i << "\":" << _fill_factor[i];
1509  ss << "}";
1510 
1511  std::stringstream name_s;
1512  name_s << "fill_factor_before_fill.txt";
1513  std::ofstream fout(name_s.str(), std::ios::out);
1514  fout << ss.str();
1515  fout.close();
1516  }
1517 #endif
1518  // Do not delete - to be used to improve algo robustness
1519  fill_missing_data(_fill_factor, _total_frames);
1520  remove_outliers(_fill_factor, _total_frames);
1521  std::stringstream ss;
1522  ss << "{\n \"calib type\":" << 0 <<
1523  ",\n \"host assistance\":" << 2 <<
1524  ",\n \"step count v3\":" << _total_frames;
1525  for (int i = 0; i < _total_frames; ++i)
1526  ss << ",\n \"fill factor " << i << "\":" << _fill_factor[i];
1527  ss << "}";
1528 
1529  std::string json = ss.str();
1530 #ifdef SAVE_RAW_IMAGE
1531  {
1532  std::stringstream name_s;
1533  name_s << "fill_factor.txt";
1534  std::ofstream fout(name_s.str(), std::ios::out);
1535  fout << json;
1536  fout.close();
1537  }
1538 #endif
1539 
1540  res = run_on_chip_calibration(timeout_ms, json, health, progress_callback);
1541  }
1542  else if (_action == auto_calib_action::RS2_OCC_ACTION_TARE_CALIB)
1543  {
1544  std::stringstream ss;
1545  ss << "{\n \"depth\":" << -1 << "}";
1546 
1547  std::string json = ss.str();
1548  res = run_tare_calibration(timeout_ms, _ground_truth_mm, json, health, progress_callback);
1549  }
1550  if (progress_callback)
1551  {
1552  progress_callback->on_update_progress(static_cast<float>(100));
1553  }
1554  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE;
1555  }
1556  return res;
1557  }
1558  catch (const std::exception&)
1559  {
1560  _interactive_state = interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE;
1561  throw;
1562  }
1563  }
1564 
1565 
1566  std::shared_ptr<ds_advanced_mode_base> auto_calibrated::change_preset()
1567  {
1568  preset old_preset_values{};
1569  rs2_rs400_visual_preset old_preset = { RS2_RS400_VISUAL_PRESET_DEFAULT };
1570 
1571  auto advanced_mode = dynamic_cast<ds_advanced_mode_base*>(this);
1572  if (advanced_mode)
1573  {
1574  old_preset = (rs2_rs400_visual_preset)(int)advanced_mode->_preset_opt->query();
1575  if (old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM)
1576  old_preset_values = advanced_mode->get_all();
1577  advanced_mode->_preset_opt->set(RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY);
1578  }
1579 
1580  std::shared_ptr<ds_advanced_mode_base> recover_preset(advanced_mode, [old_preset, advanced_mode, old_preset_values](ds_advanced_mode_base* adv)
1581  {
1582  if (old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM)
1583  {
1584  advanced_mode->_preset_opt->set(RS2_RS400_VISUAL_PRESET_CUSTOM);
1585  adv->set_all(old_preset_values);
1586  }
1587  else
1588  advanced_mode->_preset_opt->set(static_cast<float>(old_preset));
1589  });
1590 
1591  return recover_preset;
1592  }
1593 
1594  void auto_calibrated::change_preset_and_stay()
1595  {
1596  auto advanced_mode = dynamic_cast<ds_advanced_mode_base*>(this);
1597  if (advanced_mode)
1598  {
1599  _old_preset = (rs2_rs400_visual_preset)(int)advanced_mode->_preset_opt->query();
1600  if (_old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM)
1601  _old_preset_values = advanced_mode->get_all();
1602  advanced_mode->_preset_opt->set(RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY);
1603  _preset_change = true;
1604  }
1605  }
1606 
1607  void auto_calibrated::restore_preset()
1608  {
1609  if (_preset_change)
1610  {
1611  auto advanced_mode = dynamic_cast<ds_advanced_mode_base*>(this);
1612  if (!advanced_mode)
1613  throw std::runtime_error("Can not cast to advance mode base");
1614 
1615  if (_old_preset == RS2_RS400_VISUAL_PRESET_CUSTOM)
1616  {
1617  advanced_mode->_preset_opt->set(RS2_RS400_VISUAL_PRESET_CUSTOM);
1618  advanced_mode->set_all(_old_preset_values);
1619  }
1620  else
1621  advanced_mode->_preset_opt->set(static_cast<float>(_old_preset));
1622  }
1623  _preset_change = false;
1624  }
1625 
1626  void auto_calibrated::check_params(int speed, int scan_parameter, int data_sampling) const
1627  {
1628  if (speed < speed_very_fast || speed > speed_white_wall)
1629  throw invalid_value_exception( rsutils::string::from()
1630  << "Auto calibration failed! Given value of 'speed' " << speed
1631  << " is out of range (0 - 4)." );
1632  if (scan_parameter != py_scan && scan_parameter != rx_scan)
1633  throw invalid_value_exception( rsutils::string::from()
1634  << "Auto calibration failed! Given value of 'scan parameter' "
1635  << scan_parameter << " is out of range (0 - 1)." );
1636  if (data_sampling != polling && data_sampling != interrupt)
1637  throw invalid_value_exception( rsutils::string::from()
1638  << "Auto calibration failed! Given value of 'data sampling' " << data_sampling
1639  << " is out of range (0 - 1)." );
1640  }
1641 
1642  void auto_calibrated::check_tare_params(int speed, int scan_parameter, int data_sampling, int average_step_count, int step_count, int accuracy)
1643  {
1644  check_params(speed, scan_parameter, data_sampling);
1645 
1646  if (average_step_count < 1 || average_step_count > 30)
1647  throw invalid_value_exception( rsutils::string::from()
1648  << "Auto calibration failed! Given value of 'number of frames to average' "
1649  << average_step_count << " is out of range (1 - 30)." );
1650  if (step_count < 5 || step_count > 30)
1651  throw invalid_value_exception( rsutils::string::from()
1652  << "Auto calibration failed! Given value of 'max iteration steps' "
1653  << step_count << " is out of range (5 - 30)." );
1654  if (accuracy < very_high || accuracy > low)
1655  throw invalid_value_exception( rsutils::string::from()
1656  << "Auto calibration failed! Given value of 'subpixel accuracy' " << accuracy
1657  << " is out of range (0 - 3)." );
1658  }
1659 
1660  void auto_calibrated::check_focal_length_params(int step_count, int fy_scan_range, int keep_new_value_after_sucessful_scan, int interrrupt_data_samling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const
1661  {
1662  if (step_count < 8 || step_count > 256)
1663  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'step_count' " << step_count << " is out of range (8 - 256).");
1664  if (fy_scan_range < 1 || fy_scan_range > 60000)
1665  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'fy_scan_range' " << fy_scan_range << " is out of range (1 - 60000).");
1666  if (keep_new_value_after_sucessful_scan < 0 || keep_new_value_after_sucessful_scan > 1)
1667  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'keep_new_value_after_sucessful_scan' " << keep_new_value_after_sucessful_scan << " is out of range (0 - 1).");
1668  if (interrrupt_data_samling < 0 || interrrupt_data_samling > 1)
1669  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'interrrupt_data_samling' " << interrrupt_data_samling << " is out of range (0 - 1).");
1670  if (adjust_both_sides < 0 || adjust_both_sides > 1)
1671  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'adjust_both_sides' " << adjust_both_sides << " is out of range (0 - 1).");
1672  if (fl_scan_location < 0 || fl_scan_location > 1)
1673  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'fl_scan_location' " << fl_scan_location << " is out of range (0 - 1).");
1674  if (fy_scan_direction < 0 || fy_scan_direction > 1)
1675  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'fy_scan_direction' " << fy_scan_direction << " is out of range (0 - 1).");
1676  if (white_wall_mode < 0 || white_wall_mode > 1)
1677  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'white_wall_mode' " << white_wall_mode << " is out of range (0 - 1).");
1678  }
1679 
1680  void auto_calibrated::check_one_button_params(int speed, int keep_new_value_after_sucessful_scan, int data_sampling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const
1681  {
1682  if (speed < speed_very_fast || speed > speed_white_wall)
1683  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'speed' " << speed << " is out of range (0 - 4).");
1684  if (keep_new_value_after_sucessful_scan < 0 || keep_new_value_after_sucessful_scan > 1)
1685  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'keep_new_value_after_sucessful_scan' " << keep_new_value_after_sucessful_scan << " is out of range (0 - 1).");
1686  if (data_sampling != polling && data_sampling != interrupt)
1687  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'data sampling' " << data_sampling << " is out of range (0 - 1).");
1688  if (adjust_both_sides < 0 || adjust_both_sides > 1)
1689  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'adjust_both_sides' " << adjust_both_sides << " is out of range (0 - 1).");
1690  if (fl_scan_location < 0 || fl_scan_location > 1)
1691  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'fl_scan_location' " << fl_scan_location << " is out of range (0 - 1).");
1692  if (fy_scan_direction < 0 || fy_scan_direction > 1)
1693  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'fy_scan_direction' " << fy_scan_direction << " is out of range (0 - 1).");
1694  if (white_wall_mode < 0 || white_wall_mode > 1)
1695  throw invalid_value_exception( rsutils::string::from() << "Auto calibration failed! Given value of 'white_wall_mode' " << white_wall_mode << " is out of range (0 - 1).");
1696  }
1697 
1698  void auto_calibrated::handle_calibration_error(int status) const
1699  {
1700  if (status == RS2_DSC_STATUS_EDGE_TOO_CLOSE)
1701  {
1702  throw std::runtime_error("Calibration didn't converge! - edges too close\n"
1703  "Please retry in different lighting conditions");
1704  }
1705  else if (status == RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW)
1706  {
1707  throw std::runtime_error("Not enough depth pixels! - low fill factor)\n"
1708  "Please retry in different lighting conditions");
1709  }
1710  else if (status == RS2_DSC_STATUS_NOT_CONVERGE)
1711  {
1712  throw std::runtime_error("Calibration failed to converge\n"
1713  "Please retry in different lighting conditions");
1714  }
1715  else if (status == RS2_DSC_STATUS_NO_DEPTH_AVERAGE)
1716  {
1717  throw std::runtime_error("Calibration didn't converge! - no depth average\n"
1718  "Please retry in different lighting conditions");
1719  }
1720  else
1721  throw std::runtime_error( rsutils::string::from()
1722  << "Calibration didn't converge! (RESULT=" << int( status ) << ")" );
1723  }
1724 
1725  std::vector<uint8_t> auto_calibrated::get_calibration_results(float* const health) const
1726  {
1727  using namespace ds;
1728 
1729  // Get new calibration from the firmware
1730  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_calibration_result});
1731  if (res.size() < sizeof(DscResultBuffer))
1732  throw std::runtime_error("Not enough data from CALIB_STATUS!");
1733 
1734  auto reslt = (DscResultBuffer*)(res.data());
1735 
1736  table_header* header = reinterpret_cast<table_header*>(res.data() + sizeof(DscResultBuffer));
1737 
1738  if (res.size() < sizeof(DscResultBuffer) + sizeof(table_header) + header->table_size)
1739  throw std::runtime_error("Table truncated in CALIB_STATUS!");
1740 
1741  std::vector<uint8_t> calib;
1742 
1743  calib.resize(sizeof(table_header) + header->table_size, 0);
1744  memcpy(calib.data(), header, calib.size()); // Copy to new_calib
1745 
1746  if(health)
1747  *health = reslt->m_dscResultParams.m_healthCheck;
1748 
1749  return calib;
1750  }
1751 
1752  std::vector<uint8_t> auto_calibrated::get_PyRxFL_calibration_results(float* const health, float* health_fl) const
1753  {
1754  using namespace ds;
1755 
1756  // Get new calibration from the firmware
1757  auto res = _hw_monitor->send(command{ ds::AUTO_CALIB, get_py_rx_plus_fl_calib_result });
1758  if (res.size() < sizeof(DscPyRxFLCalibrationTableResult))
1759  throw std::runtime_error("Not enough data from CALIB_STATUS!");
1760 
1761  auto reslt = (DscPyRxFLCalibrationTableResult*)(res.data());
1762 
1763  table_header* header = reinterpret_cast<table_header*>(res.data() + sizeof(DscPyRxFLCalibrationTableResult));
1764 
1765  if (res.size() < sizeof(DscPyRxFLCalibrationTableResult) + sizeof(table_header) + header->table_size)
1766  throw std::runtime_error("Table truncated in CALIB_STATUS!");
1767 
1768  std::vector<uint8_t> calib;
1769  calib.resize(sizeof(table_header) + header->table_size, 0);
1770  memcpy(calib.data(), header, calib.size()); // Copy to new_calib
1771 
1772  if (health_fl)
1773  *health_fl = reslt->FL_heathCheck;
1774 
1775  if (health)
1776  *health = reslt->healthCheck;
1777 
1778  return calib;
1779  }
1780 
1781  std::vector<uint8_t> auto_calibrated::get_calibration_table() const
1782  {
1783  using namespace ds;
1784 
1785  std::vector<uint8_t> res;
1786 
1787  // Fetch current calibration using GETINITCAL command
1788  command cmd(ds::GETINTCAL, static_cast<int>(ds::d400_calibration_table_id::coefficients_table_id));
1789  auto calib = _hw_monitor->send(cmd);
1790 
1791  if (calib.size() < sizeof(table_header)) throw std::runtime_error("Missing calibration header from GETINITCAL!");
1792 
1793  auto table = (uint8_t*)(calib.data() + sizeof(table_header));
1794  auto hd = (table_header*)(calib.data());
1795 
1796  if (calib.size() < sizeof(table_header) + hd->table_size)
1797  throw std::runtime_error("Table truncated from GETINITCAL!");
1798 
1799  res.resize(sizeof(table_header) + hd->table_size, 0);
1800  memcpy(res.data(), hd, res.size()); // Copy to old_calib
1801 
1802  return res;
1803  }
1804 
1805  void auto_calibrated::write_calibration() const
1806  {
1807  using namespace ds;
1808  if(_curr_calibration.size() <= sizeof(table_header))
1809  throw std::runtime_error("Write calibration can be called only after set calibration table was called");
1810 
1811  table_header* hd = (table_header*)(_curr_calibration.data());
1812  d400_calibration_table_id tbl_id = static_cast<d400_calibration_table_id>(hd->table_type);
1813  fw_cmd cmd{};
1814  uint32_t param2 = 0;
1815  switch (tbl_id)
1816  {
1817  case d400_calibration_table_id::coefficients_table_id:
1818  cmd = SETINTCAL;
1819  break;
1820  case d400_calibration_table_id::rgb_calibration_id:
1821  cmd = SETINTCALNEW;
1822  param2 = 1;
1823  break;
1824  default:
1825  throw std::runtime_error( rsutils::string::from() << "Flashing calibration table type 0x" << std::hex
1826  << static_cast<int>(tbl_id) << " is not supported" );
1827  }
1828 
1829  command write_calib(cmd, static_cast<int>(tbl_id), param2);
1830  write_calib.data = _curr_calibration;
1831  _hw_monitor->send(write_calib);
1832 
1833  LOG_DEBUG("Flashing " << ((tbl_id == d400_calibration_table_id::coefficients_table_id) ? "Depth" : "RGB") << " calibration table");
1834 
1835  }
1836 
1837  void auto_calibrated::set_calibration_table(const std::vector<uint8_t>& calibration)
1838  {
1839  using namespace ds;
1840 
1841  table_header* hd = (table_header*)(calibration.data());
1842  ds::d400_calibration_table_id tbl_id = static_cast<ds::d400_calibration_table_id >(hd->table_type);
1843 
1844  switch (tbl_id)
1845  {
1846  case d400_calibration_table_id::coefficients_table_id: // Load the modified depth calib table into flash RAM
1847  {
1848  uint8_t* table = (uint8_t*)(calibration.data() + sizeof(table_header));
1849  command write_calib(ds::CALIBRECALC, 0, 0, 0, 0xcafecafe);
1850  write_calib.data.insert(write_calib.data.end(), (uint8_t*)table, ((uint8_t*)table) + hd->table_size);
1851  try
1852  {
1853  _hw_monitor->send(write_calib);
1854  }
1855  catch(...)
1856  {
1857  LOG_ERROR("Flashing coefficients_table_id failed");
1858  }
1859  }
1860  // case fall-through by design. For RGB skip loading to RAM (not supported)
1861  case d400_calibration_table_id::rgb_calibration_id:
1862  _curr_calibration = calibration;
1863  break;
1864  default:
1865  throw std::runtime_error( rsutils::string::from()
1866  << "the operation is not defined for calibration table type " << static_cast<int>(tbl_id));
1867  }
1868  }
1869 
1870  void auto_calibrated::reset_to_factory_calibration() const
1871  {
1872  command cmd(ds::fw_cmd::CAL_RESTORE_DFLT);
1873  _hw_monitor->send(cmd);
1874  }
1875 
1876  void auto_calibrated::get_target_rect_info(rs2_frame_queue* frames, float rect_sides[4], float& fx, float& fy, int progress, rs2_update_progress_callback_sptr progress_callback)
1877  {
1878  fx = -1.0f;
1879  std::vector<std::array<float, 4>> rect_sides_arr;
1880 
1881  rs2_error* e = nullptr;
1882  rs2_frame* f = nullptr;
1883 
1884  int queue_size = rs2_frame_queue_size(frames, &e);
1885  if (queue_size==0)
1886  throw std::runtime_error("Extract target rectangle info - no frames in input queue!");
1887  int fc = 0;
1888  while ((fc++ < queue_size) && rs2_poll_for_frame(frames, &f, &e))
1889  {
1890  rs2::frame ff(f);
1891  if (ff.get_data())
1892  {
1893  if (fx < 0.0f)
1894  {
1895  auto p = ff.get_profile();
1896  auto vsp = p.as<rs2::video_stream_profile>();
1897  rs2_intrinsics intrin = vsp.get_intrinsics();
1898  fx = intrin.fx;
1899  fy = intrin.fy;
1900  }
1901 
1902  std::array< float, 4 > rec_sides_cur{};
1903  rs2_extract_target_dimensions(f, RS2_CALIB_TARGET_ROI_RECT_GAUSSIAN_DOT_VERTICES, rec_sides_cur.data(), 4, &e);
1904  if (e)
1905  throw std::runtime_error("Failed to extract target information\nfrom the captured frames!");
1906  rect_sides_arr.emplace_back(rec_sides_cur);
1907  }
1908 
1909  rs2_release_frame(f);
1910 
1911  if (progress_callback)
1912  progress_callback->on_update_progress(static_cast<float>(++progress));
1913  }
1914 
1915  if (rect_sides_arr.size())
1916  {
1917  for (int i = 0; i < 4; ++i)
1918  rect_sides[i] = rect_sides_arr[0][i];
1919 
1920  for (int j = 1; j < rect_sides_arr.size(); ++j)
1921  {
1922  for (int i = 0; i < 4; ++i)
1923  rect_sides[i] += rect_sides_arr[j][i];
1924  }
1925 
1926  for (int i = 0; i < 4; ++i)
1927  rect_sides[i] /= rect_sides_arr.size();
1928  }
1929  else
1930  throw std::runtime_error("Failed to extract the target rectangle info!");
1931  }
1932 
1933  std::vector<uint8_t> auto_calibrated::run_focal_length_calibration(rs2_frame_queue* left, rs2_frame_queue* right, float target_w, float target_h,
1934  int adjust_both_sides, float *ratio, float * angle, rs2_update_progress_callback_sptr progress_callback)
1935  {
1936  float fx[2] = { -1.0f, -1.0f };
1937  float fy[2] = { -1.0f, -1.0f };
1938 
1939  float left_rect_sides[4] = {0.f};
1940  get_target_rect_info(left, left_rect_sides, fx[0], fy[0], 50, progress_callback); // Report 50% progress
1941 
1942  float right_rect_sides[4] = {0.f};
1943  get_target_rect_info(right, right_rect_sides, fx[1], fy[1], 75, progress_callback);
1944 
1945  std::vector<uint8_t> ret;
1946  const float correction_factor = 0.5f;
1947 
1948  auto calib_table = get_calibration_table();
1949  auto table = (librealsense::ds::d400_coefficients_table*)calib_table.data();
1950 
1951  float ar[2] = { 0 };
1952  float tmp = left_rect_sides[2] + left_rect_sides[3];
1953  if (tmp > 0.1f)
1954  ar[0] = (left_rect_sides[0] + left_rect_sides[1]) / tmp;
1955 
1956  tmp = right_rect_sides[2] + right_rect_sides[3];
1957  if (tmp > 0.1f)
1958  ar[1] = (right_rect_sides[0] + right_rect_sides[1]) / tmp;
1959 
1960  float align = 0.0f;
1961  if (ar[0] > 0.0f)
1962  align = ar[1] / ar[0] - 1.0f;
1963 
1964  float ta[2] = { 0 };
1965  float gt[4] = { 0 };
1966  float ave_gt = 0.0f;
1967 
1968  if (left_rect_sides[0] > 0)
1969  gt[0] = fx[0] * target_w / left_rect_sides[0];
1970 
1971  if (left_rect_sides[1] > 0)
1972  gt[1] = fx[0] * target_w / left_rect_sides[1];
1973 
1974  if (left_rect_sides[2] > 0)
1975  gt[2] = fy[0] * target_h / left_rect_sides[2];
1976 
1977  if (left_rect_sides[3] > 0)
1978  gt[3] = fy[0] * target_h / left_rect_sides[3];
1979 
1980  ave_gt = 0.0f;
1981  for (int i = 0; i < 4; ++i)
1982  ave_gt += gt[i];
1983  ave_gt /= 4.0;
1984 
1985  ta[0] = atanf(align * ave_gt / std::abs(table->baseline));
1986  ta[0] = rad2deg(ta[0]);
1987 
1988  if (right_rect_sides[0] > 0)
1989  gt[0] = fx[1] * target_w / right_rect_sides[0];
1990 
1991  if (right_rect_sides[1] > 0)
1992  gt[1] = fx[1] * target_w / right_rect_sides[1];
1993 
1994  if (right_rect_sides[2] > 0)
1995  gt[2] = fy[1] * target_h / right_rect_sides[2];
1996 
1997  if (right_rect_sides[3] > 0)
1998  gt[3] = fy[1] * target_h / right_rect_sides[3];
1999 
2000  ave_gt = 0.0f;
2001  for (int i = 0; i < 4; ++i)
2002  ave_gt += gt[i];
2003  ave_gt /= 4.0;
2004 
2005  ta[1] = atanf(align * ave_gt / std::abs(table->baseline));
2006  ta[1] = rad2deg(ta[1]);
2007 
2008  *angle = (ta[0] + ta[1]) / 2;
2009 
2010  align *= 100;
2011 
2012  float r[4] = { 0 };
2013  float c = fx[0] / fx[1];
2014 
2015  if (left_rect_sides[0] > 0.1f)
2016  r[0] = c * right_rect_sides[0] / left_rect_sides[0];
2017 
2018  if (left_rect_sides[1] > 0.1f)
2019  r[1] = c * right_rect_sides[1] / left_rect_sides[1];
2020 
2021  c = fy[0] / fy[1];
2022  if (left_rect_sides[2] > 0.1f)
2023  r[2] = c * right_rect_sides[2] / left_rect_sides[2];
2024 
2025  if (left_rect_sides[3] > 0.1f)
2026  r[3] = c * right_rect_sides[3] / left_rect_sides[3];
2027 
2028  float ra = 0.0f;
2029  for (int i = 0; i < 4; ++i)
2030  ra += r[i];
2031  ra /= 4;
2032 
2033  ra -= 1.0f;
2034  ra *= 100;
2035 
2036  *ratio = ra - correction_factor * align;
2037  float ratio_to_apply = *ratio / 100.0f + 1.0f;
2038 
2039  if (adjust_both_sides)
2040  {
2041  float ratio_to_apply_2 = sqrtf(ratio_to_apply);
2042  table->intrinsic_left.x.x /= ratio_to_apply_2;
2043  table->intrinsic_left.x.y /= ratio_to_apply_2;
2044  table->intrinsic_right.x.x *= ratio_to_apply_2;
2045  table->intrinsic_right.x.y *= ratio_to_apply_2;
2046  }
2047  else
2048  {
2049  table->intrinsic_right.x.x *= ratio_to_apply;
2050  table->intrinsic_right.x.y *= ratio_to_apply;
2051  }
2052 
2053  auto actual_data = calib_table.data() + sizeof(librealsense::ds::table_header);
2054  auto actual_data_size = calib_table.size() - sizeof(librealsense::ds::table_header);
2055  auto crc = rsutils::number::calc_crc32(actual_data, actual_data_size);
2056  table->header.crc32 = crc;
2057 
2058  return calib_table;
2059  }
2060 
2061  void auto_calibrated::undistort(uint8_t* img, const rs2_intrinsics& intrin, int roi_ws, int roi_hs, int roi_we, int roi_he)
2062  {
2063  assert(intrin.model == RS2_DISTORTION_INVERSE_BROWN_CONRADY);
2064 
2065  int width = intrin.width;
2066  int height = intrin.height;
2067 
2068  if (roi_ws < 0) roi_ws = 0;
2069  if (roi_hs < 0) roi_hs = 0;
2070  if (roi_we > width) roi_we = width;
2071  if (roi_he > height) roi_he = height;
2072 
2073  int size3 = width * height * 3;
2074  std::vector<uint8_t> tmp(size3);
2075  memset(tmp.data(), 0, size3);
2076 
2077  float x = 0;
2078  float y = 0;
2079  int m = 0;
2080  int n = 0;
2081 
2082  int width3 = width * 3;
2083  int idx_from = 0;
2084  int idx_to = 0;
2085  for (int j = roi_hs; j < roi_he; ++j)
2086  {
2087  for (int i = roi_ws; i < roi_we; ++i)
2088  {
2089  x = static_cast<float>(i);
2090  y = static_cast<float>(j);
2091 
2092  if( std::abs( intrin.fx ) > 0.00001f && std::abs( intrin.fy ) > 0.0001f )
2093  {
2094  x = (x - intrin.ppx) / intrin.fx;
2095  y = (y - intrin.ppy) / intrin.fy;
2096 
2097  float r2 = x * x + y * y;
2098  float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
2099  float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
2100  float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
2101  x = ux;
2102  y = uy;
2103 
2104  x = x * intrin.fx + intrin.ppx;
2105  y = y * intrin.fy + intrin.ppy;
2106  }
2107 
2108  m = static_cast<int>(x + 0.5f);
2109  if (m >= 0 && m < width)
2110  {
2111  n = static_cast<int>(y + 0.5f);
2112  if (n >= 0 && n < height)
2113  {
2114  idx_from = j * width3 + i * 3;
2115  idx_to = n * width3 + m * 3;
2116  tmp[idx_to++] = img[idx_from++];
2117  tmp[idx_to++] = img[idx_from++];
2118  tmp[idx_to++] = img[idx_from++];
2119  }
2120  }
2121  }
2122  }
2123 
2124  memmove(img, tmp.data(), size3);
2125  }
2126 
2127  void auto_calibrated::get_target_dots_info(rs2_frame_queue* frames, float dots_x[4], float dots_y[4], rs2::stream_profile& profile, rs2_intrinsics& intrin, int progress, rs2_update_progress_callback_sptr progress_callback)
2128  {
2129  bool got_intrinsics = false;
2130  std::vector<std::array<float, 4>> dots_x_arr;
2131  std::vector<std::array<float, 4>> dots_y_arr;
2132 
2133  rs2_error* e = nullptr;
2134  rs2_frame* f = nullptr;
2135 
2136  int queue_size = rs2_frame_queue_size(frames, &e);
2137  int fc = 0;
2138 
2139  while ((fc++ < queue_size) && rs2_poll_for_frame(frames, &f, &e))
2140  {
2141  rs2::frame ff(f);
2142  if (ff.get_data())
2143  {
2144  if (!got_intrinsics)
2145  {
2146  profile = ff.get_profile();
2147  auto vsp = profile.as<rs2::video_stream_profile>();
2148  intrin = vsp.get_intrinsics();
2149  got_intrinsics = true;
2150  }
2151 
2152  if (ff.get_profile().format() == RS2_FORMAT_RGB8)
2153  {
2154  undistort(const_cast<uint8_t *>(static_cast<const uint8_t *>(ff.get_data())), intrin,
2155  librealsense::_roi_ws - librealsense::_patch_size, librealsense::_roi_hs - librealsense::_patch_size,
2156  librealsense::_roi_we + librealsense::_patch_size, librealsense::_roi_he + librealsense::_patch_size);
2157  }
2158 
2159  float dots[8] = { 0 };
2160  rs2_extract_target_dimensions(f, RS2_CALIB_TARGET_POS_GAUSSIAN_DOT_VERTICES, dots, 8, &e);
2161  if (e)
2162  throw std::runtime_error("Failed to extract target information\nfrom the captured frames!");
2163 
2164  std::array<float, 4> dots_x_cur;
2165  std::array<float, 4> dots_y_cur;
2166  int j = 0;
2167  for (int i = 0; i < 4; ++i)
2168  {
2169  j = i << 1;
2170  dots_x_cur[i] = dots[j];
2171  dots_y_cur[i] = dots[j + 1];
2172  }
2173  dots_x_arr.emplace_back(dots_x_cur);
2174  dots_y_arr.emplace_back(dots_y_cur);
2175  }
2176 
2177  rs2_release_frame(f);
2178 
2179  if (progress_callback)
2180  progress_callback->on_update_progress(static_cast<float>(++progress));
2181  }
2182 
2183  for (int i = 0; i < 4; ++i)
2184  {
2185  dots_x[i] = dots_x_arr[0][i];
2186  dots_y[i] = dots_y_arr[0][i];
2187  }
2188 
2189  for (int j = 1; j < dots_x_arr.size(); ++j)
2190  {
2191  for (int i = 0; i < 4; ++i)
2192  {
2193  dots_x[i] += dots_x_arr[j][i];
2194  dots_y[i] += dots_y_arr[j][i];
2195  }
2196  }
2197 
2198  for (int i = 0; i < 4; ++i)
2199  {
2200  dots_x[i] /= dots_x_arr.size();
2201  dots_y[i] /= dots_y_arr.size();
2202  }
2203  }
2204 
2205  void auto_calibrated::find_z_at_corners(float left_x[4], float left_y[4], rs2_frame_queue* frames, float left_z[4])
2206  {
2207  int x1[4] = { 0 };
2208  int y1[4] = { 0 };
2209  int x2[4] = { 0 };
2210  int y2[4] = { 0 };
2211  int pos_tl[4] = { 0 };
2212  int pos_tr[4] = { 0 };
2213  int pos_bl[4] = { 0 };
2214  int pos_br[4] = { 0 };
2215 
2216  float left_z_tl[4] = { 0 };
2217  float left_z_tr[4] = { 0 };
2218  float left_z_bl[4] = { 0 };
2219  float left_z_br[4] = { 0 };
2220 
2221  bool got_width = false;
2222  int width = 0;
2223  int counter = 0;
2224  rs2_error* e = nullptr;
2225  rs2_frame* f = nullptr;
2226 
2227  int queue_size = rs2_frame_queue_size(frames, &e);
2228  int fc = 0;
2229 
2230  while ((fc++ < queue_size) && rs2_poll_for_frame(frames, &f, &e))
2231  {
2232  rs2::frame ff(f);
2233  if (ff.get_data())
2234  {
2235  if (!got_width)
2236  {
2237  auto p = ff.get_profile();
2238  auto vsp = p.as<rs2::video_stream_profile>();
2239  width = vsp.get_intrinsics().width;
2240  got_width = true;
2241 
2242  for (int i = 0; i < 4; ++i)
2243  {
2244  x1[i] = static_cast<int>(left_x[i]);
2245  y1[i] = static_cast<int>(left_y[i]);
2246 
2247  x2[i] = static_cast<int>(left_x[i] + 1.0f);
2248  y2[i] = static_cast<int>(left_y[i] + 1.0f);
2249 
2250  pos_tl[i] = y1[i] * width + x1[i];
2251  pos_tr[i] = y1[i] * width + x2[i];
2252  pos_bl[i] = y2[i] * width + x1[i];
2253  pos_br[i] = y2[i] * width + x2[i];
2254  }
2255  }
2256 
2257  const uint16_t* depth = reinterpret_cast<const uint16_t*>(ff.get_data());
2258  for (int i = 0; i < 4; ++i)
2259  {
2260  left_z_tl[i] += static_cast<float>(depth[pos_tl[i]]);
2261  left_z_tr[i] += static_cast<float>(depth[pos_tr[i]]);
2262  left_z_bl[i] += static_cast<float>(depth[pos_bl[i]]);
2263  left_z_br[i] += static_cast<float>(depth[pos_br[i]]);
2264  }
2265 
2266  ++counter;
2267  }
2268 
2269  rs2_release_frame(f);
2270  }
2271 
2272  for (int i = 0; i < 4; ++i)
2273  {
2274  if (counter > 0)
2275  {
2276  left_z_tl[i] /= counter;
2277  left_z_tr[i] /= counter;
2278  left_z_bl[i] /= counter;
2279  left_z_br[i] /= counter;
2280  }
2281  }
2282 
2283  float z_1 = 0.0f;
2284  float z_2 = 0.0f;
2285  float s = 0.0f;
2286  for (int i = 0; i < 4; ++i)
2287  {
2288  s = left_x[i] - x1[i];
2289  z_1 = (1.0f - s) * left_z_bl[i] + s * left_z_br[i];
2290  z_2 = (1.0f - s) * left_z_tl[i] + s * left_z_tr[i];
2291 
2292  s = left_y[i] - y1[i];
2293  left_z[i] = (1.0f - s) * z_2 + s * z_1;
2294  left_z[i] *= 0.001f;
2295  }
2296  }
2297 
2298  std::vector<uint8_t> auto_calibrated::run_uv_map_calibration(rs2_frame_queue* left, rs2_frame_queue* color, rs2_frame_queue* depth, int py_px_only,
2299  float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback)
2300  {
2301  float left_dots_x[4];
2302  float left_dots_y[4];
2303  rs2_intrinsics left_intrin;
2304  rs2::stream_profile left_profile;
2305  get_target_dots_info(left, left_dots_x, left_dots_y, left_profile, left_intrin, 50, progress_callback);
2306 
2307  float color_dots_x[4];
2308  float color_dots_y[4];
2309  rs2_intrinsics color_intrin;
2310  rs2::stream_profile color_profile;
2311  get_target_dots_info(color, color_dots_x, color_dots_y, color_profile, color_intrin, 75, progress_callback);
2312 
2313  float z[4] = { 0 };
2314  find_z_at_corners(left_dots_x, left_dots_y, depth, z);
2315 
2316  rs2_extrinsics extrin = left_profile.get_extrinsics_to(color_profile);
2317 
2318  float pixel_left[4][2] = { 0 };
2319  float point_left[4][3] = { 0 };
2320 
2321  float pixel_color[4][2] = { 0 };
2322  float pixel_color_norm[4][2] = { 0 };
2323  float point_color[4][3] = { 0 };
2324 
2325  for (int i = 0; i < 4; ++i)
2326  {
2327  pixel_left[i][0] = left_dots_x[i];
2328  pixel_left[i][1] = left_dots_y[i];
2329 
2330  rs2_deproject_pixel_to_point(point_left[i], &left_intrin, pixel_left[i], z[i]);
2331  rs2_transform_point_to_point(point_color[i], &extrin, point_left[i]);
2332 
2333  pixel_color_norm[i][0] = point_color[i][0] / point_color[i][2];
2334  pixel_color_norm[i][1] = point_color[i][1] / point_color[i][2];
2335  pixel_color[i][0] = pixel_color_norm[i][0] * color_intrin.fx + color_intrin.ppx;
2336  pixel_color[i][1] = pixel_color_norm[i][1] * color_intrin.fy + color_intrin.ppy;
2337  }
2338 
2339  float diff[4] = { 0 };
2340  float tmp = 0.0f;
2341  for (int i = 0; i < 4; ++i)
2342  {
2343  tmp = (pixel_color[i][0] - color_dots_x[i]);
2344  tmp *= tmp;
2345  diff[i] = tmp;
2346 
2347  tmp = (pixel_color[i][1] - color_dots_y[i]);
2348  tmp *= tmp;
2349  diff[i] += tmp;
2350 
2351  diff[i] = sqrtf(diff[i]);
2352  }
2353 
2354  float err_before = 0.0f;
2355  for (int i = 0; i < 4; ++i)
2356  err_before += diff[i];
2357  err_before /= 4;
2358 
2359  float ppx = 0.0f;
2360  float ppy = 0.0f;
2361  float fx = 0.0f;
2362  float fy = 0.0f;
2363 
2364  if (py_px_only)
2365  {
2366  fx = color_intrin.fx;
2367  fy = color_intrin.fy;
2368 
2369  ppx = 0.0f;
2370  ppy = 0.0f;
2371  for (int i = 0; i < 4; ++i)
2372  {
2373  ppx += color_dots_x[i] - pixel_color_norm[i][0] * fx;
2374  ppy += color_dots_y[i] - pixel_color_norm[i][1] * fy;
2375  }
2376  ppx /= 4.0f;
2377  ppy /= 4.0f;
2378  }
2379  else
2380  {
2381  double x = 0;
2382  double y = 0;
2383  double c_x = 0;
2384  double c_y = 0;
2385  double x_2 = 0;
2386  double y_2 = 0;
2387  double c_xc = 0;
2388  double c_yc = 0;
2389  for (int i = 0; i < 4; ++i)
2390  {
2391  x += pixel_color_norm[i][0];
2392  y += pixel_color_norm[i][1];
2393  c_x += color_dots_x[i];
2394  c_y += color_dots_y[i];
2395  x_2 += pixel_color_norm[i][0] * pixel_color_norm[i][0];
2396  y_2 += pixel_color_norm[i][1] * pixel_color_norm[i][1];
2397  c_xc += color_dots_x[i] * pixel_color_norm[i][0];
2398  c_yc += color_dots_y[i] * pixel_color_norm[i][1];
2399  }
2400 
2401  double d_x = 4 * x_2 - x * x;
2402  if (d_x > 0.01)
2403  {
2404  d_x = 1 / d_x;
2405  fx = static_cast<float>(d_x * (4 * c_xc - x * c_x));
2406  ppx = static_cast<float>(d_x * (x_2 * c_x - x * c_xc));
2407  }
2408 
2409  double d_y = 4 * y_2 - y * y;
2410  if (d_y > 0.01)
2411  {
2412  d_y = 1 / d_y;
2413  fy = static_cast<float>(d_y * (4 * c_yc - y * c_y));
2414  ppy = static_cast<float>(d_y * (y_2 * c_y - y * c_yc));
2415  }
2416  }
2417 
2418  float err_after = 0.0f;
2419  float tmpx = 0;
2420  float tmpy = 0;
2421  for (int i = 0; i < 4; ++i)
2422  {
2423  tmpx = pixel_color_norm[i][0] * fx + ppx - color_dots_x[i];
2424  tmpx *= tmpx;
2425 
2426  tmpy = pixel_color_norm[i][1] * fy + ppy - color_dots_y[i];
2427  tmpy *= tmpy;
2428 
2429  err_after += sqrtf(tmpx + tmpy);
2430  }
2431 
2432  err_after /= 4.0f;
2433 
2434  std::vector<uint8_t> ret;
2435  const float max_change = 16.0f;
2436  if (fabs(color_intrin.ppx - ppx) < max_change && fabs(color_intrin.ppy - ppy) < max_change && fabs(color_intrin.fx - fx) < max_change && fabs(color_intrin.fy - fy) < max_change)
2437  {
2438  ret = _hw_monitor->send(command{ds::GETINTCAL, static_cast<int>(ds::d400_calibration_table_id::rgb_calibration_id) });
2439  auto table = reinterpret_cast<librealsense::ds::d400_rgb_calibration_table*>(ret.data());
2440 
2441  health[0] = table->intrinsic(2, 0); // px
2442  health[1] = table->intrinsic(2, 1); // py
2443  health[2] = table->intrinsic(0, 0); // fx
2444  health[3] = table->intrinsic(1, 1); // fy
2445 
2446  table->intrinsic(2, 0) = 2.0f * ppx / color_intrin.width - 1; // ppx
2447  table->intrinsic(2, 1) = 2.0f * ppy / color_intrin.height - 1; // ppy
2448  table->intrinsic(0, 0) = 2.0f * fx / color_intrin.width; // fx
2449  table->intrinsic(1, 1) = 2.0f * fy / color_intrin.height; // fy
2450 
2451  float calib_aspect_ratio = 9.f / 16.f; // shall be overwritten with the actual calib resolution
2452  if (table->calib_width && table->calib_height)
2453  calib_aspect_ratio = float(table->calib_height) / float(table->calib_width);
2454 
2455  float actual_aspect_ratio = color_intrin.height / (float)color_intrin.width;
2456  if (actual_aspect_ratio < calib_aspect_ratio)
2457  {
2458  table->intrinsic(1, 1) /= calib_aspect_ratio / actual_aspect_ratio; // fy
2459  table->intrinsic(2, 1) /= calib_aspect_ratio / actual_aspect_ratio; // ppy
2460  }
2461  else
2462  {
2463  table->intrinsic(0, 0) /= actual_aspect_ratio / calib_aspect_ratio; // fx
2464  table->intrinsic(2, 0) /= actual_aspect_ratio / calib_aspect_ratio; // ppx
2465  }
2466 
2467  table->header.crc32 = rsutils::number::calc_crc32( ret.data() + sizeof( librealsense::ds::table_header ),
2468  ret.size() - sizeof( librealsense::ds::table_header ) );
2469 
2470  health[0] = (std::abs(table->intrinsic(2, 0) / health[0]) - 1) * 100; // px
2471  health[1] = (std::abs(table->intrinsic(2, 1) / health[1]) - 1) * 100; // py
2472  health[2] = (std::abs(table->intrinsic(0, 0) / health[2]) - 1) * 100; // fx
2473  health[3] = (std::abs(table->intrinsic(1, 1) / health[3]) - 1) * 100; // fy
2474  }
2475 
2476  return ret;
2477  }
2478 
2479  float auto_calibrated::calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3,
2480  float target_w, float target_h, rs2_update_progress_callback_sptr progress_callback)
2481  {
2482  constexpr size_t min_frames_required = 10;
2483  bool created = false;
2484 
2485  float4 rect_sides{};
2486  float target_fw = 0;
2487  float target_fh = 0;
2488 
2489  float target_z_value = -1.f;
2490  std::vector<float4> rec_sides_data;
2491  rect_calculator target_z_calculator(true);
2492 
2493  int frm_idx = 0;
2494  int progress = 0;
2495  rs2_error* e = nullptr;
2496  rs2_frame* f = nullptr;
2497 
2498  int queue_size = rs2_frame_queue_size(queue1, &e);
2499  int fc = 0;
2500 
2501  while ((fc++ < queue_size) && rs2_poll_for_frame(queue1, &f, &e))
2502  {
2503  rs2::frame ff(f);
2504  if (ff.get_data())
2505  {
2506  if (!created)
2507  {
2508  auto vsp = ff.get_profile().as<rs2::video_stream_profile>();
2509 
2510  target_fw = vsp.get_intrinsics().fx * target_w;
2511  target_fh = vsp.get_intrinsics().fy * target_h;
2512  created = true;
2513  }
2514 
2515  // retirieve target size and accumulate results, skip frame if target could not be found
2516  if (target_z_calculator.extract_target_dims(f, rect_sides))
2517  {
2518  rec_sides_data.push_back(rect_sides);
2519  }
2520 
2521  frm_idx++;
2522  }
2523 
2524  rs2_release_frame(f);
2525 
2526  if (progress_callback)
2527  progress_callback->on_update_progress(static_cast<float>(progress++));
2528 
2529  }
2530 
2531  if (rec_sides_data.size())
2532  {
2533  // Verify that at least TBD valid extractions were made
2534  if ((frm_idx < min_frames_required))
2535  throw std::runtime_error( rsutils::string::from() << "Target distance calculation requires at least "
2536  << min_frames_required << " frames, aborting" );
2537  if (float(rec_sides_data.size() / frm_idx) < 0.5f)
2538  throw std::runtime_error("Please re-adjust the camera position \nand make sure the specific target is \nin the middle of the camera image!");
2539 
2540  rect_sides = {};
2541  auto avg_data = std::accumulate(rec_sides_data.begin(), rec_sides_data.end(), rect_sides);
2542  for (auto i = 0UL; i < 4; i++)
2543  avg_data[i] /= rec_sides_data.size();
2544 
2545  float gt[4] = { 0 };
2546 
2547  gt[0] = target_fw / avg_data[0];
2548 
2549  gt[1] = target_fw / avg_data[1];
2550 
2551  gt[2] = target_fh / avg_data[2];
2552 
2553  gt[3] = target_fh / avg_data[3];
2554 
2555  if (gt[0] <= 0.1f || gt[1] <= 0.1f || gt[2] <= 0.1f || gt[3] <= 0.1f)
2556  throw std::runtime_error("Target distance calculation failed");
2557 
2558  // Target's plane Z value is the average of the four calculated corners
2559  target_z_value = 0.f;
2560  for (int i = 0; i < 4; ++i)
2561  target_z_value += gt[i];
2562  target_z_value /= 4.f;
2563 
2564  return target_z_value;
2565  }
2566  else
2567  throw std::runtime_error("Failed to extract target dimension info!");
2568  }
2569 
2570  void auto_calibrated::set_hw_monitor_for_auto_calib(std::shared_ptr<hw_monitor> hwm)
2571  {
2572  _hw_monitor = hwm;
2573  }
2574 }
librealsense::auto_calibrated::get_target_rect_info
void get_target_rect_info(rs2_frame_queue *frames, float rect_sides[4], float &fx, float &fy, int progress, rs2_update_progress_callback_sptr progress_callback)
Definition: d400-auto-calibration.cpp:1876
librealsense::params4::data_sampling
int data_sampling
Definition: d400-auto-calibration.cpp:154
realdds::topics::control::hwm::key::param3
const std::string param3
realdds::topics::metadata::key::header
const std::string header
librealsense::low
@ low
Definition: d400-auto-calibration.cpp:127
example1 - object detection.left
left
Definition: example1 - object detection.py:70
librealsense::auto_calibrated::undistort
void undistort(uint8_t *img, const rs2_intrinsics &intrin, int roi_ws, int roi_hs, int roi_we, int roi_he)
Definition: d400-auto-calibration.cpp:2061
rs2_update_progress_callback_sptr
std::shared_ptr< rs2_update_progress_callback > rs2_update_progress_callback_sptr
Definition: rs_types.hpp:97
color
GLuint color
Definition: glad/glad/glad.h:3181
librealsense::auto_calibrated::collect_depth_frame_sum
void collect_depth_frame_sum(const rs2_frame *f)
Definition: d400-auto-calibration.cpp:1207
librealsense
Definition: algo.h:18
librealsense::DEFAULT_AVERAGE_STEP_COUNT
const int DEFAULT_AVERAGE_STEP_COUNT
Definition: d400-auto-calibration.cpp:171
librealsense::ds::d400_calibration_table_id::rgb_calibration_id
@ rgb_calibration_id
d400-private.h
librealsense::TareCalibrationResult::iterations
uint16_t iterations
Definition: d400-auto-calibration.cpp:38
RS2_FRAME_METADATA_FRAME_TIMESTAMP
@ RS2_FRAME_METADATA_FRAME_TIMESTAMP
Definition: rs_frame.h:32
intrin
rs2_intrinsics intrin
Definition: test-distortion.cpp:14
librealsense::RS2_DSC_STATUS_NO_DEPTH_AVERAGE
@ RS2_DSC_STATUS_NO_DEPTH_AVERAGE
Definition: d400-auto-calibration.cpp:85
librealsense::RS2_DSC_STATUS_EDGE_TOO_CLOSE
@ RS2_DSC_STATUS_EDGE_TOO_CLOSE
Definition: d400-auto-calibration.cpp:81
test-fw-update.cmd
list cmd
Definition: test-fw-update.py:141
RS2_CALIB_TARGET_ROI_RECT_GAUSSIAN_DOT_VERTICES
@ RS2_CALIB_TARGET_ROI_RECT_GAUSSIAN_DOT_VERTICES
Definition: rs_frame.h:87
librealsense::auto_calibrated::check_one_button_params
void check_one_button_params(int speed, int keep_new_value_after_sucessful_scan, int data_sampling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const
Definition: d400-auto-calibration.cpp:1680
count
GLint GLsizei count
Definition: glad/glad/glad.h:2301
uint8_t
unsigned char uint8_t
Definition: stdint.h:78
rs2::stream_profile::format
rs2_format format() const
Definition: rs_frame.hpp:44
librealsense::polling
@ polling
Definition: d400-auto-calibration.cpp:132
librealsense::auto_calibrated::_hw_monitor
std::shared_ptr< hw_monitor > _hw_monitor
Definition: d400-auto-calibration.h:88
librealsense::device_serializer::status
status
Defines return codes that SDK interfaces use. Negative values indicate errors, a zero value indicates...
Definition: src/core/serialization.h:311
librealsense::host_assistance_type::assistance_start
@ assistance_start
rmse.x1
int x1
Definition: rmse.py:50
min
int min(int a, int b)
Definition: lz4s.c:73
librealsense::FocalLengthCalibrationResult::status
uint16_t status
Definition: d400-auto-calibration.cpp:43
test-librs-connections.n
n
Definition: test-librs-connections.py:38
rs2::frame
Definition: rs_frame.hpp:345
librealsense::invalid_value_exception
Definition: librealsense-exception.h:114
librealsense::TareCalibrationResult::accuracyLevel
uint16_t accuracyLevel
Definition: d400-auto-calibration.cpp:37
librealsense::tare_params3::accuracy
uint8_t accuracy
Definition: d400-auto-calibration.cpp:146
librealsense::ds::d400_rgb_calibration_table::intrinsic
float3x3 intrinsic
Definition: d400-private.h:248
rs2::video_stream_profile::get_intrinsics
rs2_intrinsics get_intrinsics() const
Definition: rs_frame.hpp:240
LOG_OCC_WARN
#define LOG_OCC_WARN(...)
Definition: d400-auto-calibration.cpp:21
librealsense::TareCalibrationResult::calPx
int32_t calPx
Definition: d400-auto-calibration.cpp:34
rsutil.h
librealsense::auto_calibrated::_preset_change
bool _preset_change
Definition: d400-auto-calibration.h:90
rs2_transform_point_to_point
void rs2_transform_point_to_point(float to_point[3], const rs2_extrinsics *extrin, const float from_point[3])
librealsense::DEFAULT_FL_STEP_COUNT
const int DEFAULT_FL_STEP_COUNT
Definition: d400-auto-calibration.cpp:178
rs2_extrinsics
Cross-stream extrinsics: encodes the topology describing how the different devices are oriented.
Definition: rs_sensor.h:102
rs2::stream_profile
Definition: rs_frame.hpp:22
librealsense::ds::success
@ success
Definition: d400-private.h:266
test-hdr-long.frame_ts
frame_ts
Definition: test-hdr-long.py:350
librealsense::param4
Definition: d400-auto-calibration.cpp:163
librealsense::speed_fast
@ speed_fast
Definition: d400-auto-calibration.cpp:108
librealsense::DEFAULT_SPEED
const auto_calib_speed DEFAULT_SPEED
Definition: d400-auto-calibration.cpp:174
d400-device.h
test-projection-from-recording.frames
frames
Definition: test-projection-from-recording.py:30
rs2::stream_profile::as
T as() const
Definition: rs_frame.hpp:103
uint16_t
unsigned short uint16_t
Definition: stdint.h:79
RS2_RS400_VISUAL_PRESET_CUSTOM
@ RS2_RS400_VISUAL_PRESET_CUSTOM
Definition: rs_option.h:198
librealsense::param4::param_4
uint32_t param_4
Definition: d400-auto-calibration.cpp:166
librealsense::FocalLengthCalibrationResult::leftFy
float leftFy
Definition: d400-auto-calibration.cpp:48
librealsense::host_assistance_type::assistance_second_feed
@ assistance_second_feed
librealsense::auto_calibrated::_skipped_frames
int _skipped_frames
Definition: d400-auto-calibration.h:108
librealsense::sensor_interface
Definition: sensor-interface.h:28
librealsense::DEFAULT_FY_SCAN_RANGE
const int DEFAULT_FY_SCAN_RANGE
Definition: d400-auto-calibration.cpp:179
librealsense::DEFAULT_KEEP_NEW_VALUE_AFTER_SUCESSFUL_SCAN
const int DEFAULT_KEEP_NEW_VALUE_AFTER_SUCESSFUL_SCAN
Definition: d400-auto-calibration.cpp:180
librealsense::DEFAULT_STEP_COUNT
const int DEFAULT_STEP_COUNT
Definition: d400-auto-calibration.cpp:172
librealsense::medium
@ medium
Definition: d400-auto-calibration.cpp:126
rsutils::number::float4
Definition: third-party/rsutils/include/rsutils/number/float3.h:47
librealsense::auto_calibrated::run_on_chip_calibration
std::vector< uint8_t > run_on_chip_calibration(int timeout_ms, std::string json, float *const health, rs2_update_progress_callback_sptr progress_callback) override
Definition: d400-auto-calibration.cpp:335
librealsense::DEFAULT_SCAN
const int DEFAULT_SCAN
Definition: d400-auto-calibration.cpp:175
video-frame.h
librealsense::rect_calculator
Definition: algo.h:252
librealsense::auto_calibrated::find_z_at_corners
void find_z_at_corners(float left_x[4], float left_y[4], rs2_frame_queue *frames, float left_z[4])
Definition: d400-auto-calibration.cpp:2205
librealsense::interactive_scan_control
@ interactive_scan_control
Definition: d400-auto-calibration.cpp:93
rs2_intrinsics
Video stream intrinsics.
Definition: rs_types.h:58
librealsense::ds_advanced_mode_base::_preset_opt
std::shared_ptr< advanced_mode_preset_option > _preset_opt
Definition: advanced_mode.h:208
data
Definition: parser.hpp:153
librealsense::high
@ high
Definition: d400-auto-calibration.cpp:125
librealsense::thermal_compensation_guard::thermal_compensation_guard
thermal_compensation_guard(auto_calibrated_interface *handle)
Definition: d400-auto-calibration.cpp:231
rs2_frame
struct rs2_frame rs2_frame
Definition: rs_types.h:230
librealsense::host_assistance_type
host_assistance_type
Definition: d400-auto-calibration.cpp:114
librealsense::DEFAULT_WHITE_WALL_MODE
const int DEFAULT_WHITE_WALL_MODE
Definition: d400-auto-calibration.cpp:188
rs2_error
Definition: rs.cpp:227
RS2_FORMAT_RGB8
@ RS2_FORMAT_RGB8
Definition: rs_sensor.h:68
librealsense::ds::table_header
Definition: ds-private.h:259
string
GLsizei const GLchar *const * string
Definition: glad/glad/glad.h:2861
rs2_intrinsics::ppx
float ppx
Definition: rs_types.h:62
librealsense::host_assistance_type::assistance_first_feed
@ assistance_first_feed
librealsense::py_scan
@ py_scan
Definition: d400-auto-calibration.cpp:138
rs2_release_frame
void rs2_release_frame(rs2_frame *frame)
Definition: rs.cpp:1382
librealsense::TareCalibrationResult::aveDepth
uint32_t aveDepth
Definition: d400-auto-calibration.cpp:32
counter
static unsigned int counter
Definition: events.c:46
rs2_intrinsics::height
int height
Definition: rs_types.h:61
librealsense::auto_calibrated::run_tare_calibration
std::vector< uint8_t > run_tare_calibration(int timeout_ms, float ground_truth_mm, std::string json, float *const health, rs2_update_progress_callback_sptr progress_callback) override
Definition: d400-auto-calibration.cpp:848
realdds::topics::control::hwm::key::param2
const std::string param2
librealsense::auto_calibrated::_ground_truth_mm
float _ground_truth_mm
Definition: d400-auto-calibration.h:95
librealsense::speed_white_wall
@ speed_white_wall
Definition: d400-auto-calibration.cpp:111
right
GLdouble right
Definition: glad/glad/glad.h:2249
librealsense::params4
Definition: d400-auto-calibration.cpp:150
librealsense::auto_calibrated::reset_to_factory_calibration
void reset_to_factory_calibration() const override
Definition: d400-auto-calibration.cpp:1870
librealsense::DEFAULT_FL_SAMPLING
const int DEFAULT_FL_SAMPLING
Definition: d400-auto-calibration.cpp:181
librealsense::params4::reserved
int reserved
Definition: d400-auto-calibration.cpp:153
librealsense::speed_very_fast
@ speed_very_fast
Definition: d400-auto-calibration.cpp:107
librealsense::params4::scan_parameter
int scan_parameter
Definition: d400-auto-calibration.cpp:152
librealsense::tare_calib_begin
@ tare_calib_begin
Definition: d400-auto-calibration.cpp:95
LOG_DEBUG
#define LOG_DEBUG(...)
Definition: easyloggingpp.h:70
realdds::topics::notification::dfu_apply::key::progress
const std::string progress
librealsense::FocalLengthCalibrationResult::FL_healthCheck
float FL_healthCheck
Definition: d400-auto-calibration.cpp:50
librealsense::TareCalibrationResult::tareDepth
uint32_t tareDepth
Definition: d400-auto-calibration.cpp:31
from.h
librealsense::DscResultBuffer
Definition: d400-auto-calibration.cpp:69
opencv_pointcloud_viewer.out
out
Definition: opencv_pointcloud_viewer.py:276
librealsense::legacy_file_format::frame_counter
constexpr uint32_t frame_counter
Definition: ros_file_format.h:640
param
GLenum GLfloat param
Definition: glad/glad/glad.h:1400
table
GLenum GLenum GLsizei void * table
Definition: glad/glad/glad.h:3584
rs2_poll_for_frame
int rs2_poll_for_frame(rs2_frame_queue *queue, rs2_frame **output_frame, rs2_error **error)
Definition: rs.cpp:1500
librealsense::interrupt
@ interrupt
Definition: d400-auto-calibration.cpp:133
librealsense::ds::SETINTCAL
@ SETINTCAL
Definition: ds-private.h:95
librealsense::ds::d400_rgb_calibration_table
Definition: d400-private.h:244
librealsense::auto_calibrated::_curr_calibration
std::vector< uint8_t > _curr_calibration
Definition: d400-auto-calibration.h:87
x2
GLdouble GLdouble x2
Definition: glad/glad/glad.h:1781
rs2::frame::get_data
const void * get_data() const
Definition: rs_frame.hpp:547
test-streaming.r2
r2
Definition: test-streaming.py:92
librealsense::frame_interface
Definition: frame-interface.h:20
rs2_frame_queue
Definition: rs.cpp:195
rs2::stream_profile::get_extrinsics_to
rs2_extrinsics get_extrinsics_to(const stream_profile &to) const
Definition: rs_frame.hpp:148
librealsense::DEFAULT_TARE_SAMPLING
const int DEFAULT_TARE_SAMPLING
Definition: d400-auto-calibration.cpp:184
RS2_CALIB_TARGET_POS_GAUSSIAN_DOT_VERTICES
@ RS2_CALIB_TARGET_POS_GAUSSIAN_DOT_VERTICES
Definition: rs_frame.h:88
algo.h
RS2_FRAME_METADATA_FRAME_COUNTER
@ RS2_FRAME_METADATA_FRAME_COUNTER
Definition: rs_frame.h:31
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_NOT_ACTIVE
@ RS2_OCC_STATE_NOT_ACTIVE
rs2_deproject_pixel_to_point
void rs2_deproject_pixel_to_point(float point[3], const rs2_intrinsics *intrin, const float pixel[2], float depth)
librealsense::command::data
std::vector< uint8_t > data
Definition: hw-monitor.h:254
test-projection-from-recording.color_profile
color_profile
Definition: test-projection-from-recording.py:28
librealsense::very_high
@ very_high
Definition: d400-auto-calibration.cpp:124
librealsense::auto_calibrated::parse_json
std::map< std::string, int > parse_json(std::string json)
Definition: d400-auto-calibration.cpp:204
librealsense::DscResultParams::m_status
uint16_t m_status
Definition: d400-auto-calibration.cpp:65
librealsense::TareCalibrationResult
Definition: d400-auto-calibration.cpp:28
librealsense::auto_calibrated::change_preset_and_stay
void change_preset_and_stay()
Definition: d400-auto-calibration.cpp:1594
LOG_WARNING
#define LOG_WARNING(...)
Definition: easyloggingpp.h:72
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL
@ RS2_OCC_STATE_WAIT_FOR_FINAL_FW_CALL
test-projection-from-recording.color_intrin
color_intrin
Definition: test-projection-from-recording.py:44
librealsense::set_coefficients
@ set_coefficients
Definition: d400-auto-calibration.cpp:102
width
GLint GLsizei width
Definition: glad/glad/glad.h:1397
size
GLsizeiptr size
Definition: glad/glad/glad.h:2734
librealsense::auto_calibrated::_max_valid_depth
uint16_t _max_valid_depth
Definition: d400-auto-calibration.h:107
librealsense::ds_advanced_mode_base::set_all
void set_all(const preset &p)
Definition: ds/advanced_mode/advanced_mode.cpp:769
librealsense::FocalLengthCalibrationResult
Definition: d400-auto-calibration.cpp:41
val
GLuint GLfloat * val
Definition: glad/glad/glad.h:3411
librealsense::json
rsutils::json json
Definition: json_loader.hpp:26
rs2_rs400_visual_preset_to_string
const char * rs2_rs400_visual_preset_to_string(rs2_rs400_visual_preset preset)
Definition: rs_advanced_mode.cpp:46
librealsense::speed_medium
@ speed_medium
Definition: d400-auto-calibration.cpp:109
librealsense::DscPyRxFLCalibrationTableResult::tableSize
uint16_t tableSize
Definition: d400-auto-calibration.cpp:60
m
std::mutex m
Definition: test-waiting-on.cpp:126
librealsense::RS2_DSC_STATUS_SUCCESS
@ RS2_DSC_STATUS_SUCCESS
Definition: d400-auto-calibration.cpp:78
librealsense::ds::table_header::table_size
uint32_t table_size
Definition: ds-private.h:263
r
GLdouble GLdouble r
Definition: glad/glad/glad.h:1853
librealsense::auto_calibrated::process_calibration_frame
std::vector< uint8_t > process_calibration_frame(int timeout_ms, const rs2_frame *f, float *const health, rs2_update_progress_callback_sptr progress_callback) override
Definition: d400-auto-calibration.cpp:1279
librealsense::focal_length_calib_begin
@ focal_length_calib_begin
Definition: d400-auto-calibration.cpp:98
test-unit-transform.fx
fx
Definition: test-unit-transform.py:25
librealsense::thermal_compensation_guard::~thermal_compensation_guard
virtual ~thermal_compensation_guard()
Definition: d400-auto-calibration.cpp:255
librealsense::DscPyRxFLCalibrationTableResult::FL_heathCheck
float FL_heathCheck
Definition: d400-auto-calibration.cpp:59
librealsense::param4::param4_struct
params4 param4_struct
Definition: d400-auto-calibration.cpp:165
RS2_OPTION_THERMAL_COMPENSATION
@ RS2_OPTION_THERMAL_COMPENSATION
Definition: rs_option.h:101
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CALIB_START
@ RS2_OCC_STATE_WAIT_TO_CALIB_START
librealsense::auto_calibrated::get_calibration_status
DirectSearchCalibrationResult get_calibration_status(int timeout_ms, std::function< void(const int count)> progress_func, bool wait_for_final_results=true)
Definition: d400-auto-calibration.cpp:277
librealsense::_roi_we
const int _roi_we
Definition: algo.h:151
uint32_t
unsigned int uint32_t
Definition: stdint.h:80
test-unit-transform.ppx
ppx
Definition: test-unit-transform.py:23
rs2_intrinsics::fx
float fx
Definition: rs_types.h:64
rs2::video_stream_profile
Definition: rs_frame.hpp:201
librealsense::RS2_DSC_STATUS_BURN_SUCCESS
@ RS2_DSC_STATUS_BURN_SUCCESS
Definition: d400-auto-calibration.cpp:83
librealsense::auto_calib_sub_cmd
auto_calib_sub_cmd
Definition: d400-auto-calibration.cpp:90
librealsense::ds::d400_calibration_table_id::coefficients_table_id
@ coefficients_table_id
librealsense::DscPyRxFLCalibrationTableResult::healthCheck
float healthCheck
Definition: d400-auto-calibration.cpp:58
librealsense::auto_calibrated::check_params
void check_params(int speed, int scan_parameter, int data_sampling) const
Definition: d400-auto-calibration.cpp:1626
librealsense::DscPyRxFLCalibrationTableResult::status
uint16_t status
Definition: d400-auto-calibration.cpp:57
librealsense::tare_params3::reserved
uint8_t reserved
Definition: d400-auto-calibration.cpp:147
librealsense::thermal_compensation_guard::restart_tl
bool restart_tl
Definition: d400-auto-calibration.cpp:268
opencv_pointcloud_viewer.tmp
tmp
Definition: opencv_pointcloud_viewer.py:325
height
GLint GLsizei GLsizei height
Definition: glad/glad/glad.h:1397
rs2_intrinsics::model
rs2_distortion model
Definition: rs_types.h:66
librealsense::py_rx_calib_begin
@ py_rx_calib_begin
Definition: d400-auto-calibration.cpp:94
librealsense::speed_slow
@ speed_slow
Definition: d400-auto-calibration.cpp:110
librealsense::FocalLengthCalibrationResult::fillFactor0
uint16_t fillFactor0
Definition: d400-auto-calibration.cpp:51
librealsense::ds::table_header::table_type
uint16_t table_type
Definition: ds-private.h:262
value
GLfloat value
Definition: glad/glad/glad.h:2099
d400-auto-calibration.h
librealsense::subpixel_accuracy
subpixel_accuracy
Definition: d400-auto-calibration.cpp:122
f
GLdouble f
Definition: glad/glad/glad.h:1517
i
int i
Definition: rs-pcl-color.cpp:54
librealsense::RS2_DSC_STATUS_RESULT_NOT_READY
@ RS2_DSC_STATUS_RESULT_NOT_READY
Definition: d400-auto-calibration.cpp:79
z
GLdouble GLdouble z
Definition: glad/glad/glad.h:1733
librealsense::auto_calibrated::_resize_factor
int _resize_factor
Definition: d400-auto-calibration.h:101
librealsense::ds::CAL_RESTORE_DFLT
@ CAL_RESTORE_DFLT
Definition: ds-private.h:111
librealsense::_patch_size
const int _patch_size
Definition: algo.h:154
librealsense::py_rx_calib_check_status
@ py_rx_calib_check_status
Definition: d400-auto-calibration.cpp:92
librealsense::TareCalibrationResult::curPx
int32_t curPx
Definition: d400-auto-calibration.cpp:33
librealsense::scan_parameter
scan_parameter
Definition: d400-auto-calibration.cpp:136
j
GLint j
Definition: glad/glad/glad.h:2165
librealsense::ds::AUTO_CALIB
@ AUTO_CALIB
Definition: ds-private.h:125
librealsense::DEFAULT_OCC_FL_SCAN_LOCATION
const int DEFAULT_OCC_FL_SCAN_LOCATION
Definition: d400-auto-calibration.cpp:186
librealsense::ds::SETINTCALNEW
@ SETINTCALNEW
Definition: ds-private.h:112
depth_auto_calibration_example.progress_callback
def progress_callback(progress)
Definition: depth_auto_calibration_example.py:96
librealsense::_roi_ws
const int _roi_ws
Definition: algo.h:150
rsutils::number::calc_crc32
uint32_t calc_crc32(const uint8_t *buf, size_t bufsize)
Calculate CRC code for arbitrary characters buffer.
Definition: crc32.cpp:47
rs2_metadata_type
long long rs2_metadata_type
Definition: rs_types.h:272
librealsense::data_sampling
data_sampling
Definition: d400-auto-calibration.cpp:130
rs2_intrinsics::ppy
float ppy
Definition: rs_types.h:63
librealsense::auto_calibrated::check_tare_params
void check_tare_params(int speed, int scan_parameter, int data_sampling, int average_step_count, int step_count, int accuracy)
Definition: d400-auto-calibration.cpp:1642
angle
cv::RotateFlags angle(cv::RotateFlags::ROTATE_90_CLOCKWISE)
y2
GLdouble GLdouble GLdouble y2
Definition: glad/glad/glad.h:1781
librealsense::DscResultParams::m_healthCheck
float m_healthCheck
Definition: d400-auto-calibration.cpp:66
librealsense::FocalLengthCalibrationResult::leftFx
float leftFx
Definition: d400-auto-calibration.cpp:49
librealsense::auto_calibrated::get_target_dots_info
void get_target_dots_info(rs2_frame_queue *frames, float dots_x[4], float dots_y[4], rs2::stream_profile &profile, rs2_intrinsics &fy, int progress, rs2_update_progress_callback_sptr progress_callback)
Definition: d400-auto-calibration.cpp:2127
d400-thermal-monitor.h
librealsense::auto_calibrated::_old_preset
rs2_rs400_visual_preset _old_preset
Definition: d400-auto-calibration.h:92
test-shorten-json.ss
string ss
Definition: test-shorten-json.py:119
librealsense::tare_params3::average_step_count
uint8_t average_step_count
Definition: d400-auto-calibration.cpp:144
librealsense::frame::get_frame_number
unsigned long long get_frame_number() const override
Definition: frame.cpp:177
librealsense::RS2_DSC_STATUS_BURN_ERROR
@ RS2_DSC_STATUS_BURN_ERROR
Definition: d400-auto-calibration.cpp:84
test-post-processing-from-bag.align
align
Definition: test-post-processing-from-bag.py:108
librealsense::FocalLengthCalibrationResult::rightFy
float rightFy
Definition: d400-auto-calibration.cpp:46
opencv_pointcloud_viewer.now
now
Definition: opencv_pointcloud_viewer.py:314
librealsense::DscResultParams
Definition: d400-auto-calibration.cpp:63
rs2_intrinsics::fy
float fy
Definition: rs_types.h:65
librealsense::DEFAULT_CALIB_TYPE
const int DEFAULT_CALIB_TYPE
Definition: d400-auto-calibration.cpp:169
librealsense::tare_params3
Definition: d400-auto-calibration.cpp:142
librealsense::get_py_rx_plus_fl_calib_result
@ get_py_rx_plus_fl_calib_result
Definition: d400-auto-calibration.cpp:101
start
GLuint start
Definition: glad/glad/glad.h:2395
librealsense::_roi_hs
const int _roi_hs
Definition: algo.h:152
librealsense::FocalLengthCalibrationResult::rightFx
float rightFx
Definition: d400-auto-calibration.cpp:47
librealsense::auto_calibrated::_collected_frame_num
int _collected_frame_num
Definition: d400-auto-calibration.h:99
librealsense::video_frame
Definition: video-frame.h:12
librealsense::auto_calibrated::write_calibration
void write_calibration() const override
Definition: d400-auto-calibration.cpp:1805
librealsense::frame
Definition: frame.h:19
librealsense::rect_calculator::extract_target_dims
int extract_target_dims(const rs2_frame *frame_ref, float4 &rect_sides)
Definition: algo.cpp:1018
RS2_FRAME_METADATA_CALIB_INFO
@ RS2_FRAME_METADATA_CALIB_INFO
Definition: rs_frame.h:75
librealsense::tare_calib_check_status
@ tare_calib_check_status
Definition: d400-auto-calibration.cpp:96
librealsense::auto_calibrated::calculate_target_z
float calculate_target_z(rs2_frame_queue *queue1, rs2_frame_queue *queue2, rs2_frame_queue *queue3, float target_width, float target_height, rs2_update_progress_callback_sptr progress_callback) override
Definition: d400-auto-calibration.cpp:2479
librealsense::auto_calibrated::_collected_sum
double _collected_sum
Definition: d400-auto-calibration.h:100
librealsense::DscResultBuffer::m_paramSize
uint16_t m_paramSize
Definition: d400-auto-calibration.cpp:71
librealsense::auto_calibrated::calc_fill_rate
uint16_t calc_fill_rate(const rs2_frame *f)
Definition: d400-auto-calibration.cpp:1055
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_DATA_COLLECT
@ RS2_OCC_STATE_DATA_COLLECT
librealsense::get_focal_legth_calib_result
@ get_focal_legth_calib_result
Definition: d400-auto-calibration.cpp:99
librealsense::auto_calibrated::set_hw_monitor_for_auto_calib
void set_hw_monitor_for_auto_calib(std::shared_ptr< hw_monitor > hwm)
Definition: d400-auto-calibration.cpp:2570
librealsense::auto_calibrated::change_preset
std::shared_ptr< ds_advanced_mode_base > change_preset()
Definition: d400-auto-calibration.cpp:1566
librealsense::auto_calibrated::remove_outliers
void remove_outliers(uint16_t data[256], int size)
Definition: d400-auto-calibration.cpp:1158
librealsense::auto_calibrated::_json
std::string _json
Definition: d400-auto-calibration.h:94
realsense_device_manager.c
c
Definition: realsense_device_manager.py:322
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_WAIT_TO_CAMERA_START
@ RS2_OCC_STATE_WAIT_TO_CAMERA_START
librealsense::DirectSearchCalibrationResult
Definition: d400-auto-calibration.h:14
done
static int done
Definition: unit-tests-common.cpp:344
librealsense::auto_calibrated::auto_calib_action::RS2_OCC_ACTION_TARE_CALIB
@ RS2_OCC_ACTION_TARE_CALIB
RS2_RS400_VISUAL_PRESET_DEFAULT
@ RS2_RS400_VISUAL_PRESET_DEFAULT
Definition: rs_option.h:199
assert
#define assert(condition)
Definition: lz4.c:245
librealsense::rs2_dsc_status
rs2_dsc_status
Definition: d400-auto-calibration.cpp:76
librealsense::TareCalibrationResult::status
uint16_t status
Definition: d400-auto-calibration.cpp:30
librealsense::auto_calibrated::get_PyRxFL_calibration_results
std::vector< uint8_t > get_PyRxFL_calibration_results(float *const health=nullptr, float *health_fl=nullptr) const
Definition: d400-auto-calibration.cpp:1752
librealsense::auto_calibrated::auto_calib_action
auto_calib_action
Definition: d400-auto-calibration.h:31
rs2_rs400_visual_preset
rs2_rs400_visual_preset
For RS400 devices: provides optimized settings (presets) for specific types of usage.
Definition: rs_option.h:196
librealsense::FocalLengthCalibrationResult::scanRange
uint16_t scanRange
Definition: d400-auto-calibration.cpp:45
librealsense::host_assistance_type::no_assistance
@ no_assistance
key
void key(GLFWwindow *window, int k, int s, int action, int mods)
Definition: gears.c:213
rs2_intrinsics::width
int width
Definition: rs_types.h:60
librealsense::option::set
virtual void set(float value)=0
librealsense::DEFAULT_ADJUST_BOTH_SIDES
const int DEFAULT_ADJUST_BOTH_SIDES
Definition: d400-auto-calibration.cpp:182
librealsense::auto_calibrated::_prev_frame_counter
rs2_metadata_type _prev_frame_counter
Definition: d400-auto-calibration.h:105
librealsense::rx_scan
@ rx_scan
Definition: d400-auto-calibration.cpp:139
librealsense::try_fetch
void try_fetch(std::map< std::string, int > jsn, std::string key, int *value)
Definition: d400-auto-calibration.cpp:218
librealsense::thermal_compensation_guard::operator=
thermal_compensation_guard & operator=(const thermal_compensation_guard &)
librealsense::auto_calibrated::fill_missing_data
void fill_missing_data(uint16_t data[256], int size)
Definition: d400-auto-calibration.cpp:1116
librealsense::tare_calibration_params::param3
uint32_t param3
Definition: d400-auto-calibration.cpp:160
librealsense::TareCalibrationResult::calRightRotation
float calRightRotation[9]
Definition: d400-auto-calibration.cpp:36
rsutils::string::from
Definition: from.h:19
librealsense::DscPyRxFLCalibrationTableResult
Definition: d400-auto-calibration.cpp:54
librealsense::options_interface::get_option
virtual option & get_option(rs2_option id)=0
librealsense::tare_params3::step_count
uint8_t step_count
Definition: d400-auto-calibration.cpp:145
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_FINAL_FW_CALL
@ RS2_OCC_STATE_FINAL_FW_CALL
int32_t
signed int int32_t
Definition: stdint.h:77
librealsense::options_interface::supports_option
virtual bool supports_option(rs2_option id) const =0
p
double p[GRIDW][GRIDH]
Definition: wave.c:109
rad2deg
T rad2deg(T val)
Definition: src/types.h:46
librealsense::auto_calibrated::_old_preset_values
preset _old_preset_values
Definition: d400-auto-calibration.h:91
librealsense::auto_calibrated::_average_step_count
int _average_step_count
Definition: d400-auto-calibration.h:97
librealsense::auto_calibrated::_collected_counter
int _collected_counter
Definition: d400-auto-calibration.h:98
librealsense::auto_calibrated::run_focal_length_calibration
std::vector< uint8_t > run_focal_length_calibration(rs2_frame_queue *left, rs2_frame_queue *right, float target_w, float target_h, int adjust_both_sides, float *ratio, float *angle, rs2_update_progress_callback_sptr progress_callback) override
Definition: d400-auto-calibration.cpp:1933
test-fps.ds
ds
Definition: test-fps.py:77
librealsense::get_calibration_result
@ get_calibration_result
Definition: d400-auto-calibration.cpp:97
librealsense::DEFAULT_ACCURACY
const int DEFAULT_ACCURACY
Definition: d400-auto-calibration.cpp:173
librealsense::ds_advanced_mode_base
Definition: advanced_mode.h:101
rs2_frame_queue_size
int rs2_frame_queue_size(rs2_frame_queue *queue, rs2_error **error)
Definition: rs.cpp:1552
librealsense::tare_calibration_params
Definition: d400-auto-calibration.cpp:157
librealsense::auto_calib_speed
auto_calib_speed
Definition: d400-auto-calibration.cpp:105
librealsense::auto_calibrated::_action
auto_calib_action _action
Definition: d400-auto-calibration.h:102
rs2::frame::get_profile
stream_profile get_profile() const
Definition: rs_frame.hpp:559
librealsense::thermal_compensation_guard
Definition: d400-auto-calibration.cpp:228
librealsense::auto_calibrated::_fill_factor
uint16_t _fill_factor[256]
Definition: d400-auto-calibration.h:106
librealsense::align
Definition: align.h:14
librealsense::auto_calibrated::auto_calib_action::RS2_OCC_ACTION_ON_CHIP_CALIB
@ RS2_OCC_ACTION_ON_CHIP_CALIB
librealsense::preset
Definition: presets.h:99
json.h
librealsense::DscResultBuffer::m_tableSize
uint16_t m_tableSize
Definition: d400-auto-calibration.cpp:73
librealsense::frame::get_frame_data
const uint8_t * get_frame_data() const override
Definition: frame.cpp:155
librealsense::py_rx_plus_fl_calib_begin
@ py_rx_plus_fl_calib_begin
Definition: d400-auto-calibration.cpp:100
rmse.e
e
Definition: rmse.py:177
librealsense::ds::fw_cmd
fw_cmd
Definition: ds-private.h:83
librealsense::auto_calibrated::get_calibration_results
std::vector< uint8_t > get_calibration_results(float *const health=nullptr) const
Definition: d400-auto-calibration.cpp:1725
librealsense::ds::CALIBRECALC
@ CALIBRECALC
Definition: ds-private.h:109
img
GLint void * img
Definition: glad/glad/glad.h:2435
librealsense::DscPyRxFLCalibrationTableResult::headerSize
uint16_t headerSize
Definition: d400-auto-calibration.cpp:56
x
GLdouble x
Definition: glad/glad/glad.h:2279
librealsense::thermal_compensation_guard::snr
librealsense::sensor_interface * snr
Definition: d400-auto-calibration.cpp:269
librealsense::ds::GETINTCAL
@ GETINTCAL
Definition: ds-private.h:94
librealsense::auto_calibrated::restore_preset
void restore_preset()
Definition: d400-auto-calibration.cpp:1607
librealsense::auto_calibrated::_interactive_scan
bool _interactive_scan
Definition: d400-auto-calibration.h:104
librealsense::ds::d400_calibration_table_id
d400_calibration_table_id
Definition: d400-private.h:181
librealsense::_roi_he
const int _roi_he
Definition: algo.h:153
end
GLuint GLuint end
Definition: glad/glad/glad.h:2395
librealsense::ds::d400_coefficients_table
Definition: d400-private.h:219
librealsense::option::query
virtual float query() const =0
y1
GLdouble y1
Definition: glad/glad/glad.h:1781
librealsense::auto_calibrated::run_uv_map_calibration
std::vector< uint8_t > run_uv_map_calibration(rs2_frame_queue *left, rs2_frame_queue *color, rs2_frame_queue *depth, int py_px_only, float *const health, int health_size, rs2_update_progress_callback_sptr progress_callback) override
Definition: d400-auto-calibration.cpp:2298
rs2_intrinsics::coeffs
float coeffs[5]
Definition: rs_types.h:67
librealsense::auto_calibrated::_interactive_state
interactive_calibration_state _interactive_state
Definition: d400-auto-calibration.h:103
it
static auto it
Definition: openvino-face-detection.cpp:375
RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY
@ RS2_RS400_VISUAL_PRESET_HIGH_ACCURACY
Definition: rs_option.h:201
librealsense::auto_calibrated::check_focal_length_params
void check_focal_length_params(int step_count, int fy_scan_range, int keep_new_value_after_sucessful_scan, int interrrupt_data_samling, int adjust_both_sides, int fl_scan_location, int fy_scan_direction, int white_wall_mode) const
Definition: d400-auto-calibration.cpp:1660
test-depth.result
result
Definition: test-depth.py:183
test-unit-transform.ppy
ppy
Definition: test-unit-transform.py:24
librealsense::RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW
@ RS2_DSC_STATUS_FILL_FACTOR_TOO_LOW
Definition: d400-auto-calibration.cpp:80
librealsense::auto_calibrated::interactive_calibration_state::RS2_OCC_STATE_INITIAL_FW_CALL
@ RS2_OCC_STATE_INITIAL_FW_CALL
unit-test-config.handle
handle
Definition: unit-test-config.py:371
s
GLdouble s
Definition: glad/glad/glad.h:2441
librealsense::TareCalibrationResult::curRightRotation
float curRightRotation[9]
Definition: d400-auto-calibration.cpp:35
librealsense::FocalLengthCalibrationResult::stepCount
uint16_t stepCount
Definition: d400-auto-calibration.cpp:44
librealsense::DscResultBuffer::m_dscResultParams
DscResultParams m_dscResultParams
Definition: d400-auto-calibration.cpp:72
values
GLsizei const GLfloat * values
Definition: glad/glad/glad.h:2180
librealsense::auto_calibrated::handle_calibration_error
void handle_calibration_error(int status) const
Definition: d400-auto-calibration.cpp:1698
librealsense::auto_calibrated::_total_frames
int _total_frames
Definition: d400-auto-calibration.h:96
LOG_INFO
LOG_INFO("Log message using LOG_INFO()")
librealsense::DEFAULT_SAMPLING
const int DEFAULT_SAMPLING
Definition: d400-auto-calibration.cpp:176
RS2_DISTORTION_INVERSE_BROWN_CONRADY
@ RS2_DISTORTION_INVERSE_BROWN_CONRADY
Definition: rs_types.h:49
librealsense::auto_calibrated::auto_calibrated
auto_calibrated()
Definition: d400-auto-calibration.cpp:190
rs2::configurations::ply::binary
@ binary
Definition: device-model.h:174
librealsense::command
Definition: hw-monitor.h:247
librealsense::auto_calibrated::get_calibration_table
std::vector< uint8_t > get_calibration_table() const override
Definition: d400-auto-calibration.cpp:1781
librealsense::auto_calibrated_interface
Definition: auto-calibrated-device.h:12
profile
Definition: unit-tests-common.h:58
librealsense::tare_calibration_params::param3_struct
tare_params3 param3_struct
Definition: d400-auto-calibration.cpp:159
test-unit-transform.fy
fy
Definition: test-unit-transform.py:26
rs2_extract_target_dimensions
void rs2_extract_target_dimensions(const rs2_frame *frame, rs2_calib_target_type calib_type, float *target_dims, unsigned int target_dims_size, rs2_error **error)
Definition: rs.cpp:2863
LOG_ERROR
#define LOG_ERROR(...)
Definition: easyloggingpp.h:73
depth
GLint GLint GLsizei GLsizei GLsizei depth
Definition: glad/glad/glad.h:2398
librealsense::auto_calibrated::_min_valid_depth
uint16_t _min_valid_depth
Definition: d400-auto-calibration.h:107
MatlabParamParser::parse
static T parse(const mxArray *cell)
Definition: MatlabParamParser.h:161
y
GLint y
Definition: glad/glad/glad.h:1397
sw.h
int h
Definition: sw-dev/sw.py:11
librealsense::auto_calibrated::set_calibration_table
void set_calibration_table(const std::vector< uint8_t > &calibration) override
Definition: d400-auto-calibration.cpp:1837
librealsense::DEFAULT_FY_SCAN_DIRECTION
const int DEFAULT_FY_SCAN_DIRECTION
Definition: d400-auto-calibration.cpp:187
librealsense::auto_calibrated::interactive_calibration_state
interactive_calibration_state
Definition: d400-auto-calibration.h:37
librealsense::RS2_DSC_STATUS_NOT_CONVERGE
@ RS2_DSC_STATUS_NOT_CONVERGE
Definition: d400-auto-calibration.cpp:82

librealsense2
Author(s): LibRealSense ROS Team
autogenerated on Fri Aug 2 2024 08:30:01