Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

STANAG 4607 packets + mission segment + dwell segment #1716

Open
wants to merge 10 commits into
base: master
Choose a base branch
from
Open

STANAG 4607 packets + mission segment + dwell segment #1716

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
4d933ae
Add STANAG 4607 packet/segment headers, mission segment
Dec 15, 2022
f969ba0
Add dwell segment, file reorg
Dec 22, 2022
ada4b8d
Add stanag 4607 to python
Jan 24, 2023
1e7402e
Be more specific with int types
Feb 14, 2023
6c2f1f1
Read [signed]binary angles via SA and BA definitions in standard
Feb 15, 2023
bd8e371
Merge branch 'master' into dev/stanag_4607_mission
hdefazio Feb 15, 2023
666bbc5
bug fix: height was not printing in python
Feb 16, 2023
4feda91
Merge branch 'dev/stanag_4607_mission' of github.com:hdefazio/kwiver …
Feb 16, 2023
6d1bf86
Remove extra imports
Feb 16, 2023
b1f4d0f
Fix bug in docker
Feb 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
56 changes: 33 additions & 23 deletions arrows/stanag/segments/stanag_4607_dwell_segment.cxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,10 @@ ::read( ptr_t& ptr ) const
{
stanag_4607_scale_factor result;

result.lat_scale = klv::klv_read_int< int >( ptr, (size_t)4 );
result.long_scale = klv::klv_read_int< int >( ptr, (size_t)4 );
result.lat_scale = float_to_signed_binary_angle( float(
klv::klv_read_int< int32_t >( ptr, (size_t)4 ) ), 32 );
result.long_scale = float_to_binary_angle( float(
klv::klv_read_int< uint32_t >( ptr, (size_t)4 ) ), 32 );

return result;

Expand Down Expand Up @@ -132,9 +134,12 @@ ::read( ptr_t& ptr ) const
{
stanag_4607_orientation result;

result.heading = klv::klv_read_flint< uint16_t >({0, 359.9945}, ptr, 2);
result.pitch = klv::klv_read_flint< int16_t >({-90, 90}, ptr, 2);
result.roll = klv::klv_read_flint< int16_t >({-90, 90}, ptr, 2);
result.heading = float_to_binary_angle( float(
klv::klv_read_int< uint16_t >(ptr, 2) ), 16);
result.pitch = float_to_signed_binary_angle( float(
klv::klv_read_int< int16_t >(ptr, 2) ), 16);
result.roll = float_to_signed_binary_angle( float(
klv::klv_read_int< int16_t >(ptr, 2) ), 16);

return result;
}
Expand Down Expand Up @@ -403,9 +408,10 @@ ::read( ptr_t& ptr,
if( existence_mask.count(
STANAG_4607_DWELL_EXIST_MASK_BIT_TARGET_REPORT_LOCATION_HI_RES_LAT) )
{
result.hi_res_lat = klv::klv_read_flint< int32_t >({-90, 90}, ptr, 4);
result.hi_res_long = klv::klv_read_flint< uint32_t >({0, 359.999999916},
ptr, 4);
result.hi_res_lat = float_to_signed_binary_angle( float(
klv::klv_read_int< int32_t >(ptr, (size_t)4) ), 32);
result.hi_res_long = float_to_binary_angle( float(
klv::klv_read_int< uint32_t >(ptr, (size_t)4) ), 32);
}
// Fields D32.4-D32.5 are conditional and always sent together
// Condition: Sent if D32.2 and D32.3 are not sent
Expand Down Expand Up @@ -729,11 +735,12 @@ ::read( ptr_t& ptr ) const

result.dwell_time = klv::klv_read_int< uint32_t >( ptr, (size_t)4 );

result.sensor_position.latitude = klv::klv_read_flint< int32_t >({-90, 90},
ptr, (size_t)4);
result.sensor_position.longitude =klv::klv_read_flint< uint32_t >(
{0, 359.999999916}, ptr, (size_t)4);
result.sensor_position.altitude = klv::klv_read_int< int32_t >( ptr, (size_t)4 );
result.sensor_position.latitude = float_to_signed_binary_angle( float(
klv::klv_read_int< int32_t >(ptr, (size_t)4) ), 32 );
result.sensor_position.longitude = float_to_binary_angle( float(
klv::klv_read_int< uint32_t >(ptr, (size_t)4)), 32 );
result.sensor_position.altitude = klv::klv_read_int< int32_t >( ptr,
(size_t)4 );

// Fields D10-11 are conditional and always sent together
// Condition: Sent if D32.4 and D32.5 are sent
Expand All @@ -756,8 +763,8 @@ ::read( ptr_t& ptr ) const
if( result.existence_mask.count(
STANAG_4607_DWELL_EXIST_MASK_BIT_SENSOR_TRACK) )
{
result.sensor_track = klv::klv_read_flint< uint16_t >({0, 359.9945},
ptr, 2);
result.sensor_track = float_to_binary_angle( float(
klv::klv_read_int< uint16_t >( ptr, (size_t)2) ), 16 );
result.sensor_speed = klv::klv_read_int< uint32_t >( ptr, (size_t)4 );
result.sensor_vertical_vel = klv::klv_read_int< int8_t >( ptr, (size_t)1 );
}
Expand All @@ -766,8 +773,10 @@ ::read( ptr_t& ptr ) const
if( result.existence_mask.count(
STANAG_4607_DWELL_EXIST_MASK_BIT_SENSOR_TRACK_UNCERT) )
{
result.sensor_track_uncert = klv::klv_read_int< uint8_t >( ptr, (size_t)1 );
result.sensor_speed_uncert = klv::klv_read_int< uint16_t >( ptr, (size_t)2 );
result.sensor_track_uncert = klv::klv_read_int< uint8_t >( ptr,
(size_t)1 );
result.sensor_speed_uncert = klv::klv_read_int< uint16_t >( ptr,
(size_t)2 );
result.sensor_vertical_vel_uncert = klv::klv_read_int< uint16_t >( ptr,
(size_t)2 );
}
Expand All @@ -781,10 +790,10 @@ ::read( ptr_t& ptr ) const
}

// Fields D24-27 are mandatory
result.dwell_area.center_lat = klv::klv_read_flint< int32_t >({-90, 90},
ptr, 4);
result.dwell_area.center_long = klv::klv_read_flint< uint32_t >(
{0, 359.999979}, ptr, 4);
result.dwell_area.center_lat = float_to_signed_binary_angle( float(
klv::klv_read_int< int32_t >(ptr, (size_t)4) ), 32);
result.dwell_area.center_long = float_to_binary_angle( float(
klv::klv_read_int< uint32_t >(ptr, (size_t)4) ), 32);

kwiver::vital::interval<double> interval = {0, 255.9928};
size_t length = 2;
Expand All @@ -794,8 +803,9 @@ ::read( ptr_t& ptr ) const

result.dwell_area.range_half_ext = v * scale + interval.lower();

result.dwell_area.dwell_angle_half_ext = klv::klv_read_flint< uint16_t >(
{0, 359.9945}, ptr, 2);
result.dwell_area.dwell_angle_half_ext = float_to_binary_angle( float(
klv::klv_read_int< uint16_t >(ptr,
(size_t)2) ), 16);

// Fields D28-D30 are optional
// If at least one is sent, any omitted fields are set to 0
Expand Down
36 changes: 13 additions & 23 deletions arrows/stanag/segments/stanag_4607_dwell_segment.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,11 @@
#include <initializer_list>
#include <map>
#include <memory>
#include <optional>
#include <ostream>
#include <set>
#include <string>
#include <vector>
#include <optional>

namespace kwiver {

Expand All @@ -32,13 +32,12 @@ namespace stanag {

namespace kv = kwiver::vital;


// ----------------------------------------------------------------------------
/// Position of the sensor at the temporal center of the dwell
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_sensor_position
{
double latitude;
double longitude;
float latitude;
float longitude;
int32_t altitude;
};

Expand All @@ -56,8 +55,8 @@ DECLARE_STANAG_CMP( stanag_4607_sensor_position )
/// Report.
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_scale_factor
{
int lat_scale;
int long_scale;
float lat_scale;
float long_scale;
};

// ----------------------------------------------------------------------------
Expand All @@ -78,7 +77,6 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_scale_factor_format
read( ptr_t& ptr ) const;
};


// ----------------------------------------------------------------------------
/// Estimate of the standard deviation in the estimated sensor location at
/// the time of the dwell. Expressed in centimeters.
Expand Down Expand Up @@ -107,7 +105,6 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_sensor_pos_uncert_format
read( ptr_t& ptr ) const;
};


// ----------------------------------------------------------------------------
/// The toll angle of the platform at the time of the dwell.
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_orientation
Expand Down Expand Up @@ -135,23 +132,21 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_orientation_format
read( ptr_t& ptr ) const;
};


// ----------------------------------------------------------------------------
/// The position of the center of the dwell area
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_dwell_area
{
double center_lat;
double center_long;
double range_half_ext;
double dwell_angle_half_ext;
float center_lat;
float center_long;
float range_half_ext;
float dwell_angle_half_ext;
};

// ----------------------------------------------------------------------------
KWIVER_ALGO_STANAG_EXPORT
std::ostream&
operator<<( std::ostream& os, stanag_4607_dwell_area const& value );


// ----------------------------------------------------------------------------
DECLARE_STANAG_CMP( stanag_4607_dwell_area )

Expand Down Expand Up @@ -231,7 +226,6 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_target_measure_uncert_format
read( ptr_t& ptr ) const;
};


// ----------------------------------------------------------------------------
/// Information used to generate the MTI Target.
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_truth_tag
Expand All @@ -258,7 +252,6 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_truth_tag_format
read( ptr_t& ptr ) const;
};


// ----------------------------------------------------------------------------
/// Each bit of the Existence Mask indicates whether or not the corresponding
/// field of the Dwell Segment is present in the data stream.
Expand Down Expand Up @@ -324,8 +317,8 @@ operator<<( std::ostream& os,
/// The position of the reported detection.
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_target_location
{
std::optional< double > hi_res_lat;
std::optional< double > hi_res_long;
std::optional< float > hi_res_lat;
std::optional< float > hi_res_long;
std::optional< int16_t > delta_lat;
std::optional< int16_t > delta_long;
std::optional< int16_t > geodetic_height;
Expand All @@ -350,7 +343,6 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_target_location_format
std::set< stanag_4607_dwell_existence_mask_bit > existence_mask ) const;
};


// ----------------------------------------------------------------------------
/// Each target observed within the dwell.
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_target_report
Expand Down Expand Up @@ -389,10 +381,9 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_target_report_format

stanag_4607_target_report
read( ptr_t& ptr,
std::set< stanag_4607_dwell_existence_mask_bit > existence_mask ) const;
std::set< stanag_4607_dwell_existence_mask_bit > existence_mask ) const;
};


// ----------------------------------------------------------------------------
/// A report on a grouping of zero or more target reports.
struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_dwell_segment
Expand All @@ -406,7 +397,7 @@ struct KWIVER_ALGO_STANAG_EXPORT stanag_4607_dwell_segment
stanag_4607_sensor_position sensor_position;
std::optional< stanag_4607_scale_factor > scale_factor;
std::optional< stanag_4607_sensor_pos_uncert > sensor_pos_uncert;
std::optional< uint16_t > sensor_track;
std::optional< float > sensor_track;
std::optional< uint32_t > sensor_speed;
std::optional< int8_t > sensor_vertical_vel;
std::optional< uint8_t > sensor_track_uncert;
Expand Down Expand Up @@ -434,7 +425,6 @@ class KWIVER_ALGO_STANAG_EXPORT stanag_4607_dwell_segment_format
public:
stanag_4607_dwell_segment_format();


size_t size;

stanag_4607_dwell_segment
Expand Down
13 changes: 13 additions & 0 deletions arrows/stanag/stanag_util.cxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,19 @@ trim_whitespace( std::string input )
return input.substr( str_begin, ( str_end - str_begin ) + 1 );
}

// ----------------------------------------------------------------------------
float
float_to_binary_angle( float value, int n )
{
return value * 1.40625 * ( 1 / pow( 2, n - 8 ) );
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

One thing you might like to consider here (and for the SA case) is that n will only ever be one of two values (16 or 32). So you can turn this into a straight scaling factor, either with if (n == ...) construct or by defining two functions for each case (e.g. float_to_binary_angle_16()). Then you don't have the pay the cost of the pow() call each time.

}

// ----------------------------------------------------------------------------
float
float_to_signed_binary_angle( float value, int n )
{
return value * 1.40625 * ( 1 / pow( 2, n - 7 ) );
}

} // namespace stanag

Expand Down
12 changes: 12 additions & 0 deletions arrows/stanag/stanag_util.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,18 @@ KWIVER_ALGO_STANAG_EXPORT
std::string
trim_whitespace( std::string input );

// ----------------------------------------------------------------------------
/// Convert a floating point number to a binary angle
KWIVER_ALGO_STANAG_EXPORT
float
float_to_binary_angle( float value, int n );

// ----------------------------------------------------------------------------
/// Convert a floating point number to a signed binary angle
KWIVER_ALGO_STANAG_EXPORT
float
float_to_signed_binary_angle( float value, int n );

} // namespace stanag

} // namespace arrows
Expand Down
12 changes: 6 additions & 6 deletions python/kwiver/arrows/stanag/segments/stanag_4607_dwell_segment.cxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ stanag_4607_dwell_segment( py::module& m )
{
py::class_< kas::stanag_4607_sensor_position >( m,
"stanag_4607_sensor_position" )
.def( py::init( []( double latitude, double longitude, int32_t altitude){
.def( py::init( []( float latitude, float longitude, int32_t altitude){
return kas::stanag_4607_sensor_position{ latitude,
longitude,
altitude };
Expand All @@ -29,7 +29,7 @@ stanag_4607_dwell_segment( py::module& m )
} );

py::class_< kas::stanag_4607_scale_factor >( m, "stanag_4607_scale_factor" )
.def( py::init( []( int lat_scale, int long_scale ){
.def( py::init( []( float lat_scale, float long_scale ){
return kas::stanag_4607_scale_factor{ lat_scale,
long_scale };
} ) )
Expand Down Expand Up @@ -101,8 +101,8 @@ stanag_4607_dwell_segment( py::module& m )
} );

py::class_< kas::stanag_4607_dwell_area >( m, "stanag_4607_dwell_area" )
.def( py::init( []( double center_lat, double center_long,
double range_half_ext, double dwell_angle_half_ext ){
.def( py::init( []( float center_lat, float center_long,
float range_half_ext, float dwell_angle_half_ext ){
return kas::stanag_4607_dwell_area{ center_lat,
center_long,
range_half_ext,
Expand Down Expand Up @@ -364,7 +364,7 @@ stanag_4607_dwell_segment( py::module& m )

py::class_< kas::stanag_4607_target_location >( m,
"stanag_4607_target_location" )
.def( py::init( []( double hi_res_lat, double hi_res_long, int16_t delta_lat,
.def( py::init( []( float hi_res_lat, float hi_res_long, int16_t delta_lat,
int16_t delta_long, int16_t geodetic_height ){
return kas::stanag_4607_target_location{ hi_res_lat,
hi_res_long,
Expand Down Expand Up @@ -503,7 +503,7 @@ stanag_4607_dwell_segment( py::module& m )
scale_factor,
std::optional< kas::stanag_4607_sensor_pos_uncert >
sensor_pos_uncert,
std::optional< uint16_t > sensor_track,
std::optional< float > sensor_track,
std::optional< uint32_t > sensor_speed,
std::optional< int8_t > sensor_vertical_vel,
std::optional< uint8_t > sensor_track_uncert,
Expand Down