depth-to-rgb-calibration.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2020 Intel Corporation. All Rights Reserved.
3 
5 #include <librealsense2/rs.hpp>
6 #include "core/streaming.h"
7 #include "calibrated-sensor.h"
8 #include "context.h"
9 #include "api.h" // VALIDATE_INTERFACE_NO_THROW
11 #include "l500/l500-device.h"
12 #include "l500/l500-color.h"
14 
15 using namespace librealsense;
18 
19 #define CHECK_IF_NEEDS_TO_STOP() if (_should_continue) _should_continue()
20 
22  impl::optimizer::settings const & settings,
24  rs2::frame ir,
25  rs2::frame yuy,
26  rs2::frame prev_yuy,
27  std::vector< impl::yuy_t > const & last_yuy_data,
28  impl::algo_calibration_info const & cal_info,
29  impl::algo_calibration_registers const & cal_regs,
30  rs2_intrinsics const & yuy_intrinsics,
31  thermal::thermal_calibration_table_interface const & thermal_table,
32  std::function<void()> should_continue
33 )
34  : _algo( settings )
35  , _raw_intr( yuy_intrinsics )
36  , _thermal_intr( _raw_intr )
37  , _extr(to_raw_extrinsics( depth.get_profile().get_extrinsics_to( yuy.get_profile() )))
38  , _from( depth.get_profile().get()->profile )
39  , _to( yuy.get_profile().get()->profile )
40  , _should_continue( should_continue )
41  , _thermal_table( thermal_table )
42 {
43  AC_LOG( DEBUG, "Setting YUY data" );
45  auto yuy_data = (impl::yuy_t const *) yuy.get_data();
46  auto prev_yuy_data = (impl::yuy_t const *) prev_yuy.get_data();
47  if( last_yuy_data.size() == yuy.get_data_size() / sizeof( impl::yuy_t ) )
48  _last_successful_frame_data = last_yuy_data; // copy -- will be moved to algo
49  else if( ! last_yuy_data.empty() )
50  AC_LOG( DEBUG, "Not using last successfully-calibrated scene: it's of a different resolution" );
51 
52  if( thermal_table.is_valid() )
53  {
54  auto scale = thermal_table.get_thermal_scale( settings.hum_temp );
55  AC_LOG( DEBUG, " scaling K_rgb by {scale}" << scale << " [TH]" );
56  _thermal_intr.fx = float( _thermal_intr.fx * scale );
57  _thermal_intr.fy = float( _thermal_intr.fy * scale );
58  }
59 
60  impl::calib calibration( _thermal_intr, _extr );
61 
63 
65  std::vector< impl::yuy_t >( yuy_data, yuy_data + yuy.get_data_size() / sizeof( impl::yuy_t )),
66  std::vector< impl::yuy_t >( prev_yuy_data, prev_yuy_data + yuy.get_data_size() / sizeof( impl::yuy_t ) ),
68  calibration
69  );
70 
71  AC_LOG( DEBUG, "Setting IR data" );
72  auto ir_profile = ir.get_profile().as< rs2::video_stream_profile >();
73  auto ir_data = (impl::ir_t const *) ir.get_data();
74 
76 
78  std::vector< impl::ir_t >( ir_data, ir_data + ir.get_data_size() / sizeof( impl::ir_t )),
79  ir_profile.width(), ir_profile.height()
80  );
81 
83 
84  auto si = ((frame_interface *) depth.get() )->get_sensor();
86  if( !cs )
87  {
88  // We can only calibrate depth sensors that supply this interface!
89  throw librealsense::not_implemented_exception( "the depth frame supplied is not from a calibrated_sensor" );
90  }
91  _dsm_params = cs->get_dsm_params();
92 
93  AC_LOG( DEBUG, "Setting Z data" );
94  auto z_profile = depth.get_profile().as< rs2::video_stream_profile >();
95  auto z_data = (impl::z_t const *) depth.get_data();
96 
98 
100  std::vector< impl::z_t >( z_data, z_data + depth.get_data_size() / sizeof( impl::z_t ) ),
101  z_profile.get_intrinsics(), _dsm_params, cal_info, cal_regs,
102  depth.as< rs2::depth_frame >().get_units() * 1000.f // same scaling as for extrinsics!
103  );
104 
105  debug_calibration( "old" );
106 }
107 
108 
110 {
111  _algo.write_data_to( dir );
112 
113  // VAL asked that we write this file even with no thermal table, so they can just make up
114  // something fictitious...
115  impl::write_to_file( &_raw_intr, sizeof( _raw_intr ), dir, "raw_rgb.intrinsics" );
116  if( _thermal_table.is_valid() )
117  {
118  impl::write_vector_to_file( _thermal_table.build_raw_data(), dir, "rgb_thermal_table" );
119  }
120 }
121 
122 
124  std::function<void( rs2_calibration_status )> call_back
125 )
126 {
127 #define DISABLE_RS2_CALIBRATION_CHECKS "RS2_AC_DISABLE_RETRIES"
128 
129  try
130  {
131  AC_LOG( DEBUG, "Checking scene validity" );
132  if( !_algo.is_scene_valid() )
133  {
134  //AC_LOG( ERROR, "Calibration scene was found invalid!" );
135  call_back( RS2_CALIBRATION_SCENE_INVALID );
136  if( !getenv( DISABLE_RS2_CALIBRATION_CHECKS ) )
137  {
138  // Default behavior is to stop AC and trigger a retry
139  return RS2_CALIBRATION_RETRY;
140  }
141  if( getenv( "RS2_AC_INVALID_SCENE_FAIL" ) )
142  {
143  // Here we don't want a retry, but we also do not want the calibration
144  // to possibly be successful -- fail it
145  AC_LOG( DEBUG, DISABLE_RS2_CALIBRATION_CHECKS << " is on but so is RS2_AC_INVALID_SCENE_FAIL: fail!" );
146  return RS2_CALIBRATION_FAILED;
147  }
148  AC_LOG( DEBUG, DISABLE_RS2_CALIBRATION_CHECKS << " is on; continuing" );
149  }
150 
152 
153 
154  AC_LOG( DEBUG, "Optimizing" );
155 
156  _algo.optimize(
157  [&]( impl::data_collect const & data ) { CHECK_IF_NEEDS_TO_STOP(); }
158  );
159 
160  AC_LOG( DEBUG, "Checking result validity" );
161  if( !_algo.is_valid_results() )
162  {
163  // Error would have printed inside
164  call_back( RS2_CALIBRATION_BAD_RESULT );
165  if( !getenv( DISABLE_RS2_CALIBRATION_CHECKS ) )
166  {
167  // Default behavior is to stop and trigger a retry
168  AC_LOG( DEBUG, DISABLE_RS2_CALIBRATION_CHECKS << " is off; will retry if possible" );
169  return RS2_CALIBRATION_RETRY;
170  }
171  if( !getenv( "RS2_AC_FORCE_BAD_RESULT" ) )
172  {
173  // This is mostly for validation use, where we don't want the retries and instead want
174  // to fail on bad results (we don't want to write bad results to the camera!)
175  AC_LOG( DEBUG, DISABLE_RS2_CALIBRATION_CHECKS << " is on; no retries" );
176  return RS2_CALIBRATION_FAILED;
177  }
178  // Allow forcing of results... be careful! This may damage the camera in AC2!
179  AC_LOG( DEBUG, DISABLE_RS2_CALIBRATION_CHECKS << " is on but so is RS2_AC_FORCE_BAD_RESULT: results will be used!" );
180  }
181 
182  // AC_LOG( INFO, "Calibration finished; original cost= " << original_cost << " optimized
183  // cost= " << params_curr.cost );
184 
185  AC_LOG( DEBUG, "Optimization successful!" );
187 
188  _thermal_intr.model = l500_distortion; // restore LRS model
189 
190  // Override everything in the raw intrinsics except the focal length (fx and fy)
191  // TODO: AC is not "supposed" to change focal length, but we shouldn't assume this! The
192  // proper way to do the following is not by restoring the original, but rather by DEscaling
193  // the results from AC.
194  auto original_fx = _raw_intr.fx, original_fy = _raw_intr.fy;
196  _raw_intr.fx = original_fx;
197  _raw_intr.fy = original_fy;
198 
201  _last_successful_frame_data = _algo.get_yuy_data().orig_frame; // copy -- will be moved to ac_trigger
202  debug_calibration( "new" );
204  }
205  catch( std::exception const & e )
206  {
207  AC_LOG( ERROR, "Calibration threw error: " << e.what() );
208  return RS2_CALIBRATION_FAILED;
209  }
210 }
211 
212 
214 {
215  AC_LOG( DEBUG, AC_F_PREC << " " << prefix << " th" << _thermal_intr );
216  AC_LOG( DEBUG, AC_F_PREC << " " << prefix << " raw" << _raw_intr );
217  AC_LOG( DEBUG, AC_F_PREC << " " << prefix << " extr" << _extr );
218  AC_LOG( DEBUG, AC_F_PREC << " " << prefix << " dsm" << _dsm_params );
219 }
220 
algo::depth_to_rgb_calibration::optimizer _algo
rs2_extrinsics_double get_extrinsics() const
Definition: calibration.cpp:63
rs2_frame * get() const
Definition: rs_frame.hpp:590
void write_to_file(void const *data, size_t cb, std::string const &dir, char const *filename)
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:10806
#define CHECK_IF_NEEDS_TO_STOP()
stream_profile get_profile() const
Definition: rs_frame.hpp:557
rs2_intrinsics_double get_intrinsics() const
Definition: calibration.cpp:55
algo::thermal_loop::thermal_calibration_table_interface const & _thermal_table
#define DISABLE_RS2_CALIBRATION_CHECKS
GLint GLint GLsizei GLsizei GLsizei depth
const void * get_data() const
Definition: rs_frame.hpp:545
GLsizei const GLchar *const * string
e
Definition: rmse.py:177
depth_to_rgb_calibration(algo::depth_to_rgb_calibration::optimizer::settings const &settings, rs2::frame depth, rs2::frame ir, rs2::frame yuy, rs2::frame prev_yuy, std::vector< algo::depth_to_rgb_calibration::yuy_t > const &last_yuy_data, algo::depth_to_rgb_calibration::algo_calibration_info const &cal_info, algo::depth_to_rgb_calibration::algo_calibration_registers const &cal_regs, rs2_intrinsics const &yuy_intrinsics, algo::thermal_loop::thermal_calibration_table_interface const &, std::function< void()> should_continue=nullptr)
size_t optimize(std::function< void(data_collect const &data) > iteration_callback=nullptr)
Definition: optimizer.cpp:1977
void write_vector_to_file(std::vector< T > const &v, std::string const &dir, char const *filename)
Definition: utils.h:39
rs2_extrinsics to_raw_extrinsics(rs2_extrinsics extr)
Definition: types.cpp:26
void set_z_data(std::vector< z_t > &&z_data, rs2_intrinsics_double const &depth_intrinsics, rs2_dsm_params const &dms_params, algo_calibration_info const &cal_info, algo_calibration_registers const &cal_regs, float depth_units)
Definition: optimizer.cpp:455
#define VALIDATE_INTERFACE_NO_THROW(X, T)
Definition: api.h:411
rs2_calibration_status optimize(std::function< void(rs2_calibration_status)> call_back=nullptr)
rs2_extrinsics from_raw_extrinsics(rs2_extrinsics extr)
Definition: types.cpp:39
rs2_calibration_status
Definition: rs_device.h:356
void set_yuy_data(std::vector< yuy_t > &&yuy_data, std::vector< yuy_t > &&prev_yuy_data, std::vector< yuy_t > &&last_successful_yuy_data, calib const &calibration)
Definition: optimizer.cpp:991
BOOST_DEDUCED_TYPENAME optional< T >::reference_const_type get(optional< T > const &opt)
#define AC_LOG(TYPE, MSG)
yuy2_frame_data const & get_yuy_data() const
Definition: optimizer.h:348
rs2_distortion model
Definition: rs_types.h:66
void set_ir_data(std::vector< ir_t > &&ir_data, size_t width, size_t height)
Definition: optimizer.cpp:1081
std::vector< algo::depth_to_rgb_calibration::yuy_t > _last_successful_frame_data
bool is_scene_valid(input_validity_data *data=nullptr)
typename::boost::move_detail::remove_reference< T >::type && move(T &&t) BOOST_NOEXCEPT
Video stream intrinsics.
Definition: rs_types.h:58
const int get_data_size() const
Definition: rs_frame.hpp:533
Definition: parser.hpp:150
const rs2_distortion l500_distortion
Definition: l500-color.h:19
void write_data_to(std::string const &directory)
Definition: optimizer.cpp:1715
T as() const
Definition: rs_frame.hpp:580


librealsense2
Author(s): Sergey Dorodnicov , Doron Hirshberg , Mark Horn , Reagan Lopez , Itay Carpis
autogenerated on Mon May 3 2021 02:47:12