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 import java.io.Serializable;
22
23 /**
24 * Base class to estimate position, velocity, acceleration and orientation of
25 * a device using sensor data such as accelerometers, gyroscopes and magnetic
26 * fields (to obtain absolute orientation).
27 * Implementations of this class are designed taking into account sensors
28 * available on Android devices.
29 * This subclass of BaseSlamEstimator takes into account magnetic field measures
30 * to compute device orientation respect Earth's frame rather than relative to
31 * the start position and orientation of the device.
32 *
33 * @param <D> calibrator type associated to implementations of SLAM calibration
34 * data.
35 */
36 public abstract class AbsoluteOrientationBaseSlamEstimator<D extends BaseCalibrationData> extends
37 BaseSlamEstimator<D> implements Serializable {
38
39 /**
40 * Timestamp expressed in nanoseconds since the epoch time of the last
41 * sample containing absolute orientation.
42 */
43 protected long orientationTimestampNanos = -1;
44
45 /**
46 * Number of orientation samples accumulated since last full sample.
47 */
48 protected int accumulatedOrientationSamples = 0;
49
50 /**
51 * Average orientation accumulated since last full sample.
52 */
53 protected Quaternion accumulatedOrientation = new Quaternion();
54
55 /**
56 * Temporary quaternion. For memory reuse.
57 */
58 private Quaternion tempQ;
59
60 /**
61 * Constructor.
62 */
63 protected AbsoluteOrientationBaseSlamEstimator() {
64 super();
65 }
66
67 /**
68 * Gets timestamp expressed in nanoseconds since the epoch time of the last
69 * orientation sample, or -1 if no sample has been set yet.
70 *
71 * @return timestamp expressed in nanoseconds since the epoch time of the
72 * last orientation sample, or -1.
73 */
74 public long getOrientationTimestampNanos() {
75 return orientationTimestampNanos;
76 }
77
78 /**
79 * Gets average orientation accumulated since last full sample.
80 *
81 * @return orientation accumulated since last full sample.
82 */
83 public Rotation3D getAccumulatedOrientation() {
84 return accumulatedOrientation.toQuaternion();
85 }
86
87 /**
88 * Gets average orientation accumulated since last full sample.
89 *
90 * @param result instance where orientation accumulated since last full
91 * sample will be stored.
92 */
93 public void getAccumulatedOrientation(final Rotation3D result) {
94 result.fromRotation(accumulatedOrientation);
95 }
96
97 /**
98 * Gets number of orientation samples accumulated since last full sample.
99 *
100 * @return number of orientation samples accumulated since last full sample.
101 */
102 public int getAccumulatedOrientationSamples() {
103 return accumulatedOrientationSamples;
104 }
105
106 /**
107 * Indicates whether the orientation sample has been received since last
108 * full sample (accelerometer + gyroscope + orientation).
109 *
110 * @return true if orientation sample has been received, false otherwise.
111 */
112 public boolean isOrientationSampleReceived() {
113 return accumulatedOrientationSamples > 0;
114 }
115
116 /**
117 * Indicates whether a full sample (accelerometer + gyroscope +
118 * magnetic field) has been received or not.
119 *
120 * @return true if full sample has been received, false otherwise.
121 */
122 @Override
123 public boolean isFullSampleAvailable() {
124 return super.isFullSampleAvailable() && isOrientationSampleReceived();
125 }
126
127 /**
128 * Provides a new orientation sample.
129 * If accumulation is enabled, samples are averaged until a full sample is
130 * received.
131 * When a full sample (accelerometer + gyroscope + orientation) is
132 * received, internal state gets also updated.
133 *
134 * @param timestamp timestamp of accelerometer sample since epoch time and
135 * expressed in nanoseconds.
136 * @param orientation new orientation.
137 */
138 @SuppressWarnings("DuplicatedCode")
139 public void updateOrientationSample(final long timestamp, final Rotation3D orientation) {
140 if (!isFullSampleAvailable()) {
141 orientationTimestampNanos = timestamp;
142 if (isAccumulationEnabled() && isOrientationSampleReceived()) {
143 // accumulation enabled
144 final var nextSamples = accumulatedOrientationSamples + 1;
145
146 var accumA = accumulatedOrientation.getA();
147 var accumB = accumulatedOrientation.getB();
148 var accumC = accumulatedOrientation.getC();
149 var accumD = accumulatedOrientation.getD();
150
151 if (tempQ == null) {
152 tempQ = new Quaternion();
153 }
154 tempQ.fromRotation(orientation);
155 tempQ.normalize();
156 final var a = tempQ.getA();
157 final var b = tempQ.getB();
158 final var c = tempQ.getC();
159 final var d = tempQ.getD();
160
161 accumA = (accumA * accumulatedOrientationSamples + a) / nextSamples;
162 accumB = (accumB * accumulatedOrientationSamples + b) / nextSamples;
163 accumC = (accumC * accumulatedOrientationSamples + c) / nextSamples;
164 accumD = (accumD * accumulatedOrientationSamples + d) / nextSamples;
165
166 accumulatedOrientation.setA(accumA);
167 accumulatedOrientation.setB(accumB);
168 accumulatedOrientation.setC(accumC);
169 accumulatedOrientation.setD(accumD);
170 accumulatedOrientationSamples = nextSamples;
171 } else {
172 // accumulation disabled
173 accumulatedOrientation.fromRotation(orientation);
174 accumulatedOrientationSamples++;
175 }
176 notifyFullSampleAndResetSampleReceive();
177 }
178 }
179
180 /**
181 * Gets most recent timestamp of received partial samples (accelerometer,
182 * gyroscope or magnetic field).
183 *
184 * @return most recent timestamp of received partial sample.
185 */
186 @Override
187 public long getMostRecentTimestampNanos() {
188 final var mostRecent = super.getMostRecentTimestampNanos();
189 return Math.max(mostRecent, orientationTimestampNanos);
190 }
191
192 /**
193 * Notifies that a full sample has been received and resets flags indicating
194 * whether partial samples have been received.
195 */
196 @Override
197 protected void notifyFullSampleAndResetSampleReceive() {
198 if (isFullSampleAvailable()) {
199 processFullSample();
200 accumulatedAccelerometerSamples = accumulatedGyroscopeSamples = accumulatedOrientationSamples = 0;
201 }
202 }
203 }