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