Skip to content

I
imagej-elphel
Commit 2014ae08 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Added more DID sane checks - found DID_INS1 with wrong LLA

parent 598a2074
Hide whitespace changes
Inline Side-by-side
Showing with 289 additions and 112 deletions
src/main/java/com/elphel/imagej/ims/Did_gps_pos.java
View file @ 2014ae08
......@@ -101,6 +101,18 @@ public class Did_gps_pos {
getProperties(prefix, properties);
}
public boolean isDidSane() { // add more!
if ( (lla[0] <-90) || (lla[0] > 90) || // latitude
(lla[1] <-90) || (lla[1] > 90) || // longitude
(lla[2] <-10) || (lla[1] > 20000)) { // altitude
System.out.println("isDidSane(): bad lla=["+
lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeekMs="+timeOfWeekMs);
return false;
}
return true;
}
public Did_gps_pos interpolate(double frac, Did_gps_pos next_did) {
Did_gps_pos new_did = new Did_gps_pos();
double time_ms_this = timeOfWeekMs + Did_strobe_in_time.WEEK_MS * week;
......
src/main/java/com/elphel/imagej/ims/Did_ins.java
View file @ 2014ae08
......@@ -91,6 +91,13 @@ public abstract class Did_ins <T extends Did_ins <T>>{
(insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask));
return false;
}
if ( (lla[0] <-90) || (lla[0] > 90) || // latitude
(lla[1] <-90) || (lla[1] > 90) || // longitude
(lla[2] <-10) || (lla[1] > 20000)) { // altitude
System.out.println("isDidSane(): bad lla=["+
lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeek="+timeOfWeek);
return false;
}
return true;
}
......
src/main/java/com/elphel/imagej/ims/Did_pimu.java
View file @ 2014ae08
......@@ -7,6 +7,14 @@ import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
public class Did_pimu {
public static final int IMU_STATUS_IMU_OK_MASK = 0x003F0000; // lower bits may have error
public static final int IMU_STATUS_SATURATION_MASK = 0x0000003F;
public static final int IMU_STATUS_ZEROS = 0xffc0f800;
public static final double IMU_MAX_THETA = 10.0; //rad/s
public static final double IMU_MAX_VEL = 20.0; //m/s/s
public static final double IMU_MIN_PER = 0.008; // s
public static final double IMU_MAX_PER = 1.0; // s
/** Time since boot up in seconds. Convert to GPS time of week by adding gps.towOffset */
public double time;
/** Integral period in seconds for delta theta and delta velocity. This is configured using DID_FLASH_CONFIG.startupNavDtMs. */
......@@ -44,6 +52,39 @@ public class Did_pimu {
return new_did;
}
public boolean isDidSane() {
if (((status ^ IMU_STATUS_IMU_OK_MASK) & IMU_STATUS_IMU_OK_MASK) != 0) {
System.out.println("Did_pimu.isDidSane(): not OK status = "+String.format("0x%08x", status));
return false;
}
if ((status & IMU_STATUS_SATURATION_MASK) != 0) {
System.out.println("Did_pimu.isDidSane(): saturated status = "+String.format("0x%08x", status));
return false;
}
if ((status & IMU_STATUS_ZEROS) != 0) {
System.out.println("Did_pimu.isDidSane(): garbage status = "+String.format("0x%08x", status));
return false;
}
if (dt < IMU_MIN_PER) {
System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s < "+IMU_MIN_PER+" s");
return false;
}
if (dt > IMU_MAX_PER) {
System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s > "+IMU_MAX_PER+" s");
return false;
}
for (int i = 0; i < 3; i++) {
if (Math.abs(theta[i]) > IMU_MAX_THETA) {
System.out.println("Did_pimu.isDidSane(): insane theta ["+i+"]= "+theta[i]+" rad/s");
return false;
}
if (Math.abs(vel[i]) > IMU_MAX_VEL) {
System.out.println("Did_pimu.isDidSane(): insane vel ["+i+"]= "+vel[i]+" m/s/s");
return false;
}
}
return true;
}
public int pack(ByteBuffer bb) {
int p_start = bb.position();
......
src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java
View file @ 2014ae08
......@@ -134,7 +134,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
// Timestamp is frame start for images
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
break search_opposite; // return nrec;
// is DID sane?
if (isDidSane(nrec, did)) {
break search_opposite; // return nrec;
} else {
System.out.println("getLastBeforeIndex()-1: Skipping insane DID="+did);
}
}
}
}
......@@ -147,7 +152,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
if (after?(ts_master_indx > ts_master) :(ts_master_indx <= ts_master)) {
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
return nrec;
// is DID sane?
if (isDidSane(nrec, did)) {
return nrec;
} else {
System.out.println("getLastBeforeIndex()-2: Skipping insane DID="+did);
}
}
}
}
......@@ -441,7 +451,40 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
return true;
}
public boolean isDidSane(int nrec, int type) {
// int did_ins_type = type_ins_2 ? Imx5.DID_INS_2 : Imx5.DID_INS_1;
byte[] payload;
try {
payload = getDidPayload(
null, // next_fileinfo, // may be null if payload does not extend beyond single record
nrec);
} catch (IOException e) {
// TODO Auto-generated catch block
System.out.println("isDidSane(): failed to read record "+nrec);
e.printStackTrace();
return false;
}
if (payload == null) {
System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
return false;
}
ByteBuffer bb_payload = ByteBuffer.wrap(payload);
bb_payload.order(ByteOrder.LITTLE_ENDIAN);
switch (type) {
case Imx5.DID_INS_1:
return (new Did_ins_1(bb_payload)).isDidSane();
case Imx5.DID_INS_2:
return (new Did_ins_2(bb_payload)).isDidSane();
case Imx5.DID_GPS1_POS:
case Imx5.DID_GPS2_POS:
case Imx5.DID_GPS1_UBX_POS:
return (new Did_gps_pos(bb_payload)).isDidSane();
case Imx5.DID_PIMU:
return (new Did_pimu(bb_payload)).isDidSane();
}
return true;
}
/**
* Absolute timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
......
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ 2014ae08
......@@ -17,104 +17,104 @@ public class Imx5 {
// defines adapted from Inertial Sense data_sets.h
public static int DID_NULL = 0 ; /** NULL (INVALID) */
public static int DID_DEV_INFO = 1 ; /** (dev_info_t) Device information */
public static int DID_SYS_FAULT = 2 ; /** (system_fault_t) System fault information */
public static int DID_PIMU = 3 ; /** (pimu_t) Preintegrated IMU (a.k.a. Coning and Sculling integral) in body/IMU frame. Updated at IMU rate. Also know as delta theta delta velocity, or preintegrated IMU (PIMU). For clarification, the name "Preintegrated IMU" or "PIMU" throughout our User Manual. This data is integrated from the IMU data at the IMU update rate (startupImuDtMs, default 1ms). The integration period (dt) and output data rate are the same as the NAV rate (startupNavDtMs) and cannot be output at any other rate. If a faster output data rate is desired, DID_IMU_RAW can be used instead. PIMU data acts as a form of compression, adding the benefit of higher integration rates for slower output data rates, preserving the IMU data without adding filter delay and addresses antialiasing. It is most effective for systems that have higher dynamics and lower communications data rates. The minimum data period is DID_FLASH_CONFIG.startupImuDtMs or 4, whichever is larger (250Hz max). The PIMU value can be converted to IMU by dividing PIMU by dt (i.e. IMU = PIMU / dt) */
public static int DID_INS_1 = 4 ; /** (ins_1_t) INS output: euler rotation w/ respect to NED, NED position from reference LLA. */
public static int DID_INS_2 = 5 ; /** (ins_2_t) INS output: quaternion rotation w/ respect to NED, ellipsoid altitude */
public static int DID_GPS1_UBX_POS = 6 ; /** (gps_pos_t) GPS 1 position data from ublox receiver. */
public static int DID_SYS_CMD = 7 ; /** (system_command_t) System commands. Both the command and invCommand fields must be set at the same time for a command to take effect. */
public static int DID_ASCII_BCAST_PERIOD = 8 ; /** (ascii_msgs_t) Broadcast period for ASCII messages */
public static int DID_RMC = 9 ; /** (rmc_t) Realtime Message Controller (RMC). The data sets available through RMC are driven by the availability of the data. The RMC provides updates from various data sources (i.e. sensors) as soon as possible with minimal latency. Several of the data sources (sensors) output data at different data rates that do not all correspond. The RMC is provided so that broadcast of sensor data is done as soon as it becomes available. All RMC messages can be enabled using the standard Get Data packet format. */
public static int DID_SYS_PARAMS = 10; /** (sys_params_t) System parameters / info */
public static int DID_SYS_SENSORS = 11; /** (sys_sensors_t) System sensor information */
public static int DID_FLASH_CONFIG = 12; /** (nvm_flash_cfg_t) Flash memory configuration */
public static int DID_GPS1_POS = 13; /** (gps_pos_t) GPS 1 position data. This comes from DID_GPS1_UBX_POS or DID_GPS1_RTK_POS, depending on whichever is more accurate. */
public static int DID_GPS2_POS = 14; /** (gps_pos_t) GPS 2 position data */
public static int DID_GPS1_SAT = 15; /** (gps_sat_t) GPS 1 GNSS and sat identifiers, carrier to noise ratio (signal strength), elevation and azimuth angles, pseudo range residual. */
public static int DID_GPS2_SAT = 16; /** (gps_sat_t) GPS 2 GNSS and sat identifiers, carrier to noise ratio (signal strength), elevation and azimuth angles, pseudo range residual. */
public static int DID_GPS1_VERSION = 17; /** (gps_version_t) GPS 1 version info */
public static int DID_GPS2_VERSION = 18; /** (gps_version_t) GPS 2 version info */
public static int DID_MAG_CAL = 19; /** (mag_cal_t) Magnetometer calibration */
public static int DID_INTERNAL_DIAGNOSTIC = 20; /** INTERNAL USE ONLY (internal_diagnostic_t) Internal diagnostic info */
public static int DID_GPS1_RTK_POS_REL = 21; /** (gps_rtk_rel_t) RTK precision position base to rover relative info. */
public static int DID_GPS1_RTK_POS_MISC = 22; /** (gps_rtk_misc_t) RTK precision position related data. */
public static int DID_FEATURE_BITS = 23; /** INTERNAL USE ONLY (feature_bits_t) */
public static int DID_SENSORS_UCAL = 24; /** INTERNAL USE ONLY (sensors_w_temp_t) Uncalibrated IMU output. */
public static int DID_SENSORS_TCAL = 25; /** INTERNAL USE ONLY (sensors_w_temp_t) Temperature compensated IMU output. */
public static int DID_SENSORS_TC_BIAS = 26; /** INTERNAL USE ONLY (sensors_t) */
public static int DID_IO = 27; /** (io_t) I/O */
public static int DID_SENSORS_ADC = 28; /** INTERNAL USE ONLY (sys_sensors_adc_t) */
public static int DID_SCOMP = 29; /** INTERNAL USE ONLY (sensor_compensation_t) */
public static int DID_GPS1_VEL = 30; /** (gps_vel_t) GPS 1 velocity data */
public static int DID_GPS2_VEL = 31; /** (gps_vel_t) GPS 2 velocity data */
public static int DID_HDW_PARAMS = 32; /** INTERNAL USE ONLY (hdw_params_t) */
public static int DID_NVR_MANAGE_USERPAGE = 33; /** INTERNAL USE ONLY (nvr_manage_t) */
public static int DID_NVR_USERPAGE_SN = 34; /** INTERNAL USE ONLY (nvm_group_sn_t) */
public static int DID_NVR_USERPAGE_G0 = 35; /** INTERNAL USE ONLY (nvm_group_0_t) */
public static int DID_NVR_USERPAGE_G1 = 36; /** INTERNAL USE ONLY (nvm_group_1_t) */
public static int DID_DEBUG_STRING = 37; /** INTERNAL USE ONLY (debug_string_t) */
public static int DID_RTOS_INFO = 38; /** (rtos_info_t) RTOS information. */
public static int DID_DEBUG_ARRAY = 39; /** INTERNAL USE ONLY (debug_array_t) */
public static int DID_SENSORS_MCAL = 40; /** INTERNAL USE ONLY (sensors_w_temp_t) Temperature compensated and motion calibrated IMU output. */
public static int DID_GPS1_TIMEPULSE = 41; /** INTERNAL USE ONLY (gps_timepulse_t) */
public static int DID_CAL_SC = 42; /** INTERNAL USE ONLY (sensor_cal_t) */
public static int DID_CAL_TEMP_COMP = 43; /** INTERNAL USE ONLY (sensor_tcal_group_t) */
public static int DID_CAL_MOTION = 44; /** INTERNAL USE ONLY (sensor_mcal_group_t) */
public static int DID_UNUSED_45 = 45; /** used to be internal DID_SYS_SENSORS_SIGMA */
public static int DID_SENSORS_ADC_SIGMA = 46; /** INTERNAL USE ONLY (sys_sensors_adc_t) */
public static int DID_REFERENCE_MAGNETOMETER = 47; /** (magnetometer_t) Reference or truth magnetometer used for manufacturing calibration and testing */
public static int DID_INL2_STATES = 48; /** (inl2_states_t) */
public static int DID_INL2_COVARIANCE_LD = 49; /** (INL2_COVARIANCE_LD_ARRAY_SIZE) */
public static int DID_INL2_STATUS = 50; /** (inl2_status_t) */
public static int DID_INL2_MISC = 51; /** (inl2_misc_t) */
public static int DID_MAGNETOMETER = 52; /** (magnetometer_t) Magnetometer sensor output */
public static int DID_BAROMETER = 53; /** (barometer_t) Barometric pressure sensor data */
public static int DID_GPS1_RTK_POS = 54; /** (gps_pos_t) GPS RTK position data */
public static int DID_ROS_COVARIANCE_POSE_TWIST = 55; /** (ros_covariance_pose_twist_t) INL2 EKF covariances matrix lower diagonals */
public static int DID_COMMUNICATIONS_LOOPBACK = 56; /** INTERNAL USE ONLY - Unit test for communications manager */
public static int DID_IMU3_UNCAL = 57; /** INTERNAL USE ONLY (imu3_t) Uncalibrated triple IMU data. We recommend use of DID_IMU or DID_PIMU as they are calibrated and oversampled and contain less noise. Minimum data period is DID_FLASH_CONFIG.startupImuDtMs or 4, whichever is larger (250Hz max). */
public static int DID_IMU = 58; /** (imu_t) Inertial measurement unit data down-sampled from IMU rate (DID_FLASH_CONFIG.startupImuDtMs (1KHz)) to navigation update rate (DID_FLASH_CONFIG.startupNavDtMs) as an anti-aliasing filter to reduce noise and preserve accuracy. Minimum data period is DID_FLASH_CONFIG.startupNavDtMs (1KHz max). */
public static int DID_INL2_MAG_OBS_INFO = 59; /** (inl2_mag_obs_info_t) INL2 magnetometer calibration information. */
public static int DID_GPS_BASE_RAW = 60; /** (gps_raw_t) GPS raw data for base station (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
public static int DID_GPS_RTK_OPT = 61; /** (gps_rtk_opt_t) RTK options - requires little endian CPU. */
public static int DID_REFERENCE_PIMU = 62; /** (pimu_t) Reference or truth IMU used for manufacturing calibration and testing */
public static int DID_MANUFACTURING_INFO = 63; /** INTERNAL USE ONLY (manufacturing_info_t) Manufacturing info */
public static int DID_BIT = 64; /** (bit_t) System built-in self-test */
public static int DID_INS_3 = 65; /** (ins_3_t) Inertial navigation data with quaternion NED to body rotation and ECEF position. */
public static int DID_INS_4 = 66; /** (ins_4_t) INS output: quaternion rotation w/ respect to ECEF, ECEF position. */
public static int DID_INL2_NED_SIGMA = 67; /** (inl2_ned_sigma_t) Standard deviation of INL2 EKF estimates in the NED frame. */
public static int DID_STROBE_IN_TIME = 68; /** (strobe_in_time_t) Timestamp for input strobe. */
public static int DID_GPS1_RAW = 69; /** (gps_raw_t) GPS raw data for rover (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
public static int DID_GPS2_RAW = 70; /** (gps_raw_t) GPS raw data for rover (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
public static int DID_WHEEL_ENCODER = 71; /** (wheel_encoder_t) Wheel encoder data to be fused with GPS-INS measurements, set DID_GROUND_VEHICLE for configuration before sending this message */
public static int DID_DIAGNOSTIC_MESSAGE = 72; /** (diag_msg_t) Diagnostic message */
public static int DID_SURVEY_IN = 73; /** (survey_in_t) Survey in, used to determine position for RTK base station. Base correction output cannot run during a survey and will be automatically disabled if a survey is started. */
public static int DID_CAL_SC_INFO = 74; /** INTERNAL USE ONLY (sensor_cal_info_t) */
public static int DID_PORT_MONITOR = 75; /** (port_monitor_t) Data rate and status monitoring for each communications port. */
public static int DID_RTK_STATE = 76; /** INTERNAL USE ONLY (rtk_state_t) */
public static int DID_RTK_PHASE_RESIDUAL = 77; /** INTERNAL USE ONLY (rtk_residual_t) */
public static int DID_RTK_CODE_RESIDUAL = 78; /** INTERNAL USE ONLY (rtk_residual_t) */
public static int DID_RTK_DEBUG = 79; /** INTERNAL USE ONLY (rtk_debug_t) */
public static int DID_EVB_STATUS = 80; /** (evb_status_t) EVB monitor and log control interface. */
public static int DID_EVB_FLASH_CFG = 81; /** (evb_flash_cfg_t) EVB configuration. */
public static int DID_EVB_DEBUG_ARRAY = 82; /** INTERNAL USE ONLY (debug_array_t) */
public static int DID_EVB_RTOS_INFO = 83; /** (evb_rtos_info_t) EVB-2 RTOS information. */
// public static int DID_UNUSED_84 = 84; /** Unused */
public static int DID_IMU_MAG = 85; /** (imu_mag_t) DID_IMU + DID_MAGNETOMETER. Only one of DID_IMU_MAG or DID_PIMU_MAG should be streamed simultaneously. */
public static int DID_PIMU_MAG = 86; /** (pimu_mag_t) DID_PIMU + DID_MAGNETOMETER. Only one of DID_IMU_MAG or DID_PIMU_MAG should be streamed simultaneously. */
public static int DID_GROUND_VEHICLE = 87; /** (ground_vehicle_t) Static configuration for wheel transform measurements. */
public static int DID_POSITION_MEASUREMENT = 88; /** (pos_measurement_t) External position estimate */
public static int DID_RTK_DEBUG_2 = 89; /** INTERNAL USE ONLY (rtk_debug_2_t) */
public static int DID_CAN_CONFIG = 90; /** (can_config_t) Addresses for CAN messages*/
public static int DID_GPS2_RTK_CMP_REL = 91; /** (gps_rtk_rel_t) Dual GNSS RTK compassing / moving base to rover (GPS 1 to GPS 2) relative info. */
public static int DID_GPS2_RTK_CMP_MISC = 92; /** (gps_rtk_misc_t) RTK Dual GNSS RTK compassing related data. */
public static int DID_EVB_DEV_INFO = 93; /** (dev_info_t) EVB device information */
public static int DID_INFIELD_CAL = 94; /** (infield_cal_t) Measure and correct IMU calibration error. Estimate INS rotation to align INS with vehicle. */
public static int DID_REFERENCE_IMU = 95; /** (imu_t) Raw reference or truth IMU used for manufacturing calibration and testing. Input from testbed. */
public static int DID_IMU3_RAW = 96; /** (imu3_t) Triple IMU data calibrated from DID_IMU3_UNCAL. We recommend use of DID_IMU or DID_PIMU as they are oversampled and contain less noise. */
public static int DID_IMU_RAW = 97; /** (imu_t) IMU data averaged from DID_IMU3_RAW. Use this IMU data for output data rates faster than DID_FLASH_CONFIG.startupNavDtMs. Otherwise we recommend use of DID_IMU or DID_PIMU as they are oversampled and contain less noise. */
public static final int DID_NULL = 0 ; /** NULL (INVALID) */
public static final int DID_DEV_INFO = 1 ; /** (dev_info_t) Device information */
public static final int DID_SYS_FAULT = 2 ; /** (system_fault_t) System fault information */
public static final int DID_PIMU = 3 ; /** (pimu_t) Preintegrated IMU (a.k.a. Coning and Sculling integral) in body/IMU frame. Updated at IMU rate. Also know as delta theta delta velocity, or preintegrated IMU (PIMU). For clarification, the name "Preintegrated IMU" or "PIMU" throughout our User Manual. This data is integrated from the IMU data at the IMU update rate (startupImuDtMs, default 1ms). The integration period (dt) and output data rate are the same as the NAV rate (startupNavDtMs) and cannot be output at any other rate. If a faster output data rate is desired, DID_IMU_RAW can be used instead. PIMU data acts as a form of compression, adding the benefit of higher integration rates for slower output data rates, preserving the IMU data without adding filter delay and addresses antialiasing. It is most effective for systems that have higher dynamics and lower communications data rates. The minimum data period is DID_FLASH_CONFIG.startupImuDtMs or 4, whichever is larger (250Hz max). The PIMU value can be converted to IMU by dividing PIMU by dt (i.e. IMU = PIMU / dt) */
public static final int DID_INS_1 = 4 ; /** (ins_1_t) INS output: euler rotation w/ respect to NED, NED position from reference LLA. */
public static final int DID_INS_2 = 5 ; /** (ins_2_t) INS output: quaternion rotation w/ respect to NED, ellipsoid altitude */
public static final int DID_GPS1_UBX_POS = 6 ; /** (gps_pos_t) GPS 1 position data from ublox receiver. */
public static final int DID_SYS_CMD = 7 ; /** (system_command_t) System commands. Both the command and invCommand fields must be set at the same time for a command to take effect. */
public static final int DID_ASCII_BCAST_PERIOD = 8 ; /** (ascii_msgs_t) Broadcast period for ASCII messages */
public static final int DID_RMC = 9 ; /** (rmc_t) Realtime Message Controller (RMC). The data sets available through RMC are driven by the availability of the data. The RMC provides updates from various data sources (i.e. sensors) as soon as possible with minimal latency. Several of the data sources (sensors) output data at different data rates that do not all correspond. The RMC is provided so that broadcast of sensor data is done as soon as it becomes available. All RMC messages can be enabled using the standard Get Data packet format. */
public static final int DID_SYS_PARAMS = 10; /** (sys_params_t) System parameters / info */
public static final int DID_SYS_SENSORS = 11; /** (sys_sensors_t) System sensor information */
public static final int DID_FLASH_CONFIG = 12; /** (nvm_flash_cfg_t) Flash memory configuration */
public static final int DID_GPS1_POS = 13; /** (gps_pos_t) GPS 1 position data. This comes from DID_GPS1_UBX_POS or DID_GPS1_RTK_POS, depending on whichever is more accurate. */
public static final int DID_GPS2_POS = 14; /** (gps_pos_t) GPS 2 position data */
public static final int DID_GPS1_SAT = 15; /** (gps_sat_t) GPS 1 GNSS and sat identifiers, carrier to noise ratio (signal strength), elevation and azimuth angles, pseudo range residual. */
public static final int DID_GPS2_SAT = 16; /** (gps_sat_t) GPS 2 GNSS and sat identifiers, carrier to noise ratio (signal strength), elevation and azimuth angles, pseudo range residual. */
public static final int DID_GPS1_VERSION = 17; /** (gps_version_t) GPS 1 version info */
public static final int DID_GPS2_VERSION = 18; /** (gps_version_t) GPS 2 version info */
public static final int DID_MAG_CAL = 19; /** (mag_cal_t) Magnetometer calibration */
public static final int DID_INTERNAL_DIAGNOSTIC = 20; /** INTERNAL USE ONLY (internal_diagnostic_t) Internal diagnostic info */
public static final int DID_GPS1_RTK_POS_REL = 21; /** (gps_rtk_rel_t) RTK precision position base to rover relative info. */
public static final int DID_GPS1_RTK_POS_MISC = 22; /** (gps_rtk_misc_t) RTK precision position related data. */
public static final int DID_FEATURE_BITS = 23; /** INTERNAL USE ONLY (feature_bits_t) */
public static final int DID_SENSORS_UCAL = 24; /** INTERNAL USE ONLY (sensors_w_temp_t) Uncalibrated IMU output. */
public static final int DID_SENSORS_TCAL = 25; /** INTERNAL USE ONLY (sensors_w_temp_t) Temperature compensated IMU output. */
public static final int DID_SENSORS_TC_BIAS = 26; /** INTERNAL USE ONLY (sensors_t) */
public static final int DID_IO = 27; /** (io_t) I/O */
public static final int DID_SENSORS_ADC = 28; /** INTERNAL USE ONLY (sys_sensors_adc_t) */
public static final int DID_SCOMP = 29; /** INTERNAL USE ONLY (sensor_compensation_t) */
public static final int DID_GPS1_VEL = 30; /** (gps_vel_t) GPS 1 velocity data */
public static final int DID_GPS2_VEL = 31; /** (gps_vel_t) GPS 2 velocity data */
public static final int DID_HDW_PARAMS = 32; /** INTERNAL USE ONLY (hdw_params_t) */
public static final int DID_NVR_MANAGE_USERPAGE = 33; /** INTERNAL USE ONLY (nvr_manage_t) */
public static final int DID_NVR_USERPAGE_SN = 34; /** INTERNAL USE ONLY (nvm_group_sn_t) */
public static final int DID_NVR_USERPAGE_G0 = 35; /** INTERNAL USE ONLY (nvm_group_0_t) */
public static final int DID_NVR_USERPAGE_G1 = 36; /** INTERNAL USE ONLY (nvm_group_1_t) */
public static final int DID_DEBUG_STRING = 37; /** INTERNAL USE ONLY (debug_string_t) */
public static final int DID_RTOS_INFO = 38; /** (rtos_info_t) RTOS information. */
public static final int DID_DEBUG_ARRAY = 39; /** INTERNAL USE ONLY (debug_array_t) */
public static final int DID_SENSORS_MCAL = 40; /** INTERNAL USE ONLY (sensors_w_temp_t) Temperature compensated and motion calibrated IMU output. */
public static final int DID_GPS1_TIMEPULSE = 41; /** INTERNAL USE ONLY (gps_timepulse_t) */
public static final int DID_CAL_SC = 42; /** INTERNAL USE ONLY (sensor_cal_t) */
public static final int DID_CAL_TEMP_COMP = 43; /** INTERNAL USE ONLY (sensor_tcal_group_t) */
public static final int DID_CAL_MOTION = 44; /** INTERNAL USE ONLY (sensor_mcal_group_t) */
public static final int DID_UNUSED_45 = 45; /** used to be internal DID_SYS_SENSORS_SIGMA */
public static final int DID_SENSORS_ADC_SIGMA = 46; /** INTERNAL USE ONLY (sys_sensors_adc_t) */
public static final int DID_REFERENCE_MAGNETOMETER = 47; /** (magnetometer_t) Reference or truth magnetometer used for manufacturing calibration and testing */
public static final int DID_INL2_STATES = 48; /** (inl2_states_t) */
public static final int DID_INL2_COVARIANCE_LD = 49; /** (INL2_COVARIANCE_LD_ARRAY_SIZE) */
public static final int DID_INL2_STATUS = 50; /** (inl2_status_t) */
public static final int DID_INL2_MISC = 51; /** (inl2_misc_t) */
public static final int DID_MAGNETOMETER = 52; /** (magnetometer_t) Magnetometer sensor output */
public static final int DID_BAROMETER = 53; /** (barometer_t) Barometric pressure sensor data */
public static final int DID_GPS1_RTK_POS = 54; /** (gps_pos_t) GPS RTK position data */
public static final int DID_ROS_COVARIANCE_POSE_TWIST = 55; /** (ros_covariance_pose_twist_t) INL2 EKF covariances matrix lower diagonals */
public static final int DID_COMMUNICATIONS_LOOPBACK = 56; /** INTERNAL USE ONLY - Unit test for communications manager */
public static final int DID_IMU3_UNCAL = 57; /** INTERNAL USE ONLY (imu3_t) Uncalibrated triple IMU data. We recommend use of DID_IMU or DID_PIMU as they are calibrated and oversampled and contain less noise. Minimum data period is DID_FLASH_CONFIG.startupImuDtMs or 4, whichever is larger (250Hz max). */
public static final int DID_IMU = 58; /** (imu_t) Inertial measurement unit data down-sampled from IMU rate (DID_FLASH_CONFIG.startupImuDtMs (1KHz)) to navigation update rate (DID_FLASH_CONFIG.startupNavDtMs) as an anti-aliasing filter to reduce noise and preserve accuracy. Minimum data period is DID_FLASH_CONFIG.startupNavDtMs (1KHz max). */
public static final int DID_INL2_MAG_OBS_INFO = 59; /** (inl2_mag_obs_info_t) INL2 magnetometer calibration information. */
public static final int DID_GPS_BASE_RAW = 60; /** (gps_raw_t) GPS raw data for base station (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
public static final int DID_GPS_RTK_OPT = 61; /** (gps_rtk_opt_t) RTK options - requires little endian CPU. */
public static final int DID_REFERENCE_PIMU = 62; /** (pimu_t) Reference or truth IMU used for manufacturing calibration and testing */
public static final int DID_MANUFACTURING_INFO = 63; /** INTERNAL USE ONLY (manufacturing_info_t) Manufacturing info */
public static final int DID_BIT = 64; /** (bit_t) System built-in self-test */
public static final int DID_INS_3 = 65; /** (ins_3_t) Inertial navigation data with quaternion NED to body rotation and ECEF position. */
public static final int DID_INS_4 = 66; /** (ins_4_t) INS output: quaternion rotation w/ respect to ECEF, ECEF position. */
public static final int DID_INL2_NED_SIGMA = 67; /** (inl2_ned_sigma_t) Standard deviation of INL2 EKF estimates in the NED frame. */
public static final int DID_STROBE_IN_TIME = 68; /** (strobe_in_time_t) Timestamp for input strobe. */
public static final int DID_GPS1_RAW = 69; /** (gps_raw_t) GPS raw data for rover (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
public static final int DID_GPS2_RAW = 70; /** (gps_raw_t) GPS raw data for rover (observation, ephemeris, etc.) - requires little endian CPU. The contents of data can vary for this message and are determined by dataType field. RTK positioning or RTK compassing must be enabled to stream this message. */
public static final int DID_WHEEL_ENCODER = 71; /** (wheel_encoder_t) Wheel encoder data to be fused with GPS-INS measurements, set DID_GROUND_VEHICLE for configuration before sending this message */
public static final int DID_DIAGNOSTIC_MESSAGE = 72; /** (diag_msg_t) Diagnostic message */
public static final int DID_SURVEY_IN = 73; /** (survey_in_t) Survey in, used to determine position for RTK base station. Base correction output cannot run during a survey and will be automatically disabled if a survey is started. */
public static final int DID_CAL_SC_INFO = 74; /** INTERNAL USE ONLY (sensor_cal_info_t) */
public static final int DID_PORT_MONITOR = 75; /** (port_monitor_t) Data rate and status monitoring for each communications port. */
public static final int DID_RTK_STATE = 76; /** INTERNAL USE ONLY (rtk_state_t) */
public static final int DID_RTK_PHASE_RESIDUAL = 77; /** INTERNAL USE ONLY (rtk_residual_t) */
public static final int DID_RTK_CODE_RESIDUAL = 78; /** INTERNAL USE ONLY (rtk_residual_t) */
public static final int DID_RTK_DEBUG = 79; /** INTERNAL USE ONLY (rtk_debug_t) */
public static final int DID_EVB_STATUS = 80; /** (evb_status_t) EVB monitor and log control interface. */
public static final int DID_EVB_FLASH_CFG = 81; /** (evb_flash_cfg_t) EVB configuration. */
public static final int DID_EVB_DEBUG_ARRAY = 82; /** INTERNAL USE ONLY (debug_array_t) */
public static final int DID_EVB_RTOS_INFO = 83; /** (evb_rtos_info_t) EVB-2 RTOS information. */
// public static final int DID_UNUSED_84 = 84; /** Unused */
public static final int DID_IMU_MAG = 85; /** (imu_mag_t) DID_IMU + DID_MAGNETOMETER. Only one of DID_IMU_MAG or DID_PIMU_MAG should be streamed simultaneously. */
public static final int DID_PIMU_MAG = 86; /** (pimu_mag_t) DID_PIMU + DID_MAGNETOMETER. Only one of DID_IMU_MAG or DID_PIMU_MAG should be streamed simultaneously. */
public static final int DID_GROUND_VEHICLE = 87; /** (ground_vehicle_t) Static configuration for wheel transform measurements. */
public static final int DID_POSITION_MEASUREMENT = 88; /** (pos_measurement_t) External position estimate */
public static final int DID_RTK_DEBUG_2 = 89; /** INTERNAL USE ONLY (rtk_debug_2_t) */
public static final int DID_CAN_CONFIG = 90; /** (can_config_t) Addresses for CAN messages*/
public static final int DID_GPS2_RTK_CMP_REL = 91; /** (gps_rtk_rel_t) Dual GNSS RTK compassing / moving base to rover (GPS 1 to GPS 2) relative info. */
public static final int DID_GPS2_RTK_CMP_MISC = 92; /** (gps_rtk_misc_t) RTK Dual GNSS RTK compassing related data. */
public static final int DID_EVB_DEV_INFO = 93; /** (dev_info_t) EVB device information */
public static final int DID_INFIELD_CAL = 94; /** (infield_cal_t) Measure and correct IMU calibration error. Estimate INS rotation to align INS with vehicle. */
public static final int DID_REFERENCE_IMU = 95; /** (imu_t) Raw reference or truth IMU used for manufacturing calibration and testing. Input from testbed. */
public static final int DID_IMU3_RAW = 96; /** (imu3_t) Triple IMU data calibrated from DID_IMU3_UNCAL. We recommend use of DID_IMU or DID_PIMU as they are oversampled and contain less noise. */
public static final int DID_IMU_RAW = 97; /** (imu_t) IMU data averaged from DID_IMU3_RAW. Use this IMU data for output data rates faster than DID_FLASH_CONFIG.startupNavDtMs. Otherwise we recommend use of DID_IMU or DID_PIMU as they are oversampled and contain less noise. */
// Adding a new data id?
// 1] Add it above and increment the previous number, include the matching data structure type in the comments
......@@ -125,12 +125,12 @@ public class Imx5 {
// 6] Test!
/** Count of data ids (including null data id 0) - MUST BE MULTPLE OF 4 and larger than last DID number! */
public static int DID_COUNT = 120; // Used in SDK
public static int DID_COUNT_UINS = 100; // Used in uINS
public static final int DID_COUNT = 120; // Used in SDK
public static final int DID_COUNT_UINS = 100; // Used in uINS
public static double EARTH_RADIUS = 6356752.0; // meters
/** Maximum number of data ids */
public static int DID_MAX_COUNT = 256;
public static final int DID_MAX_COUNT = 256;
public static byte[] test_imx() {
Did_ins_1 did_ins_1 = new Did_ins_1();
......@@ -232,7 +232,21 @@ public class Imx5 {
return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
}
/**
* Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion
* @param ims_atr original ATR of the IMS mount
* @param corr_q correction quaternion
* @return corrected ATR of teh IMU mount
*/
public static double [] adjustMountAtrByQuat(
double [] ims_atr,
double [] corr_q) {
Rotation rot_ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_atr[0], ims_atr[1], ims_atr[2]);
Rotation rot_corr = new Rotation(corr_q[0],corr_q[1],corr_q[2],corr_q[3],false);
Rotation rot = rot_ims_atr.applyTo(rot_corr);
return rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
}
public static double [] quatToCamAtr(double[]quat) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 2014ae08
......@@ -5352,12 +5352,14 @@ public class Interscene {
double [] quat_lma_enu_xyz = (quatCorr != null) ?
Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
cam_xyz_enu;
// NED without ims_mount_atr correction
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
// NED with ims_mount_atr correction
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
sb.append("\t"+cam_xyz_ned[0]+ "\t"+cam_xyz_ned[1]+ "\t"+cam_xyz_ned[2]); //
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); //
sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); //
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr
sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 2014ae08
......@@ -98,6 +98,13 @@ public class IntersceneMatchParameters {
ims_mount_atr[1] * Math.PI/180,
ims_mount_atr[2] * Math.PI/180};
}
public void setImsMountATR( double [] atr) {
ims_mount_atr[0] = atr[0] * 180/Math.PI;
ims_mount_atr[1] = atr[1] * 180/Math.PI;
ims_mount_atr[2] = atr[2] * 180/Math.PI;
}
public boolean center_reference = false;
public boolean overlap_sequences = false; // overlap sequences: scan down from the previous center
public boolean reset_photometric = true; // reset photometric calibration - once for each new series
......@@ -106,7 +113,7 @@ public class IntersceneMatchParameters {
public boolean run_ly = false; // will return just after LY adjustments, skipping all output generation
public int min_num_orient = 2; // make from parameters, should be >= 1
public int min_num_interscene = 2; // make from parameters, should be >= 1
public boolean adjust_imu_orient = false;// adjust IMU misalignment to the camera
public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
public boolean use_quat_corr = true; // Use orientation correction everywhere if available
......@@ -631,6 +638,8 @@ public class IntersceneMatchParameters {
"Minimal number of fitting scenes cycles, should be >=1. First cycle includes spiral search for the first scene");
gd.addNumericField("Minimal number of interscene accumulations", this.min_num_interscene, 0,3,"",
"Minimal required number of re-calculations of the interscene-accumulated DSI.");
gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient,
"Adjust IMU misalignment to the camera.");
gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
"Calculate camera orientation correction from predicted by IMS .");
gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
......@@ -1415,6 +1424,7 @@ public class IntersceneMatchParameters {
this.run_ly = gd.getNextBoolean();
this.min_num_orient = (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
this.adjust_imu_orient = gd.getNextBoolean();
this.calc_quat_corr = gd.getNextBoolean();
this.apply_quat_corr = gd.getNextBoolean();
this.use_quat_corr = gd.getNextBoolean();
......@@ -1845,6 +1855,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"run_ly", this.run_ly + ""); // boolean
properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int
properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
properties.setProperty(prefix+"adjust_imu_orient", this.adjust_imu_orient+""); // boolean
properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
......@@ -2240,6 +2251,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"run_ly")!=null) this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));
if (properties.getProperty(prefix+"min_num_orient")!=null) this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
if (properties.getProperty(prefix+"adjust_imu_orient")!=null) this.adjust_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_imu_orient"));
if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
......@@ -2662,6 +2674,7 @@ public class IntersceneMatchParameters {
imp.run_ly = this.run_ly;
imp.min_num_orient = this.min_num_orient;
imp.min_num_interscene = this.min_num_interscene;
imp.adjust_imu_orient = this.adjust_imu_orient;
imp.calc_quat_corr = this.calc_quat_corr;
imp.apply_quat_corr = this.apply_quat_corr;
imp.use_quat_corr = this.use_quat_corr;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 2014ae08
......@@ -4658,6 +4658,7 @@ public class OpticalFlow {
save_mapped_mono_color[1] || gen_avi_mono_color[1] || show_mono_color[1]} ;
// skip completely if no color or mono, tiff or video
boolean adjust_imu_orient = clt_parameters.imp.adjust_imu_orient; // calculate camera orientation correction from predicted by IMS
boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
// apply_quat_corr used inside getGroundIns() - used when generating output
// boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
......@@ -5420,7 +5421,7 @@ public class OpticalFlow {
}
}
// later move to the right place
if ((quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
if (adjust_imu_orient) { // (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
......@@ -5438,7 +5439,7 @@ public class OpticalFlow {
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 4; // 3; // 2; // 1;
int debug_lev = 3;
int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......@@ -5456,7 +5457,33 @@ public class OpticalFlow {
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
debug_lev); // int debugLevel
if (rotated_xyzatr != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
if (debugLevel > -3) {
System.out.println("Applying correction ot the IMS mount orientation:");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
}
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) {
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println ("*** Need to save the main configuration file ***");
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
}
if (run_ly) {
if (debugLevel > -3) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 2014ae08
......@@ -1210,13 +1210,31 @@ public class QuaternionLma {
String pfx,
double [] data) {
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
if (mode == 0 ) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z"));
for (int nscene = 0; nscene < N; nscene++) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2]));
}
System.out.println();
} else if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
pfx+"A",pfx+"T",pfx+"R"));
for (int nscene = 0; nscene < N; nscene++) {
if ( (data[7*nscene + 3]*data[7*nscene + 3] +
data[7*nscene + 4]*data[7*nscene + 4] +
data[7*nscene + 5]*data[7*nscene + 5]+
data[7*nscene + 6]*data[7*nscene + 6]) < 0.001) {
System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
continue;
}
Rotation rot = new Rotation(data[7*nscene + 3],data[7*nscene + 4],data[7*nscene + 5],data[7*nscene + 6], false);
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
System.out.println(String.format("%3d"+
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment