RobustRangingRadioSourceEstimator2D.java
/*
* Copyright (C) 2018 Alberto Irurueta Carro (alberto@irurueta.com)
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package com.irurueta.navigation.indoor.radiosource;
import com.irurueta.geometry.Point2D;
import com.irurueta.navigation.LockedException;
import com.irurueta.navigation.NavigationException;
import com.irurueta.navigation.indoor.Beacon;
import com.irurueta.navigation.indoor.BeaconLocated2D;
import com.irurueta.navigation.indoor.RadioSource;
import com.irurueta.navigation.indoor.RadioSourceLocated;
import com.irurueta.navigation.indoor.RangingReadingLocated;
import com.irurueta.navigation.indoor.WifiAccessPoint;
import com.irurueta.navigation.indoor.WifiAccessPointLocated2D;
import com.irurueta.numerical.robust.RobustEstimatorMethod;
import java.util.ArrayList;
import java.util.List;
/**
* This is an abstract class to robustly estimate 2D position of a radio source (e.g. Wi-Fi
* access point or bluetooth beacon), by discarding outliers.
*
* @param <S> a {@link RadioSource} type.
*/
@SuppressWarnings("DuplicatedCode")
public abstract class RobustRangingRadioSourceEstimator2D<S extends RadioSource> extends
RobustRangingRadioSourceEstimator<S, Point2D> {
/**
* Radio source estimator used internally.
*/
protected final RangingRadioSourceEstimator2D<S> innerEstimator = new RangingRadioSourceEstimator2D<>();
/**
* Subset of readings used by inner estimator.
*/
private final List<RangingReadingLocated<S, Point2D>> innerReadings = new ArrayList<>();
/**
* Constructor.
*/
protected RobustRangingRadioSourceEstimator2D() {
super();
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
* Sets radio signal ranging readings belonging to the same radio source.
*
* @param readings radio signal ranging readings belonging to the same
* radio source.
* @throws IllegalArgumentException if readings are not valid.
*/
protected RobustRangingRadioSourceEstimator2D(final List<? extends RangingReadingLocated<S, Point2D>> readings) {
super(readings);
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
*
* @param listener listener in charge of attending events raised by this instance.
*/
protected RobustRangingRadioSourceEstimator2D(
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
super(listener);
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
* Sets radio signal readings belonging to the same radio source.
*
* @param readings radio signal readings belonging to the same radio source.
* @param listener listener in charge of attending events raised by this instance.
* @throws IllegalArgumentException if readings are not valid.
*/
protected RobustRangingRadioSourceEstimator2D(
final List<? extends RangingReadingLocated<S, Point2D>> readings,
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
super(readings, listener);
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
*
* @param initialPosition initial position to start the estimation or radio
* source position.
*/
protected RobustRangingRadioSourceEstimator2D(final Point2D initialPosition) {
super(initialPosition);
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
* Sets radio signal readings belonging to the same radio source.
*
* @param readings radio signal readings belonging to the same radio source.
* @param initialPosition initial position to start the estimation of radio
* source position.
* @throws IllegalArgumentException if readings are not valid.
*/
protected RobustRangingRadioSourceEstimator2D(
final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition) {
super(readings, initialPosition);
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
*
* @param initialPosition initial position to start the estimation of radio
* source position.
* @param listener listener in charge of attending events raised by this instance.
*/
protected RobustRangingRadioSourceEstimator2D(
final Point2D initialPosition, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
super(initialPosition, listener);
preliminarySubsetSize = getMinReadings();
}
/**
* Constructor.
* Sets radio signal ranging readings belonging to the same radio source.
*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param initialPosition initial position to start the estimation of radio source
* position.
* @param listener listener in charge of attending events raised by this instance.
* @throws IllegalArgumentException if readings are not valid.
*/
protected RobustRangingRadioSourceEstimator2D(
final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener) {
super(readings, initialPosition, listener);
preliminarySubsetSize = getMinReadings();
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>();
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>();
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>();
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>();
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>();
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final List<? extends RangingReadingLocated<S, Point2D>> readings, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(readings);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(readings);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final List<? extends RangingReadingLocated<S, Point2D>> readings,
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings, listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings, listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings, listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(readings, listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(readings, listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final Point2D initialPosition, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(initialPosition);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(initialPosition);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(initialPosition);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(initialPosition);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(initialPosition);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final Point2D initialPosition, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final List<? extends RangingReadingLocated<S, Point2D>> readings, final Point2D initialPosition,
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>();
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>();
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>();
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, readings);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, readings);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.*
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.**
* @param readings radio signal ranging readings belonging to the same radio source.
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings, listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings, listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings, listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, readings, listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, readings, listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.*
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final Point2D initialPosition, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(initialPosition);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(initialPosition);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(initialPosition);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, initialPosition);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, initialPosition);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
final Point2D initialPosition, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, readings, initialPosition);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, readings, initialPosition);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.*
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final Point2D initialPosition,
final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener, final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(initialPosition, listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, initialPosition, listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, initialPosition, listener);
};
}
/**
* Creates a robust 2D position radio source estimator.
*
* @param qualityScores quality scores corresponding to each provided
* sample. The larger the score value the better
* the quality of the sample.*
* @param readings radio signal ranging readings belonging to the same radio source.
* @param initialPosition initial position to start the estimation or radio
* source position.
* @param listener listener in charge of attending events raised by this instance.
* @param method robust estimator method.
* @param <S> a {@link RadioSource} type.
* @return a new robust 2D position radio source estimator.
*/
public static <S extends RadioSource> RobustRangingRadioSourceEstimator2D<S> create(
final double[] qualityScores, final List<? extends RangingReadingLocated<S, Point2D>> readings,
final Point2D initialPosition, final RobustRangingRadioSourceEstimatorListener<S, Point2D> listener,
final RobustEstimatorMethod method) {
return switch (method) {
case RANSAC -> new RANSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
case LMEDS -> new LMedSRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
case MSAC -> new MSACRobustRangingRadioSourceEstimator2D<>(readings, initialPosition, listener);
case PROSAC -> new PROSACRobustRangingRadioSourceEstimator2D<>(qualityScores, readings, initialPosition,
listener);
default -> new PROMedSRobustRangingRadioSourceEstimator2D<>(qualityScores, readings, initialPosition,
listener);
};
}
/**
* Gets minimum required number of readings to estimate position of radio source,
* which is 3 readings.
*
* @return minimum required number of readings.
*/
@Override
public int getMinReadings() {
return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH + 1;
}
/**
* Gets number of dimensions of position points.
* This is always 2.
*
* @return number of dimensions of position points.
*/
@Override
public int getNumberOfDimensions() {
return Point2D.POINT2D_INHOMOGENEOUS_COORDINATES_LENGTH;
}
/**
* Gets estimated located radio source.
*
* @return estimated located radio source or null.
*/
@SuppressWarnings("unchecked")
@Override
public RadioSourceLocated<Point2D> getEstimatedRadioSource() {
final var readings = getReadings();
if (readings == null || readings.isEmpty()) {
return null;
}
final var source = readings.get(0).getSource();
final var estimatedPosition = getEstimatedPosition();
if (estimatedPosition == null) {
return null;
}
final var estimatedPositionCovariance = getEstimatedPositionCovariance();
if (source instanceof WifiAccessPoint accessPoint) {
return new WifiAccessPointLocated2D(accessPoint.getBssid(), accessPoint.getFrequency(),
accessPoint.getSsid(), estimatedPosition, estimatedPositionCovariance);
} else if (source instanceof Beacon beacon) {
return new BeaconLocated2D(beacon.getIdentifiers(), beacon.getTransmittedPower(), beacon.getFrequency(),
beacon.getBluetoothAddress(), beacon.getBeaconTypeCode(), beacon.getManufacturer(),
beacon.getServiceUuid(), beacon.getBluetoothName(), estimatedPosition, estimatedPositionCovariance);
} else {
return null;
}
}
/**
* Indicates whether an homogeneous linear solver is used to estimate an initial
* position.
*
* @return true if homogeneous linear solver is used, false if an inhomogeneous linear
* one is used instead.
*/
@Override
public boolean isHomogeneousLinearSolverUsed() {
return innerEstimator.isHomogeneousLinearSolverUsed();
}
/**
* Specifies whether an homogeneous linear solver is used to estimate an initial
* position.
*
* @param useHomogeneousLinearSolver true if homogeneous linear solver is used, false
* if an inhomogeneous linear one is used instead.
* @throws LockedException if estimator is locked.
*/
@Override
public void setHomogeneousLinearSolverUsed(final boolean useHomogeneousLinearSolver) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
innerEstimator.setHomogeneousLinearSolverUsed(useHomogeneousLinearSolver);
}
/**
* Solves preliminary solution for a subset of samples.
*
* @param samplesIndices indices of subset samples.
* @param solutions instance where solution will be stored.
*/
@Override
protected void solvePreliminarySolutions(final int[] samplesIndices, final List<Solution<Point2D>> solutions) {
try {
innerReadings.clear();
for (final var samplesIndex : samplesIndices) {
innerReadings.add(readings.get(samplesIndex));
}
// initial position might or might not be available
innerEstimator.setInitialPosition(initialPosition);
innerEstimator.setReadings(innerReadings);
// for preliminary solutions, non-linear solver is not needed, and if no
// initial position is used, we can obtain faster solutions disabling
// non-linear solver and using a linear one only (because covariance is not
// required)
innerEstimator.setNonLinearSolverEnabled(initialPosition != null);
// indicates whether readings position covariances must be taken into account
innerEstimator.setUseReadingPositionCovariances(useReadingPositionCovariances);
innerEstimator.estimate();
final var estimatedPosition = innerEstimator.getEstimatedPosition();
solutions.add(new Solution<>(estimatedPosition));
} catch (final NavigationException ignore) {
// if anything fails, no solution is added
}
}
/**
* Attempts to refine estimated position and transmitted power contained in
* provided solution if refinement is requested.
* This method sets a refined result and transmitted power or provided input
* result if refinement is not requested or has failed.
* If refinement is enabled, and it is requested to keep covariance, this method
* will also keep covariance of refined result.
* solution if not requested or refinement failed.
*
* @param result result to be refined.
*/
protected void attemptRefine(final Solution<Point2D> result) {
final var initialPosition = result.getEstimatedPosition();
if (refineResult && inliersData != null) {
final var inliers = inliersData.getInliers();
final var nSamples = readings.size();
innerReadings.clear();
for (var i = 0; i < nSamples; i++) {
if (inliers.get(i)) {
// sample is inlier
innerReadings.add(readings.get(i));
}
}
try {
innerEstimator.setInitialPosition(initialPosition);
innerEstimator.setReadings(innerReadings);
innerEstimator.setNonLinearSolverEnabled(true);
innerEstimator.setUseReadingPositionCovariances(useReadingPositionCovariances);
innerEstimator.estimate();
final var cov = innerEstimator.getEstimatedCovariance();
if (keepCovariance && cov != null) {
// keep covariance
estimatedPositionCovariance = covariance = cov;
} else {
covariance = null;
estimatedPositionCovariance = null;
}
estimatedPosition = innerEstimator.getEstimatedPosition();
} catch (final Exception e) {
// refinement failed, so we return input value, and covariance
// becomes unavailable
covariance = null;
estimatedPositionCovariance = null;
estimatedPosition = initialPosition;
}
} else {
covariance = null;
estimatedPositionCovariance = null;
estimatedPosition = initialPosition;
}
}
}