void align::align_other_to_z(byte* aligned_data, const rs2::video_frame& depth, const rs2::video_frame& other, float z_scale)
{
auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();
auto other_profile = other.get_profile().as<rs2::video_stream_profile>();
auto z_intrin = depth_profile.get_intrinsics();
auto other_intrin = other_profile.get_intrinsics();
auto z_to_other = depth_profile.get_extrinsics_to(other_profile);
auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
auto other_pixels = reinterpret_cast<const byte*>(other.get_data());
align_other_to_depth(aligned_data, [z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
z_intrin, z_to_other, other_intrin, other_pixels, other_profile.format());
}
void align::align_z_to_other(byte* aligned_data, const rs2::video_frame& depth, const rs2::video_stream_profile& other_profile, float z_scale)
{
auto depth_profile = depth.get_profile().as<rs2::video_stream_profile>();
auto z_intrin = depth_profile.get_intrinsics();
auto other_intrin = other_profile.get_intrinsics();
auto z_to_other = depth_profile.get_extrinsics_to(other_profile);
auto z_pixels = reinterpret_cast<const uint16_t*>(depth.get_data());
auto out_z = (uint16_t *)(aligned_data);
align_images(z_intrin, z_to_other, other_intrin,
[z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
[out_z, z_pixels](int z_pixel_index, int other_pixel_index)
{
out_z[other_pixel_index] = out_z[other_pixel_index] ?
std::min((int)out_z[other_pixel_index], (int)z_pixels[z_pixel_index]) :
z_pixels[z_pixel_index];
});
}
void disparity_transform::update_transformation_profile(const rs2::frame& f)
{
if(f.get_profile().get() != _source_stream_profile.get())
{
_source_stream_profile = f.get_profile();
// Check if the new frame originated from stereo-based depth sensor
// and retrieve the stereo baseline parameter that will be used in transformations
auto snr = ((frame_interface*)f.get())->get_sensor().get();
librealsense::depth_stereo_sensor* dss;
// Playback sensor
if (auto a = As<librealsense::extendable_interface>(snr))
{
librealsense::depth_stereo_sensor* ptr;
if (_stereoscopic_depth = a->extend_to(TypeToExtension<librealsense::depth_stereo_sensor>::value, (void**)&ptr))
{
dss = ptr;
_depth_units = dss->get_depth_scale();
_stereo_baseline_meter = dss->get_stereo_baseline_mm()*0.001f;
}
}
else // Live sensor
{
_stereoscopic_depth = Is<librealsense::depth_stereo_sensor>(snr);
if (_stereoscopic_depth)
{
dss = As<librealsense::depth_stereo_sensor>(snr);
_depth_units = dss->get_depth_scale();
_stereo_baseline_meter = dss->get_stereo_baseline_mm()* 0.001f;
}
}
if (_stereoscopic_depth)
{
auto vp = _source_stream_profile.as<rs2::video_stream_profile>();
_focal_lenght_mm = vp.get_intrinsics().fx;
const uint8_t fractional_bits = 5;
const uint8_t fractions = 1 << fractional_bits;
_d2d_convert_factor = (_stereo_baseline_meter * _focal_lenght_mm * fractions) / _depth_units;
_width = vp.width();
_height = vp.height();
_update_target = true;
}
}
// Adjust the target profile
if (_update_target)
{
auto tgt_format = _transform_to_disparity ? RS2_FORMAT_DISPARITY32 : RS2_FORMAT_Z16;
_target_stream_profile = _source_stream_profile.clone(RS2_STREAM_DEPTH, 0, tgt_format);
auto src_vspi = dynamic_cast<video_stream_profile_interface*>(_source_stream_profile.get()->profile);
auto tgt_vspi = dynamic_cast<video_stream_profile_interface*>(_target_stream_profile.get()->profile);
rs2_intrinsics src_intrin = src_vspi->get_intrinsics();
tgt_vspi->set_intrinsics([src_intrin]() { return src_intrin; });
tgt_vspi->set_dims(src_intrin.width, src_intrin.height);
_update_target = false;
}
}
