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