1 /*
2 * Copyright (C) 2016 Alberto Irurueta Carro (alberto@irurueta.com)
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16 package com.irurueta.ar.slam;
17
18 import com.irurueta.geometry.Quaternion;
19 import com.irurueta.geometry.Rotation3D;
20
21 /**
22 * Base class for estimating mean and covariance of noise in control values
23 * when the system state is held constant (only noise is provided as control
24 * input).
25 * This subclass of BaseSlamCalibrator is used for slam estimator taking into
26 * account absolute orientation.
27 *
28 * @param <D> type of calibration data.
29 */
30 public abstract class AbsoluteOrientationBaseSlamCalibrator<D extends BaseCalibrationData> extends
31 BaseSlamCalibrator<D> {
32
33 /**
34 * Timestamp expressed in nanoseconds since the epoch time of the last
35 * sample containing absolute orientation.
36 */
37 protected long orientationTimestampNanos = -1;
38
39 /**
40 * Number of orientation samples accumulated since last full sample.
41 */
42 protected int accumulatedOrientationSamples = 0;
43
44 /**
45 * Average orientation accumulated since last full sample.
46 */
47 protected final Quaternion accumulatedOrientation;
48
49 /**
50 * Temporary quaternion. For memory reuse.
51 */
52 private Quaternion tempQ;
53
54 /**
55 * Constructor.
56 *
57 * @param sampleLength sample length of control values used during
58 * prediction stage in SLAM estimator.
59 * @throws IllegalArgumentException if sample length is less than 1.
60 */
61 protected AbsoluteOrientationBaseSlamCalibrator(final int sampleLength) {
62 super(sampleLength);
63 accumulatedOrientation = new Quaternion();
64 }
65
66 /**
67 * Gets timestamp expressed in nanoseconds since the epoch time of the last
68 * orientation sample, or -1 if no sample has been set yet.
69 *
70 * @return timestamp expressed in nanoseconds since the epoch time of the
71 * last orientation sample, or -1.
72 */
73 public long getOrientationTimestampNanos() {
74 return orientationTimestampNanos;
75 }
76
77 /**
78 * Gets average orientation accumulated since last full sample.
79 *
80 * @return orientation accumulated since last full sample.
81 */
82 public Rotation3D getAccumulatedOrientation() {
83 return accumulatedOrientation.toQuaternion();
84 }
85
86 /**
87 * Gets average orientation accumulated since last full sample.
88 *
89 * @param result instance where orientation accumulated since last full
90 * sample will be stored.
91 */
92 public void getAccumulatedOrientation(final Rotation3D result) {
93 result.fromRotation(accumulatedOrientation);
94 }
95
96 /**
97 * Gets number of orientation samples accumulated since last full sample.
98 *
99 * @return number of orientation samples accumulated since last full sample.
100 */
101 public int getAccumulatedOrientationSamples() {
102 return accumulatedOrientationSamples;
103 }
104
105 /**
106 * Indicates whether the orientation sample has been received since last
107 * full sample (accelerometer + gyroscope + orientation).
108 *
109 * @return true if orientation sample has been received, false otherwise.
110 */
111 public boolean isOrientationSampleReceived() {
112 return accumulatedOrientationSamples > 0;
113 }
114
115 /**
116 * Indicates whether a full sample (accelerometer + gyroscope +
117 * magnetic field) has been received or not.
118 *
119 * @return true if full sample has been received, false otherwise.
120 */
121 @Override
122 public boolean isFullSampleAvailable() {
123 return super.isFullSampleAvailable() && isOrientationSampleReceived();
124 }
125
126 /**
127 * Provides a new orientation sample.
128 * If accumulation is enabled, samples are averaged until a full sample is
129 * received.
130 * When a full sample (accelerometer + gyroscope + orientation) is
131 * received, internal state gets also updated.
132 *
133 * @param timestamp timestamp of accelerometer sample since epoch time and
134 * expressed in nanoseconds.
135 * @param orientation new orientation.
136 */
137 @SuppressWarnings("DuplicatedCode")
138 public void updateOrientationSample(final long timestamp, final Rotation3D orientation) {
139 if (!isFullSampleAvailable()) {
140 orientationTimestampNanos = timestamp;
141 if (isAccumulationEnabled() && isOrientationSampleReceived()) {
142 // accumulation enabled
143 final var nextSamples = accumulatedOrientationSamples + 1;
144
145 var accumA = accumulatedOrientation.getA();
146 var accumB = accumulatedOrientation.getB();
147 var accumC = accumulatedOrientation.getC();
148 var accumD = accumulatedOrientation.getD();
149
150 if (tempQ == null) {
151 tempQ = new Quaternion();
152 }
153 tempQ.fromRotation(orientation);
154 tempQ.normalize();
155 final var a = tempQ.getA();
156 final var b = tempQ.getB();
157 final var c = tempQ.getC();
158 final var d = tempQ.getD();
159
160 accumA = (accumA * accumulatedOrientationSamples + a) / nextSamples;
161 accumB = (accumB * accumulatedOrientationSamples + b) / nextSamples;
162 accumC = (accumC * accumulatedOrientationSamples + c) / nextSamples;
163 accumD = (accumD * accumulatedOrientationSamples + d) / nextSamples;
164
165 accumulatedOrientation.setA(accumA);
166 accumulatedOrientation.setB(accumB);
167 accumulatedOrientation.setC(accumC);
168 accumulatedOrientation.setD(accumD);
169 accumulatedOrientationSamples = nextSamples;
170 } else {
171 // accumulation disabled
172 accumulatedOrientation.fromRotation(orientation);
173 accumulatedOrientationSamples++;
174 }
175 notifyFullSampleAndResetSampleReceive();
176 }
177 }
178
179 /**
180 * Gets most recent timestamp of received partial samples (accelerometer,
181 * gyroscope or magnetic field).
182 *
183 * @return most recent timestamp of received partial sample.
184 */
185 @Override
186 public long getMostRecentTimestampNanos() {
187 final var mostRecent = super.getMostRecentTimestampNanos();
188 return Math.max(mostRecent, orientationTimestampNanos);
189 }
190
191 /**
192 * Notifies that a full sample has been received and resets flags indicating
193 * whether partial samples have been received.
194 */
195 @Override
196 protected void notifyFullSampleAndResetSampleReceive() {
197 if (isFullSampleAvailable()) {
198 processFullSample();
199 accumulatedAccelerometerSamples = accumulatedGyroscopeSamples = accumulatedOrientationSamples = 0;
200 }
201 }
202 }