You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
726 lines
27 KiB
726 lines
27 KiB
// License: Apache 2.0. See LICENSE file in root directory.
|
|
// Copyright(c) 2015 Intel Corporation. All Rights Reserved.
|
|
|
|
#include <librealsense2/rs.hpp>
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
#include <map>
|
|
#include <set>
|
|
#include <cstring>
|
|
#include <cmath>
|
|
#include <limits>
|
|
#include <thread>
|
|
|
|
#include <rsutils/json.h>
|
|
using rsutils::json;
|
|
|
|
#include "tclap/CmdLine.h"
|
|
|
|
using namespace std;
|
|
using namespace TCLAP;
|
|
using namespace rs2;
|
|
|
|
std::vector<std::string> tokenize_floats(string input, char separator) {
|
|
std::vector<std::string> tokens;
|
|
stringstream ss(input);
|
|
string token;
|
|
|
|
while (std::getline(ss, token, separator)) {
|
|
tokens.push_back(token);
|
|
}
|
|
|
|
return tokens;
|
|
}
|
|
|
|
void print(const rs2_extrinsics& extrinsics)
|
|
{
|
|
stringstream ss;
|
|
ss << " Rotation Matrix:\n";
|
|
|
|
// Align displayed data along decimal point
|
|
for (auto i = 0; i < 3; ++i)
|
|
{
|
|
for (auto j = 0; j < 3; ++j)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << extrinsics.rotation[j * 3 + i];
|
|
auto tokens = tokenize_floats(oss.str().c_str(), '.');
|
|
ss << right << setw(4) << tokens[0];
|
|
if (tokens.size() > 1)
|
|
ss << "." << left << setw(12) << tokens[1];
|
|
}
|
|
ss << endl;
|
|
}
|
|
|
|
ss << "\n Translation Vector: ";
|
|
for (auto i = 0u; i < sizeof(extrinsics.translation) / sizeof(extrinsics.translation[0]); ++i)
|
|
ss << setprecision(15) << extrinsics.translation[i] << " ";
|
|
|
|
cout << ss.str() << endl << endl;
|
|
}
|
|
|
|
void print(const rs2_motion_device_intrinsic& intrinsics)
|
|
{
|
|
stringstream ss;
|
|
ss << "Bias Variances: \t";
|
|
|
|
for (auto i = 0u; i < sizeof(intrinsics.bias_variances) / sizeof(intrinsics.bias_variances[0]); ++i)
|
|
ss << setprecision(15) << std::fixed << intrinsics.bias_variances[i] << " ";
|
|
|
|
ss << "\nNoise Variances: \t";
|
|
for (auto i = 0u; i < sizeof(intrinsics.noise_variances) / sizeof(intrinsics.noise_variances[0]); ++i)
|
|
ss << setprecision(15) << std::fixed << intrinsics.noise_variances[i] << " ";
|
|
|
|
ss << "\nSensitivity : " << std::endl;
|
|
for (auto i = 0u; i < sizeof(intrinsics.data) / sizeof(intrinsics.data[0]); ++i)
|
|
{
|
|
for (auto j = 0u; j < sizeof(intrinsics.data[0]) / sizeof(intrinsics.data[0][0]); ++j)
|
|
ss << std::right << std::setw(13) << setprecision(6) << intrinsics.data[i][j] << " ";
|
|
ss << "\n";
|
|
}
|
|
|
|
cout << ss.str() << endl << endl;
|
|
}
|
|
|
|
void print(const rs2_intrinsics& intrinsics)
|
|
{
|
|
stringstream ss;
|
|
ss << left << setw(14) << " Width: " << "\t" << intrinsics.width << endl <<
|
|
left << setw(14) << " Height: " << "\t" << intrinsics.height << endl <<
|
|
left << setw(14) << " PPX: " << "\t" << setprecision(15) << intrinsics.ppx << endl <<
|
|
left << setw(14) << " PPY: " << "\t" << setprecision(15) << intrinsics.ppy << endl <<
|
|
left << setw(14) << " Fx: " << "\t" << setprecision(15) << intrinsics.fx << endl <<
|
|
left << setw(14) << " Fy: " << "\t" << setprecision(15) << intrinsics.fy << endl <<
|
|
left << setw(14) << " Distortion: " << "\t" << rs2_distortion_to_string(intrinsics.model) << endl <<
|
|
left << setw(14) << " Coeffs: ";
|
|
|
|
for (auto i = 0u; i < sizeof(intrinsics.coeffs) / sizeof(intrinsics.coeffs[0]); ++i)
|
|
ss << "\t" << setprecision(15) << intrinsics.coeffs[i] << " ";
|
|
|
|
float fov[2];
|
|
rs2_fov(&intrinsics, fov);
|
|
ss << endl << left << setw(14) << " FOV (deg): " << "\t" << setprecision(4) << fov[0] << " x " << fov[1];
|
|
|
|
cout << ss.str() << endl << endl;
|
|
}
|
|
|
|
bool safe_get_intrinsics(const video_stream_profile& profile, rs2_intrinsics& intrinsics)
|
|
{
|
|
bool ret = false;
|
|
try
|
|
{
|
|
intrinsics = profile.get_intrinsics();
|
|
ret = true;
|
|
}
|
|
catch (...)
|
|
{
|
|
}
|
|
|
|
return ret;
|
|
}
|
|
|
|
bool safe_get_motion_intrinsics(const motion_stream_profile& profile, rs2_motion_device_intrinsic& intrinsics)
|
|
{
|
|
bool ret = false;
|
|
try
|
|
{
|
|
intrinsics = profile.get_motion_intrinsics();
|
|
ret = true;
|
|
}
|
|
catch (...)
|
|
{
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
struct stream_and_resolution {
|
|
rs2_stream stream;
|
|
int stream_index;
|
|
int width;
|
|
int height;
|
|
string stream_name;
|
|
|
|
bool operator <(const stream_and_resolution& obj) const
|
|
{
|
|
return (std::make_tuple(stream, stream_index, width, height) < std::make_tuple(obj.stream, obj.stream_index, obj.width, obj.height));
|
|
}
|
|
};
|
|
|
|
struct stream_and_index {
|
|
rs2_stream stream;
|
|
int stream_index;
|
|
|
|
bool operator <(const stream_and_index& obj) const
|
|
{
|
|
return (std::make_tuple(stream, stream_index) < std::make_tuple(obj.stream, obj.stream_index));
|
|
}
|
|
};
|
|
|
|
bool operator ==(const rs2_intrinsics& lhs,
|
|
const rs2_intrinsics& rhs)
|
|
{
|
|
return lhs.width == rhs.width &&
|
|
lhs.height == rhs.height &&
|
|
(fabs(lhs.ppx - rhs.ppx) < std::numeric_limits<float>::min()) &&
|
|
(fabs(lhs.ppy - rhs.ppy) < std::numeric_limits<float>::min()) &&
|
|
(fabs(lhs.fx - rhs.fx) < std::numeric_limits<float>::min()) &&
|
|
(fabs(lhs.fy - rhs.fy) < std::numeric_limits<float>::min()) &&
|
|
lhs.model == rhs.model &&
|
|
!std::memcmp(lhs.coeffs, rhs.coeffs, sizeof(rhs.coeffs));
|
|
}
|
|
|
|
bool operator ==(const rs2_motion_device_intrinsic& lhs,
|
|
const rs2_motion_device_intrinsic& rhs)
|
|
{
|
|
return !std::memcmp(&lhs, &rhs, sizeof(rs2_motion_device_intrinsic));
|
|
}
|
|
|
|
string get_str_formats(const set<rs2_format>& formats)
|
|
{
|
|
stringstream ss;
|
|
for (auto format = formats.begin(); format != formats.end(); ++format)
|
|
{
|
|
ss << *format << ((format != formats.end()) && (next(format) == formats.end()) ? "" : "/");
|
|
}
|
|
return ss.str();
|
|
}
|
|
|
|
void output_modes( std::vector< stream_profile > const & profiles, bool verbose, bool show_defaults )
|
|
{
|
|
size_t const w_res = 15;
|
|
|
|
size_t w_format = 10;
|
|
size_t w_stream = 10;
|
|
bool video_stream = false;
|
|
for( auto const & profile : profiles )
|
|
{
|
|
w_stream = std::max( profile.stream_name().length(), w_stream );
|
|
w_format = std::max( strlen( rs2_format_to_string( profile.format() ) ), w_format );
|
|
if( auto video = profile.as< video_stream_profile >() )
|
|
video_stream = true;
|
|
}
|
|
w_stream += 2;
|
|
w_format += 2;
|
|
|
|
// Heading
|
|
if( verbose )
|
|
cout << " (UID.IDX) ";
|
|
else
|
|
cout << " ";
|
|
cout << setw( w_stream ) << "STREAM";
|
|
if( video_stream )
|
|
cout << setw( w_res ) << "RESOLUTION";
|
|
cout << setw( w_format ) << "FORMAT";
|
|
cout << "FPS";
|
|
cout << endl;
|
|
// Show which streams are supported by this device
|
|
if( verbose )
|
|
{
|
|
for( auto const & profile : profiles )
|
|
{
|
|
cout << ( show_defaults && profile.is_default() ? " + " : " " );
|
|
cout << setw( 4 ) << right << ( " (" + std::to_string( profile.unique_id() ) );
|
|
cout << '.';
|
|
cout << setw( 5 ) << left << ( std::to_string( profile.stream_index() ) + ')' );
|
|
cout << setw( w_stream ) << profile.stream_name();
|
|
if( auto video = profile.as< video_stream_profile >() )
|
|
{
|
|
cout << setw( 4 ) << right << video.width();
|
|
cout << 'x';
|
|
cout << setw( w_res - 5 ) << left << video.height();
|
|
}
|
|
cout << setw( w_format ) << profile.format();
|
|
cout << "@ " << profile.fps() << " Hz";
|
|
cout << endl;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::ostringstream ss;
|
|
int p_width = 0, p_height = 0, pp_w = 0, pp_h = 0;
|
|
rs2_format p_format = RS2_FORMAT_ANY;
|
|
std::string p_stream_name;
|
|
bool p_default = false;
|
|
auto print_last_stream = [&]()
|
|
{
|
|
cout << ( p_default ? " + " : " " );
|
|
p_default = false;
|
|
cout << setw( w_stream ) << p_stream_name;
|
|
if( p_width || p_height )
|
|
{
|
|
cout << setw( w_res );
|
|
if( p_width == pp_w && p_height == pp_h )
|
|
cout << " |";
|
|
else
|
|
{
|
|
cout << setw( 4 ) << right << p_width;
|
|
cout << 'x';
|
|
cout << setw( w_res - 5 ) << left << p_height;
|
|
pp_w = p_width;
|
|
pp_h = p_height;
|
|
}
|
|
}
|
|
cout << setw( w_format ) << p_format;
|
|
cout << "@ " << ss.str() << " Hz";
|
|
cout << endl;
|
|
};
|
|
for( auto && profile : profiles )
|
|
{
|
|
std::string stream_name = profile.stream_name();
|
|
int w = 0, h = 0;
|
|
if( auto video = profile.as< video_stream_profile >() )
|
|
{
|
|
w = video.width();
|
|
h = video.height();
|
|
}
|
|
if( stream_name != p_stream_name || profile.format() != p_format || w != p_width || h != p_height )
|
|
{
|
|
if( ! ss.str().empty() )
|
|
{
|
|
print_last_stream();
|
|
ss.str( std::string() );
|
|
}
|
|
p_stream_name = stream_name;
|
|
p_format = profile.format();
|
|
p_width = w;
|
|
p_height = h;
|
|
}
|
|
else
|
|
{
|
|
ss << '/';
|
|
}
|
|
if( profile.is_default() && show_defaults )
|
|
ss << "+", p_default = true;
|
|
ss << profile.fps();
|
|
}
|
|
print_last_stream();
|
|
}
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv) try
|
|
{
|
|
CmdLine cmd("librealsense rs-enumerate-devices tool", ' ', RS2_API_FULL_VERSION_STR);
|
|
|
|
SwitchArg debug_arg( "", "debug", "Turn on LibRS debug logs" );
|
|
SwitchArg short_view_arg("s", "short", "Provide a one-line summary of the devices");
|
|
SwitchArg compact_view_arg("S", "compact", "Provide a short summary of the devices");
|
|
SwitchArg show_options_arg("o", "option", "Show all the supported options per subdevice");
|
|
SwitchArg show_calibration_data_arg( "c", "calib_data", "Show extrinsic and intrinsic of all subdevices" );
|
|
SwitchArg show_defaults("d", "defaults", "Show the default streams configuration");
|
|
SwitchArg only_sw_arg( "", "sw-only", "Show only software devices (playback, DDS, etc. -- but not USB/HID/etc.)" );
|
|
ValueArg<string> format_arg( "", "format", "Choose which 'format-conversion' to use", false, "full", "raw/basic/FULL" );
|
|
SwitchArg verbose_arg( "v", "verbose", "Show extra information" );
|
|
ValueArg<string> show_playback_device_arg("p", "playback_device", "Inspect and enumerate playback device (from file)",
|
|
false, "", "path");
|
|
cmd.add(debug_arg);
|
|
cmd.add(short_view_arg);
|
|
cmd.add(compact_view_arg);
|
|
cmd.add(show_options_arg);
|
|
cmd.add(show_calibration_data_arg);
|
|
cmd.add(only_sw_arg);
|
|
cmd.add( format_arg );
|
|
cmd.add(verbose_arg);
|
|
#ifdef BUILD_WITH_DDS
|
|
ValueArg< int > domain_arg( "", "dds-domain", "Set the DDS domain ID (default to 0)", false, 0, "0-232" );
|
|
cmd.add( domain_arg );
|
|
#endif
|
|
cmd.add(show_defaults);
|
|
cmd.add(show_playback_device_arg);
|
|
|
|
cmd.parse(argc, argv);
|
|
|
|
#ifdef BUILD_EASYLOGGINGPP
|
|
bool debugging = debug_arg.getValue();
|
|
log_to_console(debugging ? RS2_LOG_SEVERITY_DEBUG : RS2_LOG_SEVERITY_ERROR);
|
|
#endif
|
|
|
|
bool short_view = short_view_arg.getValue();
|
|
bool compact_view = compact_view_arg.getValue();
|
|
bool show_options = show_options_arg.getValue();
|
|
bool show_calibration_data = show_calibration_data_arg.getValue();
|
|
bool show_modes = !(compact_view || short_view);
|
|
auto playback_dev_file = show_playback_device_arg.getValue();
|
|
bool verbose = verbose_arg.getValue();
|
|
|
|
if ((short_view || compact_view) && (show_options || show_calibration_data))
|
|
{
|
|
std::stringstream s;
|
|
s << "rs-enumerate-devices ";
|
|
for (int i = 1; i < argc; ++i)
|
|
s << argv[i] << ' ';
|
|
std::cout << "Warning: The command line \"" << s.str().c_str()
|
|
<<"\" has conflicting requests.\n The flags \"-s\" / \"-S\" are compatible with \"-p\" only,"
|
|
<< " other options will be ignored.\nRefer to the tool's documentation for details\n" << std::endl;
|
|
}
|
|
|
|
// Obtain a list of devices currently present on the system
|
|
json settings = json::object();
|
|
#ifdef BUILD_WITH_DDS
|
|
if( domain_arg.isSet() || only_sw_arg.isSet() )
|
|
{
|
|
json dds = json::object();
|
|
if( domain_arg.isSet() )
|
|
dds["domain"] = domain_arg.getValue();
|
|
dds["enabled"]; // null: remove global dds:false or dds/enabled:false, if any
|
|
settings["dds"] = std::move( dds );
|
|
}
|
|
#endif
|
|
settings["format-conversion"] = format_arg.getValue();
|
|
context ctx( settings.dump() );
|
|
|
|
rs2::device d;
|
|
if (!playback_dev_file.empty())
|
|
d = ctx.load_device(playback_dev_file.data());
|
|
|
|
int mask = RS2_PRODUCT_LINE_ANY_INTEL;
|
|
if( only_sw_arg.getValue() )
|
|
mask |= RS2_PRODUCT_LINE_SW_ONLY;
|
|
auto devices_list = ctx.query_devices( mask );
|
|
if( only_sw_arg.getValue() )
|
|
{
|
|
// For SW-only devices, allow some time for DDS devices to connect
|
|
int tries = 3;
|
|
cout << "No device detected. Waiting..." << flush;
|
|
while( ! devices_list.size() && tries-- )
|
|
{
|
|
cout << "." << flush;
|
|
std::this_thread::sleep_for( std::chrono::seconds( 1 ) );
|
|
devices_list = ctx.query_devices( mask );
|
|
}
|
|
cout << endl;
|
|
}
|
|
|
|
// Retrieve the viable devices
|
|
std::vector<rs2::device> devices;
|
|
|
|
for ( auto i = 0u; i < devices_list.size(); i++)
|
|
{
|
|
try
|
|
{
|
|
auto dev = devices_list[i];
|
|
devices.emplace_back(dev);
|
|
}
|
|
catch (const std::exception & e)
|
|
{
|
|
std::cout << "Could not create device - " << e.what() <<" . Check SDK logs for details" << std::endl;
|
|
}
|
|
catch(...)
|
|
{
|
|
std::cout << "Failed to created device. Check SDK logs for details" << std::endl;
|
|
}
|
|
}
|
|
|
|
size_t device_count = devices.size();
|
|
if( !device_count )
|
|
{
|
|
cout << "No device detected. Is it plugged in?\n";
|
|
return EXIT_SUCCESS;
|
|
}
|
|
|
|
if (short_view || compact_view)
|
|
{
|
|
auto dev_info = []( rs2::device dev, rs2_camera_info info )
|
|
{
|
|
if( dev.supports( info ) )
|
|
return dev.get_info( info );
|
|
return "N/A";
|
|
};
|
|
|
|
size_t w_name = 28;
|
|
size_t w_sn = 18;
|
|
for( auto i = 0u; i < devices.size(); ++i )
|
|
{
|
|
auto dev = devices[i];
|
|
|
|
w_name = std::max( strlen( dev_info( dev, RS2_CAMERA_INFO_NAME ) ), w_name );
|
|
w_sn = std::max( strlen( dev_info( dev, RS2_CAMERA_INFO_SERIAL_NUMBER ) ), w_sn );
|
|
}
|
|
w_name += 2;
|
|
w_sn += 2;
|
|
|
|
cout << left << setw(w_name) << "Device Name"
|
|
<< setw(w_sn) << "Serial Number"
|
|
<< "Firmware Version"
|
|
<< endl;
|
|
|
|
for (auto i = 0u; i < devices.size(); ++i)
|
|
{
|
|
auto dev = devices[i];
|
|
|
|
cout << left << setw(w_name) << dev_info(dev,RS2_CAMERA_INFO_NAME)
|
|
<< setw(w_sn) << dev_info(dev,RS2_CAMERA_INFO_SERIAL_NUMBER)
|
|
<< dev_info(dev,RS2_CAMERA_INFO_FIRMWARE_VERSION)
|
|
<< endl;
|
|
}
|
|
|
|
show_options = show_calibration_data = show_modes = false;
|
|
}
|
|
|
|
for (auto i = 0u; i < devices.size(); ++i)
|
|
{
|
|
|
|
auto dev = devices[i];
|
|
|
|
if (!short_view)
|
|
{
|
|
// Show which options are supported by this device
|
|
cout << "Device info: \n";
|
|
if (auto pb_dev = dev.as<playback>())
|
|
cout << "Playback Device (" << pb_dev.file_name() << ")" << std::endl;
|
|
|
|
for (auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j)
|
|
{
|
|
auto param = static_cast<rs2_camera_info>(j);
|
|
if (dev.supports(param))
|
|
cout << " " << left << setw(30) << rs2_camera_info_to_string(rs2_camera_info(param))
|
|
<< ": \t" << dev.get_info(param) << endl;
|
|
}
|
|
|
|
cout << endl;
|
|
}
|
|
|
|
if( show_defaults.getValue() && ! show_modes )
|
|
{
|
|
cout << "Default streams:" << endl;
|
|
for( auto && sensor : dev.query_sensors() )
|
|
{
|
|
for( auto const & sp : sensor.get_stream_profiles() )
|
|
{
|
|
if( sp.is_default() )
|
|
{
|
|
cout << " " << sp.stream_name() << " as " << sp.format() << " at " << sp.fps() << " Hz";
|
|
if( auto vsp = sp.as<video_stream_profile>() )
|
|
{
|
|
cout << "; Resolution: " << vsp.width() << "x" << vsp.height();
|
|
}
|
|
cout << endl;
|
|
}
|
|
}
|
|
}
|
|
cout << endl;
|
|
}
|
|
|
|
if (show_options)
|
|
{
|
|
for (auto&& sensor : dev.query_sensors())
|
|
{
|
|
cout << "Options for " << sensor.get_info(RS2_CAMERA_INFO_NAME) << endl;
|
|
|
|
cout << setw(35) << " Supported options:" << setw(10) << "min" << setw(10)
|
|
<< " max" << setw(6) << " step" << setw(10) << " default" << endl;
|
|
for (auto j = 0; j < RS2_OPTION_COUNT; ++j)
|
|
{
|
|
auto opt = static_cast<rs2_option>(j);
|
|
if (sensor.supports(opt))
|
|
{
|
|
try
|
|
{
|
|
auto range = sensor.get_option_range(opt);
|
|
cout << " " << left << setw(30) << opt << " : "
|
|
<< setw(5) << range.min << "... " << setw(12) << range.max
|
|
<< setw(6) << range.step << setw(10) << range.def << "\n";
|
|
}
|
|
catch (const error & e)
|
|
{
|
|
cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << endl;
|
|
}
|
|
}
|
|
}
|
|
cout << endl;
|
|
}
|
|
}
|
|
|
|
if (show_modes)
|
|
{
|
|
for( auto&& sensor : dev.query_sensors() )
|
|
{
|
|
cout << "Stream Profiles supported by " << sensor.get_info( RS2_CAMERA_INFO_NAME ) << endl;
|
|
|
|
cout << " Supported modes:\n";
|
|
output_modes( sensor.get_stream_profiles(), verbose, show_defaults.getValue() );
|
|
cout << endl;
|
|
|
|
if( auto ds = debug_stream_sensor( sensor ) )
|
|
{
|
|
auto debug_profiles = ds.get_debug_stream_profiles();
|
|
if( !debug_profiles.empty() )
|
|
{
|
|
cout << " Supported debug modes:\n";
|
|
output_modes( debug_profiles, verbose, show_defaults.getValue() );
|
|
cout << endl;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Print Intrinsics
|
|
if (show_calibration_data)
|
|
{
|
|
std::map<stream_and_index, stream_profile> streams;
|
|
std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_intrinsics>>> intrinsics_map;
|
|
std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_motion_device_intrinsic>>> motion_intrinsics_map;
|
|
for (auto&& sensor : dev.query_sensors())
|
|
{
|
|
// Intrinsics
|
|
for (auto&& profile : sensor.get_stream_profiles())
|
|
{
|
|
if (auto video = profile.as<video_stream_profile>())
|
|
{
|
|
if (streams.find(stream_and_index{ profile.stream_type(), profile.stream_index() }) == streams.end())
|
|
{
|
|
streams[stream_and_index{ profile.stream_type(), profile.stream_index() }] = profile;
|
|
}
|
|
|
|
rs2_intrinsics intrinsics{};
|
|
stream_and_resolution stream_res{ profile.stream_type(), profile.stream_index(), video.width(), video.height(), profile.stream_name() };
|
|
if (safe_get_intrinsics(video, intrinsics))
|
|
{
|
|
auto it = std::find_if((intrinsics_map[stream_res]).begin(), (intrinsics_map[stream_res]).end(), [&](const std::pair<std::set<rs2_format>, rs2_intrinsics>& kvp) {
|
|
return intrinsics == kvp.second;
|
|
});
|
|
if (it == (intrinsics_map[stream_res]).end())
|
|
{
|
|
(intrinsics_map[stream_res]).push_back({ {profile.format()}, intrinsics });
|
|
}
|
|
else
|
|
{
|
|
it->first.insert(profile.format()); // If the intrinsics are equals, add the profile format to format set
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (motion_stream_profile motion = profile.as<motion_stream_profile>())
|
|
{
|
|
if (streams.find(stream_and_index{ profile.stream_type(), profile.stream_index() }) == streams.end())
|
|
{
|
|
streams[stream_and_index{ profile.stream_type(), profile.stream_index() }] = profile;
|
|
}
|
|
|
|
rs2_motion_device_intrinsic motion_intrinsics{};
|
|
stream_and_resolution stream_res{ profile.stream_type(), profile.stream_index(), motion.stream_type(), motion.stream_index(), profile.stream_name() };
|
|
if (safe_get_motion_intrinsics(motion, motion_intrinsics))
|
|
{
|
|
auto it = std::find_if((motion_intrinsics_map[stream_res]).begin(), (motion_intrinsics_map[stream_res]).end(),
|
|
[&](const std::pair<std::set<rs2_format>, rs2_motion_device_intrinsic>& kvp)
|
|
{
|
|
return motion_intrinsics == kvp.second;
|
|
});
|
|
if (it == (motion_intrinsics_map[stream_res]).end())
|
|
{
|
|
(motion_intrinsics_map[stream_res]).push_back({ {profile.format()}, motion_intrinsics });
|
|
}
|
|
else
|
|
{
|
|
it->first.insert(profile.format()); // If the intrinsics are equals, add the profile format to format set
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (pose_stream_profile pose = profile.as<pose_stream_profile>())
|
|
{
|
|
if (streams.find(stream_and_index{ profile.stream_type(), profile.stream_index() }) == streams.end())
|
|
{
|
|
streams[stream_and_index{ profile.stream_type(), profile.stream_index() }] = profile;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
cout << " Unhandled profile encountered: " << profile.stream_name() << "/" << profile.format() << std::endl;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (intrinsics_map.size())
|
|
{
|
|
cout << "Intrinsic Parameters:\n" << endl;
|
|
for (auto& kvp : intrinsics_map)
|
|
{
|
|
auto stream_res = kvp.first;
|
|
for (auto& intrinsics : kvp.second)
|
|
{
|
|
auto formats = "{" + get_str_formats(intrinsics.first) + "}";
|
|
cout << " Intrinsic of \"" << stream_res.stream_name << "\" / " << stream_res.width << "x"
|
|
<< stream_res.height << " / " << formats << endl;
|
|
if (intrinsics.second == rs2_intrinsics{})
|
|
{
|
|
cout << "Intrinsic NOT available!\n\n";
|
|
}
|
|
else
|
|
{
|
|
print(intrinsics.second);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (motion_intrinsics_map.size())
|
|
{
|
|
cout << "Motion Intrinsic Parameters:\n" << endl;
|
|
for (auto& kvp : motion_intrinsics_map)
|
|
{
|
|
auto stream_res = kvp.first;
|
|
for (auto& intrinsics : kvp.second)
|
|
{
|
|
auto formats = get_str_formats(intrinsics.first);
|
|
cout << "Motion Intrinsic of \"" << stream_res.stream_name << "\"\t " << formats << endl;
|
|
if (intrinsics.second == rs2_motion_device_intrinsic{})
|
|
{
|
|
cout << "Intrinsic NOT available!\n\n";
|
|
}
|
|
else
|
|
{
|
|
print(intrinsics.second);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (streams.size())
|
|
{
|
|
// Print Extrinsics
|
|
cout << "\nExtrinsic Parameters:" << endl;
|
|
rs2_extrinsics extrinsics{};
|
|
for (auto kvp1 = streams.begin(); kvp1 != streams.end(); ++kvp1)
|
|
{
|
|
for (auto kvp2 = streams.begin(); kvp2 != streams.end(); ++kvp2)
|
|
{
|
|
cout << "Extrinsic from \"" << kvp1->second.stream_name() << "\"\t " <<
|
|
"To" << "\t \"" << kvp2->second.stream_name() << "\" :\n";
|
|
try
|
|
{
|
|
extrinsics = kvp1->second.get_extrinsics_to(kvp2->second);
|
|
print(extrinsics);
|
|
}
|
|
catch (...)
|
|
{
|
|
cout << "N/A\n";
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|
|
catch (const error & e)
|
|
{
|
|
cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n " << e.what() << endl;
|
|
return EXIT_FAILURE;
|
|
}
|
|
catch( const exception & e )
|
|
{
|
|
cerr << e.what() << endl;
|
|
return EXIT_FAILURE;
|
|
}
|
|
catch( ... )
|
|
{
|
|
cerr << "some error" << endl;
|
|
return EXIT_FAILURE;
|
|
}
|