From d61befc0103d9344cac6a129a3712eb25ae3b015 Mon Sep 17 00:00:00 2001 From: aangerma Date: Tue, 29 Sep 2020 13:34:19 +0300 Subject: [PATCH 1/3] Added support for snapshot of imu and pose. --- common/model-views.cpp | 111 ++++++++++++++++++++++++++++++++++++++++- common/model-views.h | 4 ++ 2 files changed, 114 insertions(+), 1 deletion(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 884e39cd68..dd13be5d77 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -393,6 +393,61 @@ namespace rs2 return ret; } + bool motion_data_to_csv( const std::string & filename, rs2::frame frame ) + { + bool ret = false; + auto motion = frame.as< motion_frame >(); + if( frame ) + { + auto axes = motion.get_motion_data(); + std::ofstream csv( filename ); + + auto profile = frame.get_profile(); + csv << "Frame Info: " << std::endl << "Type," << profile.stream_name() << std::endl; + csv << "Format," << rs2_format_to_string( profile.format() ) << std::endl; + csv << "Frame Number," << frame.get_frame_number() << std::endl; + csv << "Timestamp (ms)," << std::fixed << std::setprecision( 2 ) + << frame.get_timestamp() << std::endl; + csv << std::setprecision( 7 ) << "x," << axes.x << std::endl; + csv << std::setprecision( 7 ) << "y," << axes.y << std::endl; + csv << std::setprecision( 7 ) << "z," << axes.z << std::endl; + + ret = true; + } + + return ret; + } + + bool pose_data_to_csv( const std::string & filename, rs2::frame frame ) + { + bool ret = false; + auto pose = frame.as< pose_frame >(); + if( frame ) + { + auto pose_data = pose.get_pose_data(); + std::ofstream csv( filename ); + + auto profile = frame.get_profile(); + csv << "Frame Info: " << std::endl << "Type," << profile.stream_name() << std::endl; + csv << "Format," << rs2_format_to_string( profile.format() ) << std::endl; + csv << "Frame Number," << frame.get_frame_number() << std::endl; + csv << "Timestamp (ms)," << std::fixed << std::setprecision( 2 ) + << frame.get_timestamp() << std::endl; + csv << std::setprecision( 7 ) << "acceleration," << pose_data.acceleration << std::endl; + csv << std::setprecision( 7 ) << "angular_acceleration," << pose_data.angular_acceleration << std::endl; + csv << std::setprecision( 7 ) << "angular_velocity," << pose_data.angular_velocity << std::endl; + csv << std::setprecision( 7 ) << "mapper_confidence," << pose_data.mapper_confidence << std::endl; + csv << std::setprecision( 7 ) << "rotation," << pose_data.rotation << std::endl; + csv << std::setprecision( 7 ) << "tracker_confidence," << pose_data.tracker_confidence << std::endl; + csv << std::setprecision( 7 ) << "translation," << pose_data.translation << std::endl; + csv << std::setprecision( 7 ) << "velocity," << pose_data.velocity << std::endl; + + ret = true; + } + + return ret; + } + std::vector get_string_pointers(const std::vector& vec) { std::vector res; @@ -3092,7 +3147,8 @@ namespace rs2 ss << "PNG snapshot was saved to " << filename_png << std::endl; } - auto original_frame = texture->get_last_frame(false).as(); + auto last_frame = texture->get_last_frame( false ); + auto original_frame = last_frame.as< video_frame >(); // For Depth-originated streams also provide a copy of the raw data accompanied by sensor-specific metadata if (original_frame && val_in_range(original_frame.get_profile().stream_type(), { RS2_STREAM_DEPTH , RS2_STREAM_INFRARED })) @@ -3125,6 +3181,59 @@ namespace rs2 } } + auto motion = last_frame.as< motion_frame >(); + if( motion + && val_in_range( motion.get_profile().stream_type(), + { RS2_STREAM_GYRO, RS2_STREAM_ACCEL } ) ) + { + stream_desc = rs2_stream_to_string( motion.get_profile().stream_type() ); + + // And the frame's attributes + auto filename = filename_base + "_" + stream_desc + ".csv"; + + try + { + if( motion_data_to_csv( filename, motion ) ) + ss << "The frame attributes are saved into\n" << filename; + else + viewer.not_model->add_notification( + { to_string() << "Failed to save frame metadata file " << filename, + RS2_LOG_SEVERITY_INFO, + RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR } ); + } + catch( std::exception & e ) + { + viewer.not_model->add_notification( { to_string() << e.what(), + RS2_LOG_SEVERITY_INFO, + RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR } ); + } + } + + auto pose = last_frame.as< pose_frame >(); + if (pose && pose.get_profile().stream_type() == RS2_STREAM_POSE) + { + stream_desc = rs2_stream_to_string( pose.get_profile().stream_type() ); + + // And the frame's attributes + auto filename = filename_base + "_" + stream_desc + ".csv"; + + try + { + if( pose_data_to_csv( filename, pose ) ) + ss << "The frame attributes are saved into\n" << filename; + else + viewer.not_model->add_notification( + { to_string() << "Failed to save frame metadata file " << filename, + RS2_LOG_SEVERITY_INFO, + RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR } ); + } + catch( std::exception & e ) + { + viewer.not_model->add_notification( { to_string() << e.what(), + RS2_LOG_SEVERITY_INFO, + RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR } ); + } + } if (ss.str().size()) viewer.not_model->add_notification(notification_data{ ss.str().c_str(), RS2_LOG_SEVERITY_INFO, RS2_NOTIFICATION_CATEGORY_HARDWARE_EVENT }); diff --git a/common/model-views.h b/common/model-views.h index 0c11314914..d8921eab87 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -84,6 +84,10 @@ namespace rs2 bool frame_metadata_to_csv(const std::string& filename, rs2::frame frame); + bool motion_data_to_csv( const std::string & filename, rs2::frame frame ); + + bool pose_data_to_csv( const std::string & filename, rs2::frame frame ); + void open_issue(std::string body); class device_model; From 4b6549ab605387f362827572b051b348f0847131 Mon Sep 17 00:00:00 2001 From: aangerma Date: Tue, 29 Sep 2020 15:56:39 +0300 Subject: [PATCH 2/3] CR fixes --- common/model-views.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index dd13be5d77..98a56acfd8 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -396,8 +396,7 @@ namespace rs2 bool motion_data_to_csv( const std::string & filename, rs2::frame frame ) { bool ret = false; - auto motion = frame.as< motion_frame >(); - if( frame ) + if( auto motion = frame.as< motion_frame >() ) { auto axes = motion.get_motion_data(); std::ofstream csv( filename ); @@ -421,8 +420,7 @@ namespace rs2 bool pose_data_to_csv( const std::string & filename, rs2::frame frame ) { bool ret = false; - auto pose = frame.as< pose_frame >(); - if( frame ) + if( auto pose = frame.as< pose_frame >() ) { auto pose_data = pose.get_pose_data(); std::ofstream csv( filename ); From 5a07462ad3a517e852178d0e123575b822d856a9 Mon Sep 17 00:00:00 2001 From: aangerma Date: Tue, 29 Sep 2020 18:19:09 +0300 Subject: [PATCH 3/3] More fixes --- common/model-views.cpp | 46 ++++++++++++++++++++++++++---------------- common/model-views.h | 2 +- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 98a56acfd8..18d2f7a8d2 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -398,6 +398,11 @@ namespace rs2 bool ret = false; if( auto motion = frame.as< motion_frame >() ) { + std::string units; + if( motion.get_profile().stream_type() == RS2_STREAM_GYRO ) + units = "( deg/sec )"; + else + units = "( m/sec^2 )"; auto axes = motion.get_motion_data(); std::ofstream csv( filename ); @@ -407,9 +412,7 @@ namespace rs2 csv << "Frame Number," << frame.get_frame_number() << std::endl; csv << "Timestamp (ms)," << std::fixed << std::setprecision( 2 ) << frame.get_timestamp() << std::endl; - csv << std::setprecision( 7 ) << "x," << axes.x << std::endl; - csv << std::setprecision( 7 ) << "y," << axes.y << std::endl; - csv << std::setprecision( 7 ) << "z," << axes.z << std::endl; + csv << std::setprecision( 7 ) << "Axes" << units << ", " << axes << std::endl; ret = true; } @@ -431,14 +434,25 @@ namespace rs2 csv << "Frame Number," << frame.get_frame_number() << std::endl; csv << "Timestamp (ms)," << std::fixed << std::setprecision( 2 ) << frame.get_timestamp() << std::endl; - csv << std::setprecision( 7 ) << "acceleration," << pose_data.acceleration << std::endl; - csv << std::setprecision( 7 ) << "angular_acceleration," << pose_data.angular_acceleration << std::endl; - csv << std::setprecision( 7 ) << "angular_velocity," << pose_data.angular_velocity << std::endl; - csv << std::setprecision( 7 ) << "mapper_confidence," << pose_data.mapper_confidence << std::endl; - csv << std::setprecision( 7 ) << "rotation," << pose_data.rotation << std::endl; - csv << std::setprecision( 7 ) << "tracker_confidence," << pose_data.tracker_confidence << std::endl; - csv << std::setprecision( 7 ) << "translation," << pose_data.translation << std::endl; - csv << std::setprecision( 7 ) << "velocity," << pose_data.velocity << std::endl; + csv << std::setprecision( 7 ) << "Acceleration( meters/sec^2 ), " + << pose_data.acceleration << std::endl; + csv << std::setprecision( 7 ) << "Angular_acceleration( radians/sec^2 ), " + << pose_data.angular_acceleration << std::endl; + csv << std::setprecision( 7 ) << "Angular_velocity( radians/sec ), " + << pose_data.angular_velocity << std::endl; + csv << std::setprecision( 7 ) + << "Mapper_confidence( 0x0 - Failed 0x1 - Low 0x2 - Medium 0x3 - High ), " + << pose_data.mapper_confidence << std::endl; + csv << std::setprecision( 7 ) + << "Rotation( quaternion rotation (relative to initial position) ), " + << pose_data.rotation << std::endl; + csv << std::setprecision( 7 ) + << "Tracker_confidence( 0x0 - Failed 0x1 - Low 0x2 - Medium 0x3 - High ), " + << pose_data.tracker_confidence << std::endl; + csv << std::setprecision( 7 ) << "Translation( meters ), " << pose_data.translation + << std::endl; + csv << std::setprecision( 7 ) << "Velocity( meters/sec ), " << pose_data.velocity + << std::endl; ret = true; } @@ -3180,9 +3194,7 @@ namespace rs2 } auto motion = last_frame.as< motion_frame >(); - if( motion - && val_in_range( motion.get_profile().stream_type(), - { RS2_STREAM_GYRO, RS2_STREAM_ACCEL } ) ) + if( motion ) { stream_desc = rs2_stream_to_string( motion.get_profile().stream_type() ); @@ -3195,7 +3207,7 @@ namespace rs2 ss << "The frame attributes are saved into\n" << filename; else viewer.not_model->add_notification( - { to_string() << "Failed to save frame metadata file " << filename, + { to_string() << "Failed to save frame file " << filename, RS2_LOG_SEVERITY_INFO, RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR } ); } @@ -3208,7 +3220,7 @@ namespace rs2 } auto pose = last_frame.as< pose_frame >(); - if (pose && pose.get_profile().stream_type() == RS2_STREAM_POSE) + if( pose ) { stream_desc = rs2_stream_to_string( pose.get_profile().stream_type() ); @@ -3221,7 +3233,7 @@ namespace rs2 ss << "The frame attributes are saved into\n" << filename; else viewer.not_model->add_notification( - { to_string() << "Failed to save frame metadata file " << filename, + { to_string() << "Failed to save frame file " << filename, RS2_LOG_SEVERITY_INFO, RS2_NOTIFICATION_CATEGORY_UNKNOWN_ERROR } ); } diff --git a/common/model-views.h b/common/model-views.h index d8921eab87..aa20a75050 100644 --- a/common/model-views.h +++ b/common/model-views.h @@ -82,7 +82,7 @@ namespace rs2 void prepare_config_file(); - bool frame_metadata_to_csv(const std::string& filename, rs2::frame frame); + bool frame_metadata_to_csv( const std::string & filename, rs2::frame frame ); bool motion_data_to_csv( const std::string & filename, rs2::frame frame );