/// <summary>
/// Run regression algorithm on collected data, returns two arrays of fitted coefficients, one for each segment
/// </summary>
public double[][] LearnGrasp()
{
// Remove outliers from main parabolic grasp trajectories
// 1. Perform preliminary regression, remove entries where abs residuals > 2*SD
double[] _preliminaryRegressionCoeffs = TofRegression.QuadraticFit(dataCollector[0], dataCollector[1]);
List <double> _preliminaryAbsResiduals = TofRegression.RegressionAbsDeviations(dataCollector[0], dataCollector[1], _preliminaryRegressionCoeffs);
double _stdDev = TofRegression.SStDev(_preliminaryAbsResiduals, _meanIsZero: true);
DataCollector _reducedDataCollector = dataCollector.RemoveAboveThresh(2d * _stdDev, _preliminaryAbsResiduals);
// 2. New regression with reduced sample, remove entries of original sample where abs deviations according to new regression > 1.5*SD
double[] _newRegressionCoeffs = TofRegression.QuadraticFit(_reducedDataCollector[0], _reducedDataCollector[1]);
List <double> _newAbsDeviations = TofRegression.RegressionAbsDeviations(dataCollector[0], dataCollector[1], _newRegressionCoeffs);
double _stdDevNew = TofRegression.SStDev(_newAbsDeviations, _meanIsZero: false);
DataCollector _finalDataCollector = dataCollector.RemoveAboveThresh(1.5d * _stdDevNew, _newAbsDeviations);
// Segment lists
List <double>[] _dc1 = new List <double> [2];
List <double>[] _dc2 = new List <double> [2];
bool _segmentSuccess = _finalDataCollector.SegmentLists(ref _dc1, ref _dc2, (CurLearningGrasp == LearningGrasp.Power) ? Max_SegmentPoint : Min_SegmentPoint);
// Perform linear regression on cleaner dataset
double[][] _betas = new double[2][];
_betas[0] = TofRegression.LinearFit(_dc1[0], _dc1[1]);
_betas[1] = (_segmentSuccess) ? TofRegression.LinearFit(_dc2[0], _dc2[1]) : _betas[0]; //if not segmented, just use same coeffs
return(_betas);
}
/// <summary>
/// Run thumb regression algorithm on collected data, returns fitted coefficients (two sets both the same)
/// </summary>
public double[][] LearnGraspThumb()
{
// Skip removing outliers and segment, and directly regress
// Perform quadratic regression on dataset
double[][] _betas = new double[2][];
_betas[0] = TofRegression.QuadraticFit(dataCollector[0], dataCollector[1]);
_betas[1] = _betas[0]; //just use same coeffs
return(_betas);
}
/// <summary>
/// Return grasp prediction between plate(min, graspPrediction=0) and power(max, graspPrediction=100) for given graspness value
/// </summary>
/// <param name="_graspness">Current graspness value</param>
public float PredictGrasp(float _graspness)
{
// Retrieve Tof reading
float _tofreading = TofManager.TofReading[(int)fingerType];
// Use coeffs from correct segment
int _segmentIndMax = (_graspness < GraspLearner.Max_SegmentPoint) ? 0 : 1;
int _segmentIndMin = (_graspness < GraspLearner.Min_SegmentPoint) ? 0 : 1;
// Predict tof max and min based on regression (max = power, min = plate)
float _tofGraspMax = TofRegression.RegressionPrediction(GraspPredictManager.GraspTofCoeffsMax[(int)fingerType][_segmentIndMax], _graspness);
float _tofGraspMin = TofRegression.RegressionPrediction(GraspPredictManager.GraspTofCoeffsMin[(int)fingerType][_segmentIndMin], _graspness);
_tofGraspMax = Mathf.Clamp(_tofGraspMax, 0, _tofGraspMax);
_tofGraspMin = Mathf.Clamp(_tofGraspMin, 0, _tofGraspMin);
// Predict grasp between max and min
float _graspPrediction = _linearPercentage(_tofGraspMax, _tofGraspMin, _tofreading);
//Debug.Log($"{fingerType} graspness {_graspness} tof {_tofreading} pred {_graspPrediction} [min {_tofGraspMin} max {_tofGraspMax}] from predictor");
return(_graspPrediction);
}
/// <summary>
/// Scales 2 curves given by _coeffs1 and _coeffs2 so they intersect at x=_thresh
/// </summary>
private static void _scaleCurvesIntersectAtThreshold(ref double[] _coeffs1, ref double[] _coeffs2, double _thresh)
{
// find (y at segment point) to y-scale about
double _yMaxPivot = TofRegression.RegressionPrediction(_coeffs1, GraspLearner.Max_SegmentPoint);
double _yMinPivot = TofRegression.RegressionPrediction(_coeffs2, GraspLearner.Min_SegmentPoint);
// find each y at the actual threshold
double _yMaxThresh = TofRegression.RegressionPrediction(_coeffs1, _thresh);
double _yMinThresh = TofRegression.RegressionPrediction(_coeffs2, _thresh);
// find y at threshold so power and plate intersect at their average
double _yAvgAtThreshold = 0.5d * (_yMaxThresh + _yMinThresh);
// find y scale factor so intersects at (x,y) = (threshold, avg of power and plate), where avg = yPivot + k_i*(f_i - yPivot)
double _kMax = (_yAvgAtThreshold - _yMaxPivot) / (_yMaxThresh - _yMaxPivot);
double _kMin = (_yAvgAtThreshold - _yMinPivot) / (_yMinThresh - _yMinPivot);
// scale coeffs //a,b -> ka,kb // c-> kc + ym (1-k)
_scaleVectorInPlace(ref _coeffs1, _kMax);
_scaleVectorInPlace(ref _coeffs2, _kMin);
// offset constant term
_coeffs1[0] += (_yMaxPivot * (1d - _kMax));
_coeffs2[0] += (_yMinPivot * (1d - _kMin));
}
