View Javadoc
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  
20  /**
21   * Processes data to estimate calibration for absolute orientation SLAM
22   * estimator.
23   * This class must be used while gathering data for a system being kept constant
24   * (under no motion).
25   */
26  public class AbsoluteOrientationSlamCalibrator extends
27          AbsoluteOrientationBaseSlamCalibrator<AbsoluteOrientationSlamCalibrationData> {
28  
29      /**
30       * Last sample of linear acceleration along x-axis.
31       */
32      private double lastAccelerationX;
33  
34      /**
35       * Last sample of linear acceleration along y-axis.
36       */
37      private double lastAccelerationY;
38  
39      /**
40       * Last sample of linear acceleration along z-axis.
41       */
42      private double lastAccelerationZ;
43  
44      /**
45       * Last sample of angular speed along x-axis.
46       */
47      private double lastAngularSpeedX;
48  
49      /**
50       * Last sample of angular speed along y-axis.
51       */
52      private double lastAngularSpeedY;
53  
54      /**
55       * Last sample of angular speed along z-axis.
56       */
57      private double lastAngularSpeedZ;
58  
59      /**
60       * Last timestamp of a full sample expressed in nanoseconds since the epoch
61       * time.
62       */
63      private long lastTimestampNanos = -1;
64  
65      /**
66       * Last sample of absolute orientation.
67       */
68      private Quaternion lastOrientation;
69  
70      /**
71       * Variation of orientation respect to last sample.
72       */
73      private final Quaternion deltaOrientation;
74  
75      /**
76       * Constructor.
77       */
78      public AbsoluteOrientationSlamCalibrator() {
79          super(AbsoluteOrientationSlamEstimator.CONTROL_LENGTH);
80          lastOrientation = new Quaternion();
81          deltaOrientation = new Quaternion();
82      }
83  
84      /**
85       * Resets calibrator.
86       */
87      @Override
88      public void reset() {
89          super.reset();
90          lastOrientation = new Quaternion();
91          lastAccelerationX = lastAccelerationY = lastAccelerationZ =
92                  lastAngularSpeedX = lastAngularSpeedY = lastAngularSpeedZ = 0.0;
93          lastTimestampNanos = -1;
94      }
95  
96      /**
97       * Obtains the number of state parameters in associated SLAM estimator.
98       *
99       * @return number of state parameters.
100      */
101     @Override
102     protected int getEstimatorStateLength() {
103         return AbsoluteOrientationSlamEstimator.STATE_LENGTH;
104     }
105 
106     /**
107      * Gets a new instance containing calibration data estimated by this
108      * calibrator.
109      *
110      * @return a new calibration data instance.
111      */
112     @Override
113     public AbsoluteOrientationSlamCalibrationData getCalibrationData() {
114         final var result = new AbsoluteOrientationSlamCalibrationData();
115         getCalibrationData(result);
116         return result;
117     }
118 
119     /**
120      * Processes a full sample of accelerometer and gyroscope data to compute
121      * statistics such as mean and covariance of variations.
122      */
123     @SuppressWarnings("DuplicatedCode")
124     @Override
125     protected void processFullSample() {
126         if (listener != null) {
127             listener.onFullSampleReceived(this);
128         }
129 
130         final var timestamp = getMostRecentTimestampNanos();
131         if (lastTimestampNanos < 0) {
132             // first time receiving control data we cannot determine its
133             // variation
134             lastOrientation.fromQuaternion(accumulatedOrientation);
135 
136             lastAccelerationX = accumulatedAccelerationSampleX;
137             lastAccelerationY = accumulatedAccelerationSampleY;
138             lastAccelerationZ = accumulatedAccelerationSampleZ;
139 
140             lastAngularSpeedX = accumulatedAngularSpeedSampleX;
141             lastAngularSpeedY = accumulatedAngularSpeedSampleY;
142             lastAngularSpeedZ = accumulatedAngularSpeedSampleZ;
143 
144             lastTimestampNanos = timestamp;
145 
146             if (listener != null) {
147                 listener.onFullSampleProcessed(this);
148             }
149 
150             return;
151         }
152 
153         accumulatedOrientation.normalize();
154 
155         lastOrientation.inverse(deltaOrientation);
156         deltaOrientation.combine(accumulatedOrientation);
157         deltaOrientation.normalize();
158 
159         final var deltaAccelerationX = accumulatedAccelerationSampleX - lastAccelerationX;
160         final var deltaAccelerationY = accumulatedAccelerationSampleY - lastAccelerationY;
161         final var deltaAccelerationZ = accumulatedAccelerationSampleZ - lastAccelerationZ;
162         final var deltaAngularSpeedX = accumulatedAngularSpeedSampleX - lastAngularSpeedX;
163         final var deltaAngularSpeedY = accumulatedAngularSpeedSampleY - lastAngularSpeedY;
164         final var deltaAngularSpeedZ = accumulatedAngularSpeedSampleZ - lastAngularSpeedZ;
165 
166         sample[0] = deltaOrientation.getA();
167         sample[1] = deltaOrientation.getB();
168         sample[2] = deltaOrientation.getC();
169         sample[3] = deltaOrientation.getD();
170         sample[4] = sample[5] = sample[6] = 0.0;
171 
172         sample[7] = deltaAccelerationX;
173         sample[8] = deltaAccelerationY;
174         sample[9] = deltaAccelerationZ;
175 
176         sample[10] = deltaAngularSpeedX;
177         sample[11] = deltaAngularSpeedY;
178         sample[12] = deltaAngularSpeedZ;
179         updateSample();
180 
181         lastOrientation.combine(deltaOrientation);
182         lastAccelerationX = accumulatedAccelerationSampleX;
183         lastAccelerationY = accumulatedAccelerationSampleY;
184         lastAccelerationZ = accumulatedAccelerationSampleZ;
185 
186         lastAngularSpeedX = accumulatedAngularSpeedSampleX;
187         lastAngularSpeedY = accumulatedAngularSpeedSampleY;
188         lastAngularSpeedZ = accumulatedAngularSpeedSampleZ;
189 
190         lastTimestampNanos = timestamp;
191 
192         if (listener != null) {
193             listener.onFullSampleProcessed(this);
194         }
195     }
196 }