diff --git a/python/PiFinder/imu/__init__.py b/python/PiFinder/imu/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/PiFinder/imu/imu_align/README.md b/python/PiFinder/imu/imu_align/README.md new file mode 100644 index 000000000..9e6600214 --- /dev/null +++ b/python/PiFinder/imu/imu_align/README.md @@ -0,0 +1,221 @@ +# Camera-IMU alignment (extrinsic calibration) + +To track the pointing using IMU dead-reckoning, we need to know the relative +orientation or alignment between the camera and IMU. The development code here +estimates the alignment. + +The alignment error will introduce a "jump" when the IMU dead-reckoning hands +off to the camera solve which is (probably) approximately proportional to the +product of the camera-IMU alignment error and the angle moved under +dead-reckoning (in radians). + +See the header comments in `imu_extrinsic_calibration.py` for explanation of +the algorithm. + +## Previous studies + +In a previous study, we used recorded telemetry data to estimate the camera-IMU +alignment. The extrinsic calibration estimated an adjustment over the nominal +orientation by 1.6 degrees with an uncertainty of ±0.5 degrees. This was based +on around 1 minute of data which gave 9 samples (after outlier removal). + +With 4 minutes of data, we might be able to get it down to something around +±0.1 degrees but this might be unrealistic because it needs continual movement +and (based on simulations) the main source of error doesn't look like +nicely-behaved random noise but something else. The difference between the +moment of camera exposure and the IMU measurement could be just one issue. + +When compared to using the improved alignment with the nominal alignment, the +improvement isn't that big. It cuts the angular jump by around a half, which is +what we'd expect given the uncertainty. + +Simulations with realistic noise gave much better results. This suggests that +the accuracy of the real results may be limited by one or more of the following +following potential root causes: + +1. The alignment algorithm needs to be fed with pairs of start/end samples +with paired camera solves and IMU measurements. Outliers could introduce errors +so better selection criteria may be needed to filter out outliers. +2. The telemetry recording used the BNO055 IMU in fusion mode. This is known to +be noisy so better filtering and outlier rejections may be needed. +3. The BNO055 is an older IMU and it may be that its poorer accuracy propagates +to alignment inaccuracies. It is possible that a more modern IMU could give better +alignment results. +4. The camera and IMU samples are assumed to be from the same time instance. +Relative delays could introduce errors. Filtering of the IMU could also +introduce delays. + +## What still needs to be done + +The study showed that the camera-IMU alsignment could be estimated to ±0.5 +degrees. This is good enough to replace the nominal alignments that need to be +set in configurations. + +A rough alignment feature could be built based on the algorithm in this +directory and the sample code below. + +To improve the alignment accuracy to reduce the "jumps", the potential root +causes listed above may need to be investigated. + +## Sample code from the Jupyter notebooks + +The following is a sample code from the Jupyter notebooks that was used to +analyse the data from telemetry. It could form the basis of an implementation +in PiFinder. + + +```python +from dataclasses import dataclass +from enum import Enum +import quaternion +import numpy as np +from pathlib import Path +import json + +from astro_coords import RaDecRoll +import quaternion_transforms as qt + + +@dataclass +class ImuData: + quat: quaternion.quaternion | None = None + gyro: list | None = None + accel: list | None = None + + +@dataclass +class SolveData: + camera_ra_dec_roll: RaDecRoll | None = None + timestamp_exposure_end: float | None = None # seconds, from time.time() + imu_quat: quaternion.quaternion | None = None # Quaternion at exposure end + + +class MeasurementType(Enum): + CAMERA = 1 + IMU = 2 + + +@dataclass +class Sample: + timestamp: float | None = None # seconds, from time.time() + measurement_type: MeasurementType | None = None + data: SolveData | ImuData | None = None + + def set(self, timestamp: float, measurement_type: MeasurementType, data): + self.timestamp = timestamp + self.measurement_type = measurement_type + self.data = data + + def get(self): + return self.timestamp, self.measurement_type, self.data + + +def read_samples_from_telemetry(path: Path, n_max_samples: int | None = None) -> list[Sample]: + """ + Reads samples from a telemetry file and returns a list of Sample objects. + Each line in the telemetry file is expected to be a JSON object with the following format: + { + "t": timestamp (float, seconds from time.time()), + "e": event type (string, either "imu" or "solve"), + "q": [w, x, y, z] (quaternion for IMU measurements), + "ra": right ascension (float, degrees), + "dec": declination (float, degrees), + "roll": roll angle (float, degrees) + } + """ + samples = [] + counter = 0 + with open(path, 'r') as f: + for line in f: + d = json.loads(line) + #print(d) # For debugging (print the raw data from the telemetry file) + if d["e"] == "imu": + q = quaternion.quaternion(*d["q"]) + imu_data = ImuData(quat=q, gyro=d["gyro"], accel=d['accel']) + samples.append(Sample(timestamp=d["t"], measurement_type=MeasurementType.IMU, data=imu_data)) + elif d["e"] == "solve": + ra_dec_roll = RaDecRoll(ra=d["cam_ra"], dec=d["cam_dec"], roll=d["cam_roll"], deg=True) + solve_data = SolveData(camera_ra_dec_roll=ra_dec_roll, + timestamp_exposure_end=d["lss"], imu_quat=quaternion.quaternion(*d["iq"])) + samples.append(Sample(timestamp=d["t"], measurement_type=MeasurementType.CAMERA, data=solve_data)) + else: + continue # Skip unknown measurement types + + counter += 1 + #print(samples[-1]) # For debugging (print the stored sample) + if n_max_samples is not None: + if counter >= n_max_samples: + break + + return samples + +def get_ang_diffs(last_camera_sample: Sample, camera_sample: Sample): + ang_diff_cam = qt.get_quat_angular_diff( + last_camera_sample.data.camera_ra_dec_roll.as_quaternion(), + camera_sample.data.camera_ra_dec_roll.as_quaternion()) + ang_diff_imu = qt.get_quat_angular_diff( + last_camera_sample.data.imu_quat, + camera_sample.data.imu_quat) + + return ang_diff_cam, ang_diff_imu + +def pair_camera_imu_samples(samples: list[Sample], + max_time_diff=0.1, # [s] Maximum time difference between IMU and platesolve + min_angle_diff=np.deg2rad(5), # Reject if angle from prev. sample is less than this + verbose=False + ): + """ + Pair up solved data (RaDecRoll) with the previous IMU sample. The time + difference between the IMU and camera must be small and the angular + movement between sequential pairs must be large enough. + """ + paired_samples = [] # The result that will be returned + quarantined_samples = [] # Samples that were too close in angle but could be used later + prev_imu_idx = None + for idx, samp in enumerate(samples): + if samp.measurement_type is MeasurementType.IMU: + prev_imu_idx = idx + continue + elif samp.measurement_type is MeasurementType.CAMERA and prev_imu_idx is not None: + if not samp.data.camera_ra_dec_roll.valid: + continue # Skip if camera sample is not valid + + # Skip if IMU sample is after the camera sample or large time difference: + #imu_sample = samples[prev_imu_idx] + #time_diff = samp.timestamp - imu_sample.timestamp + #print(f"{(time_diff)*1000:.1f} ms between IMU and camera sample") + #if (time_diff < 0) or (time_diff > max_time_diff): + # continue + + if not paired_samples: + paired_samples.append(samp) + continue + + # See if we can use the oldest quarantined sample + # TODO: Also add the time difference criterion to reject old samples + if quarantined_samples: + last_camera_sample = paired_samples[-1] + q_samp = quarantined_samples[0] + ang_diff_cam, ang_diff_imu = get_ang_diffs(last_camera_sample, q_samp) + if abs(ang_diff_imu) >= min_angle_diff and abs(ang_diff_cam) >= min_angle_diff: + # Use the quarantined sample + paired_samples.append(q_samp) + quarantined_samples = quarantined_samples[1:] + + # Save pairs of data if the angular difference since the previous sample is large enough + # Note: We could re-use these by another pairing + if paired_samples: + # Skip if angular diff too small (won't be able to solve) + last_camera_sample = paired_samples[-1] + ang_diff_cam, ang_diff_imu = get_ang_diffs(last_camera_sample, samp) + #print(f"Angular difference since last sample: {np.rad2deg(ang_diff):.1f} deg") + if abs(ang_diff_imu) < min_angle_diff and abs(ang_diff_cam) < min_angle_diff or ang_diff_imu < min_angle_diff or ang_diff_cam < min_angle_diff: + quarantined_samples.append(samp) + continue + else: + paired_samples.append(samp) + + assert "Shouldn't get here" + + return paired_samples +``` \ No newline at end of file diff --git a/python/PiFinder/imu/imu_align/__init__.py b/python/PiFinder/imu/imu_align/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/python/PiFinder/imu/imu_align/hand_eye_solver.py b/python/PiFinder/imu/imu_align/hand_eye_solver.py new file mode 100644 index 000000000..ddb9517f1 --- /dev/null +++ b/python/PiFinder/imu/imu_align/hand_eye_solver.py @@ -0,0 +1,274 @@ +""" +Core solver functionalities for solving the quaternion form of the hand-eye +problem: + +q1 * q_12 = q_12 * q2 + +Where the goal is to solve for the rotation q_12. Given enough measurements of +q1 and q2, we can solve for q_12. +""" +import logging +import numpy as np +import quaternion # Note: numpy-quaternion convention: quaternion(w, x, y, z) +from scipy.optimize import least_squares +import time +from typing import Union + +import PiFinder.pointing_model.quaternion_transforms as qt + +list_of_quats = list[quaternion.quaternion] + +logger = logging.getLogger("IMU.Align") + +N_UNKNOWN_PARAMS = 3 # Number of unknown parameters in the problem to solve + +def residual_rotation_vector(x, # (3,) Trial solution (q as rotation vector) + q1_list: list_of_quats, # List of rotation quaternions + q2_list: list_of_quats + ) -> np.ndarray: + """ + For solving q_cam2imu in the quaternion form of the hand-eye problem: + q1 * q_12 = q_12 * q2 + + Calculate the esiduals at the trial solution x for least squares + optimization. + """ + # Convert trial solution (rotation vector) to quaternion + q_12 = quaternion.from_rotation_vector(x) + + n_meas = len(q1_list) + residuals = np.zeros(3 * n_meas) + for ii, (q1, q2) in enumerate(zip(q1_list, q2_list)): + q_err = (q1 * q_12) * (q_12 * q2).conjugate() # Error quaternion + # Convert to rotation vector (Lie algebra logarithm map) + residuals[(3 * ii):(3 * ii + 3)] = quaternion.as_rotation_vector(q_err) + + return np.array(residuals) + + +def solve_rotation( + q1_list: list_of_quats, # List of rotation quaternions + q2_list: list_of_quats, + x0: Union[np.ndarray, list] = np.zeros(N_UNKNOWN_PARAMS), # Initial guess + residual_threshold = 0.01, # Reject samples with residual > resid_threshold in first pass + verbose=True + ): + """ + Solve the quaternion form of the hand-eye problem + dq1 * q_12 = q_12 * dq2 + + Where q_12 is the unknown rotation that rotates q1 to q2 + """ + # Solve for x by non-linear least squares (Levenberg-Marquardt) + # TODO: Tune LM params + # TODO: Calculate the Jacobians analytically? Current numerical Jacobians is probably fast enough? + result = least_squares(residual_rotation_vector, x0, method='lm', + args=(q1_list, q2_list)) + # TODO: Investigate using robust loss functions? + #result = least_squares(residual_rotation_vector, x0, loss='cauchy', + # args=(dq_cam, dq_imu)) + + # Re-run least-squares with outliers removed + if residual_threshold is not None: + # NOTE: Each quaternion measurement is converted to rotation vectors with 3 values + resid_reshaped = result.fun.reshape(-1, 3) # Each row is a sample + msk_accept = np.all(np.abs(resid_reshaped) < residual_threshold, axis=1) + q1_accept = np.array(q1_list)[msk_accept] + q2_accept = np.array(q2_list)[msk_accept] + if verbose: + print(f"Accepted {np.sum(msk_accept)}/{resid_reshaped.shape[0]} samples.") + # Run least-squares again (using previous solution as the initial guess) + result = least_squares(residual_rotation_vector, result.x, args=(q1_accept, q2_accept)) + + # Convert estimate from rotation vector to quaternion + q_12 = quaternion.from_rotation_vector(result.x) + + if verbose: + print(f"Estimated q_cam2imu: q_cam2imu={q_12}, ", + f"Func evaluations: {result.nfev}, Cost = {result.cost:.4g}, ", + f"Success: {result.success}, {result.message}") + + # Diagnostics TODO: Return these + sigma_total, condition_number = _solution_diagnostics(result) + residuals = result.fun + + return q_12 + + +def _solution_diagnostics(result): + """ + Returns the diagnostics of the least-squares solution. The input, + `result` is the output from scipy.optimize.least_squares. + + Condition number: < 10 excellent, < 100 acceptable, <1E4 weak observability + """ + t_start = time.time() + + # Estimate the uncertainty of the solution + residuals = result.fun + dof = len(residuals) - len(result.x) # Degrees-of-freedom = Number of meas - Number of params + residuals_var = np.sum(residuals**2) / dof # Estimate of residual variance + + # Using 'backslash' rather than inv(): Faster but could be unstable? + #JTJ = result.jac.T @ result.jac # Hessian approx from the Jacobians + #cov_x = residuals_var * np.linalg.solve(JTJ, np.eye(JTJ.shape[0])) + + # Estimate the uncertainty at the solution using SVD: More robust + U, s, Vt = np.linalg.svd(result.jac, full_matrices=False) + cov_x = residuals_var * (Vt.T / s**2) @ Vt + condition_number = s[0] / s[-1] + sigma_total = np.sqrt(np.trace(cov_x)) # [rad] Total rotaion uncertainty + + t_compute = time.time() - t_start + print(f"Diagnostics for q_cam2imu: compute time = {t_compute:.3f}s, ", + f"Total angular uncertainty = {np.rad2deg(sigma_total):.2} deg, ", + f"Condition number = {condition_number:.1g}") + + return sigma_total, condition_number + + +# ------- Helper functions ------- + + +def ensure_quat_list_continuity(q_list: list_of_quats) -> list_of_quats: + """ + Ensures that consecutive quaternions in the list have consistent signs (due + to the double coverage property of quaternions where q and -q represent + same rotation). + TODO: Possibly not needed. If so, remove. + """ + q_list_out = [q_list[0]] + for q in q_list[1:]: + q = qt.ensure_quat_continuity(q_list_out[-1], q) + q_list_out.append(q) + + return q_list_out + + +def calculate_relative_rotations(q1_list: list_of_quats, q2_list: list_of_quats) -> list_of_quats: + """ + Calculate the relative rotation between q1_list and the corresponding q2_list: + dq[k] = q1[k].conjugate() * q2[k] + """ + return [q1.conjugate() * q2 for q1, q2 in zip(q1_list, q2_list)] + + +def reject_small_rotations(dq_list: list_of_quats, + min_rotation=np.deg2rad(1.0), # Reject rotations below this [radians] + ): + """ + Reject small rotations + """ + pass + + +# ------ Simulation functions for testing & analysis -------------------------- + +def _q_noise(noise_amp: float): + """ Generates random quaternion noise. Noise amp is in radians """ + noise = np.radians(noise_amp) * np.random.randn(3) + return quaternion.from_rotation_vector(noise) + + +def _add_noise_to_quaternion_list(qs: list_of_quats, noise_amp: float): + """ Adds noise to a list of quaternions. noise_amp is in radians. """ + qs_out = [] + for q in qs: + qs_out.append(_q_noise(noise_amp) * q) + + return qs_out + +def _random_quaternions(N: int, max_rot=None) -> list_of_quats: + """ + Returns a list of N random quaternions. If max_rot is None, the quaternions + will be random. If specified, it limits the maximum swing angle from the + previous orientation. + """ + qs = [] + for ii in range(N): + axis = np.random.randn(3) + axis /= np.linalg.norm(axis) + + if (max_rot is None) or (ii == 0): + angle = np.random.uniform(0, np.pi) + q = quaternion.from_rotation_vector(axis * angle) + else: + angle = np.random.uniform(0, max_rot) + dq = quaternion.from_rotation_vector(axis * angle) + q = qs[-1] * dq + + qs.append(q) + + return qs + + +def simulate_quaternion_measurements( + q_12: quaternion.quaternion, # True rel. orientations (q1 ro q2 alignment) + N: int = 100, # Number of samples to simulate + max_rot = None, # Max rotation from previous orientation + q1_noise_amp: float = np.deg2rad(0.1), # Noise amp in radians + q2_noise_amp: float = np.deg2rad(0.1), # Noise amp in radians + seed=0 # Random seed. None to disable + ): + """ + Simulate camera and IMU measurements + """ + if seed is not None: + np.random.seed(seed) + + # Generate random IMU orientations + q2_true = _random_quaternions(N, max_rot=max_rot) + + # Generate corresponding camera orientations + q_21 = q_12.conjugate() + q1_true = [] + for q in q2_true: + q1_true.append(q * q_21) + + # Add noise + q1 = _add_noise_to_quaternion_list(q1_true, q1_noise_amp) + q2 = _add_noise_to_quaternion_list(q2_true, q2_noise_amp) + + return q1, q2 + + +if __name__ == "__main__": + """ + The main block simulates pairs of q1 and q2 measurements and solves + for the q_12 for the quaternion form of the hand-eye problem: + + q1 * q_12 = q_12 * q2 + """ + + # Set the true camera-from-body rotation + true_rotvec = np.radians([10, -5, 20]) + q_12_true = quaternion.from_rotation_vector(true_rotvec) + + # Simulate measurements: + q1, q2 = simulate_quaternion_measurements( + q_12_true, N=100, camera_noise_amp=np.deg2rad(0.1), + imu_noise_amp=np.deg2rad(0.1), seed=0) + + # Optional steps: + # Pair up and calculate relative rotations + # Reject small rotations + + # solve + q_12_est, sigma_total, condition_number = solve_rotation( + q1, q2, residual_threshold = 0.01, verbose=True) + + # Results + print("\nTrue q_12:") + print(quaternion.as_float_array(q_12_true)) + + print("\nEstimated q_12_est:") + print(quaternion.as_float_array(q_12_est)) + + # Error + q_error = q_12_est.conjugate() * q_12_true + error_deg = np.rad2deg( + np.linalg.norm( + quaternion.as_rotation_vector(q_error) + ) + ) + print(f"\nCalibration error: {error_deg:.6f} deg") diff --git a/python/PiFinder/imu/imu_align/imu_alignment.py b/python/PiFinder/imu/imu_align/imu_alignment.py new file mode 100644 index 000000000..ccf3501c1 --- /dev/null +++ b/python/PiFinder/imu/imu_align/imu_alignment.py @@ -0,0 +1,306 @@ +""" +Alignment of the IMU-camera axes (extrinsic calibration) + +For dead-reckoning with the IMU, we need the rotation between the IMU and +camera axes. This is done by the quaternion q_cam2imu and its inverse +q_imu2cam. + +The goal of this module is to estimate q_cam2imu. We can do this using pairs of +camera and IMU orientation quaternions measured simultaneously. + +Required measurements +--------------------- + +The measurements we have are: + +* q_eq2cam: Quaternion rotation of the camera center relative to the equatorial + frame. +* q_x2imu: The rotation of the IMU relative to some arbibtrary reference frame + X. + +The camera and IMU measurements are paired and assumed to be simultaneous. + +Algorithm: +---------- + +We can express the rotation between successive timesteps for the camera and +IMU: + +dq_cam = q_eq2cam[k-1].conjugate() * q_eq2cam[k] dq_imu = +q_x2imu[k-1].conjugate() * q_x2imu[k] + +where * is the quaternion multiplication and .conjugate() is the quaternion +conjugate, which is equivalent to the inverse for a unit quaternion. We can +relate the changes in orientation of the camera and IMU by + +dq_cam * q_cam2imu = q_cam2imu * dq_imu + +This is the quaternion version of the hand-eye calibration problem (better +known in the matrix form: AX = XB). + +We will solve for q_cam2imu by defining the error quaternion: + +q_err = (dq_cam * q_cam2imu) * (q_cam2imu * dq_imu).conjugate() + +In the ideal case, q_err will converge to the identity quaternion (1, 0, 0, 0) +at the solution. Quaternions are defined by 4 parameters with one constraint. +We will map the quaternion to a 3-parameter rotation vector, which can be +solved more efficiently and simply. The rotation vector is the product of the +unit vector around the axis of rotation (u) and the rotation (theta): + +e = theta * u = log(q_err) + +The optimization algorith will minimize the two-norm of the error rotation +vector for k = 1..N measurements: + +sum(||e[k]||^2) + + +Assumptions & limitations +------------------------- + +1. Small rotation angles for dq_cam and dq_imu could cause numerical problems + so successive samples should be selected so that the angles are sufficiently + large. +2. The IMU will drift over time so the time between the samples used to + calculate dq_imu should be short enough for drift to be negligible. +3. The camera and IMU samples should be taken simultaneously. If the camera + moves during exposure, this will introduce an error. Error could be reduced + by used samples when the camera movement is reasonably stationary. +4. In practice, the plate solver will have worse error in roll than RA and Dec. + This is not accounted for. +5. Ideally, the camera/IMU should be rotated around all three axes but on a + mount, the rotation will likely be around two axes. This may result in a + larger uncertainty for the rotation/alignment about some axes. +""" +import logging +import numpy as np +import quaternion +from dataclasses import dataclass + +from PiFinder.types.coordinates import RaDecRoll +from PiFinder.pointing_model import quaternion_transforms as qt + +list_of_quats = list[quaternion.quaternion] + +logger = logging.getLogger("IMU.Align") + + +@dataclass +class CameraImuSample: + """ + """ + timestamp: float + q_cam: quaternion.quaternion + q_imu: quaternion.quaternion + + +class SampleBuffer: + """ + Buffer of samples + """ + buffer: list + max_buffer_length: int + + def __init__(self, max_buffer_length=10): + self.max_buffer_length = max_buffer_length + self.reset_buffer() + + def reset_buffer(self): + self.buffer = [] + + @property + def len(self): + """Number of samples in buffer""" + return len(self.buffer) + + def add_sample(self, sample: CameraImuSample): + if len(self.buffer) >= self.max_buffer_length: + self.buffer.pop(0) # Remove oldest sample from buffer + self.buffer.append(sample) + + def pop_sample(self, idx: int): + """Remove and return the sample at the given index""" + return self.buffer.pop(idx) + + def remove_samples(self, idx_list: list[int]): + """Remove multiple samples by indices""" + self.buffer = [self.buffer[i] for i in range(len(self.buffer)) if i not in idx_list] + + def trim_to_max_length(self): + if self.len > self.max_buffer_length: + self.buffer = self.buffer[-self.max_buffer_length:] + + +class ImuCameraAlignment: + """ + Note that max_time_diff should be kept to a few seconds at most to avoid + gyro drift over the time between samples. + """ + candidate_buffer: SampleBuffer # Buffer of camera/IMU samples + diff_buffer: SampleBuffer # Buffer of paired differences in camera/IMU samples + + min_n_solve: int # Minimum number of samples for solve + max_time_diff: float # [s] Maximum time difference between pairs of samples + min_angle_diff: float # [rad] Pair samples with large enough angle difference + max_age: float # [s] Maximum age of sample compared to current time + + def __init__(self, candidate_buffer_length=60, min_n_solve=10, + max_time_diff=2.0, min_angle_diff=np.deg2rad(5), max_age=1200): + """ + candidate_buffer_length: Should be around sample_freq * max_time_diff + """ + self.candidate_buffer = SampleBuffer(max_buffer_length=candidate_buffer_length) + self.diff_buffer = SampleBuffer(max_buffer_length=min_n_solve) + + self.min_n_solve = min_n_solve + self.max_time_diff = max_time_diff + self.min_angle_diff = min_angle_diff + self.max_age = max_age + + self._samples_since_last_pair_attempt = 0 + + def reset_buffers(self): + self.candidate_buffer.reset_buffer() + self.diff_buffer.reset_buffer() + + def trim_buffers(self): + self.candidate_buffer.trim_to_max_length() + self.diff_buffer.trim_to_max_length() + + def add_sample(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion): + """ + Add to the candidate_buffer the camera solve & corresponding IMU sample + from integrator. + """ + if timestamp is None or cam_eq is None or cam_eq.valid is False or q_x2imu is None: + return + self.candidate_buffer.add_sample( + CameraImuSample(timestamp, cam_eq.as_quaternion(), q_x2imu)) + + def purge_old_samples(self, current_time: float): + """ + Remove samples from the candidate_buffer that are older than the current + time. + """ + allowed_timestamp = current_time - self.max_age # Purge anything older than this + + # Purge candidate_buffer: + remove_idx_list = [i for i, samp in enumerate(self.candidate_buffer.buffer) + if samp.timestamp < allowed_timestamp] + if remove_idx_list: + self.candidate_buffer.remove_samples(remove_idx_list) + + # Purge diff_buffer: + remove_idx_list = [i for i, (samp1, samp2) in enumerate(self.diff_buffer.buffer) + if samp1.timestamp < allowed_timestamp + or samp2.timestamp < allowed_timestamp] + if remove_idx_list: + self.diff_buffer.remove_samples(remove_idx_list) + + def purge_old_candidates(self): + """ + Remove samples from candidate_buffer that are older than + self.max_time_diff from other samples in buffer because these will be + never paired. + """ + if self.candidate_buffer.len <= 1: + return + + remove_idx_list = [] + timestamps = np.array([samp.timestamp for samp in self.candidate_buffer.buffer]) + for isamp in range(timestamps.shape[0]): + dt = np.abs(timestamps - timestamps[isamp]) + if np.sum(dt < self.max_time_diff) <= 1: + remove_idx_list.append(isamp) + + if remove_idx_list: + self.candidate_buffer.remove_samples(remove_idx_list) + + def pair_samples(self) -> int: + """ + Go through the candidate_buffer from the first sample in the buffer. + Pair two sets of camera/IMU samples from the candidate buffer that meet + the criteria and remove them from the buffer. Repeat all pairable + samples have been removed from the candidate_buffer. + """ + n_pairs = 0 + if self.candidate_buffer.len == 0: + return n_pairs + + remove_idx_list = [] + for isamp1, samp1 in enumerate(self.candidate_buffer.buffer[:-1]): + if isamp1 in remove_idx_list: + continue + + for isamp2 in range(isamp1 + 1, self.candidate_buffer.len): + if isamp2 in remove_idx_list: + continue + samp2 = self.candidate_buffer.buffer[isamp2] + + # Check time difference between samples: + dt = samp2.timestamp - samp1.timestamp + if dt > self.max_time_diff: + continue # Samples too far apart in time + if dt <= 0: + # Duplicate samples or sample1 is newer. Remove sample1 + remove_idx_list.append(isamp1) + continue + + # Check angle difference (from camera solve) between samples: + dtheta = qt.get_quat_angular_diff(samp1.q_cam, samp2.q_cam) + if np.abs(dtheta) < self.min_angle_diff: + continue # Samples too close in angle + + # Pair samples and remove from candidate buffer: + self.diff_buffer.add_sample((samp1, samp2)) + remove_idx_list.append(isamp1) + remove_idx_list.append(isamp2) + n_pairs += 1 + + if remove_idx_list: + self.candidate_buffer.remove_samples(remove_idx_list) + return n_pairs # Number of successful pairings + + def solve(self, n_pairs=None): + """ + Solve for the alignment between the camera and IMU using at least the + last n_pairs or all available pairs (if None). + """ + if n_pairs is None: + n_pairs = self.diff_buffer.len # Use all available data + #TODO + return None + + def add_sample_attempt_solve(self, timestamp: float, cam_eq: RaDecRoll, q_x2imu: quaternion.quaternion): + """ + For general use, call this method. Add a new sample to the buffer. When + the buffer fills up, pair samples and solve. + """ + self.add_sample(timestamp, cam_eq, q_x2imu) + + # Pair samples and solve + if ((self._samples_since_last_pair_attempt >= self.min_n_solve) or + (self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length)): + self.purge_old_samples(timestamp) + self.purge_old_candidates() + self.pair_samples() + self.trim_buffers() + + # If the candidate buffer is still full after pairing, remove a + # batch of the older samples from the buffer + if self.candidate_buffer.len >= self.candidate_buffer.max_buffer_length: + remove_list = list(range(self.min_n_solve)) + self.candidate_buffer.remove_samples(remove_list) + + self._samples_since_last_pair_attempt = 0 + else: + self._samples_since_last_pair_attempt += 1 + + # Solve if there are enough samples + if self.diff_buffer.len >= self.min_n_solve: + solution = self.solve() + self.diff_buffer.reset_buffer() # Flush the values used for solve + return solution + else: + return None diff --git a/python/PiFinder/integrator.py b/python/PiFinder/integrator.py index a2b2a85ac..5aca7136b 100644 --- a/python/PiFinder/integrator.py +++ b/python/PiFinder/integrator.py @@ -146,6 +146,18 @@ def integrator( ) estimate = _apply_successful_solve(estimate, solve_result, idr) pointing_updated = True + + # Append plate-solve and IMU states to IMU/camera alignment buffer + # TODO: Append the following: + # solve_result.last_solve_success (timestamp) + # solve_result.camera.as_radecroll() (RaDecRoll type) + # solve_result.imu_anchor + # + # Update idr.q_imu2cam with the new estimate from IMU/camera alignment + # + # TODO: SuccessfulSolve.last_solve_success is the exposure end time. It's ambiguous... + # TODO: Move ImuDeadReckoning._q_imu2cam() to a stand-alone func in imu_dead_reckoning.py with a view to deprecating it + elif isinstance(solve_result, FailedSolve): telemetry.record_solve( solve_result, predicted=estimate.pointing.aligned.estimate @@ -250,11 +262,12 @@ def _apply_successful_solve( estimate.matched_stars = result.matched_stars estimate.matched_catID = result.matched_catID - # Reseed the dead-reckoner from the new anchor. camera/aligned are - # always present on a SuccessfulSolve, so no None-guard is needed. + # Reset the dead-reckoning from the plate-solved pointing. camera/aligned + # are always present on a SuccessfulSolve, so no None-guard is needed. q_anchor = result.imu_anchor if q_anchor is None: q_anchor = quaternion.quaternion(np.nan) + idr.solve( result.camera.as_radecroll(), result.aligned.as_radecroll(), diff --git a/python/PiFinder/pointing_model/quaternion_transforms.py b/python/PiFinder/pointing_model/quaternion_transforms.py index 7177643b4..bf82654df 100644 --- a/python/PiFinder/pointing_model/quaternion_transforms.py +++ b/python/PiFinder/pointing_model/quaternion_transforms.py @@ -62,6 +62,21 @@ def get_quat_angular_diff( return d_theta # In radians +def ensure_quat_continuity(q_prev: quaternion.quaternion, + q_new: quaternion.quaternion) -> quaternion.quaternion: + """ + Ensures that consecutive quaternions to have consistent signs (due + to the double coverage property of quaternions where q and -q represent + same rotation). + """ + q0 = quaternion.as_float_array(q_prev) + q1 = quaternion.as_float_array(q_new) + + if np.dot(q0, q1) < 0: + return quaternion.quaternion(-q1) + else: + return q_new + # ========== Equatorial frame functions ============================ diff --git a/python/PiFinder/types/coordinates.py b/python/PiFinder/types/coordinates.py index 1ed9da611..62c9ecdfe 100644 --- a/python/PiFinder/types/coordinates.py +++ b/python/PiFinder/types/coordinates.py @@ -32,7 +32,7 @@ def __init__(self, ra: float, dec: float, roll: float, deg=False): @classmethod def from_quaternion(cls, q_eq: quaternion.quaternion): ra, dec, roll = q_eq2radec(q_eq) - return cls(ra, dec, roll) + return cls(ra=ra, dec=dec, roll=roll, valid=True) def reset(self): """Reset to unset state"""