//************This method could be used only when there are many steps, otherwise it might be removed*******************************************//
//Create a combined mxb matrix for trajectories from A to B to A
Eigen::MatrixXf CreateCombinedMXBMatrix(Eigen::MatrixXf matrixA, Eigen::MatrixXf matrixB, float increment, int i, int j, int offset)
{
//Create a matrix with zmp trajectory from A to B
Eigen::MatrixXf mxbMatrixBA = MXB(matrixA.row(i), matrixB.row(j), increment, offset);
Eigen::MatrixXf noZMPMovement(mxbMatrixBA.rows(), mxbMatrixBA.cols());
for(int i = 0; i < mxbMatrixBA.rows(); i++)
{
noZMPMovement.row(i) = mxbMatrixBA.bottomRows(1);
}
Eigen::MatrixXf tempMatrix(2*mxbMatrixBA.rows(), mxbMatrixBA.cols());
tempMatrix << mxbMatrixBA, noZMPMovement;
mxbMatrixBA.swap(tempMatrix);
//Append both step trajectories
if(matrixA.rows() > i+1)
{
//Create a matrix with zmp trajectory from B to A
Eigen::MatrixXf mxbMatrixAB = MXB(matrixB.row(j), matrixA.row(i+1), increment, offset);
for(int i = 0; i < mxbMatrixAB.rows(); i++)
{
noZMPMovement.row(i) = mxbMatrixAB.bottomRows(1);
}
Eigen::MatrixXf tempMatrix2(2*mxbMatrixAB.rows(), mxbMatrixAB.cols());
tempMatrix2 << mxbMatrixAB, noZMPMovement;
mxbMatrixAB.swap(tempMatrix2);
Eigen::MatrixXf mxbMatrix(mxbMatrixBA.rows()+mxbMatrixAB.rows(), mxbMatrixBA.cols());
mxbMatrix << mxbMatrixBA, mxbMatrixAB;
return mxbMatrix;
}
else
{
return mxbMatrixBA;
}
}
int Shell::split(QVariantList points)
{
QPointF point;
//omit last value, because it is always zero
int dataSize = points.size()-1;
Eigen::MatrixXf x; x.resize(dataSize,2);
Eigen::VectorXf y(dataSize);
//create the linear eq system in the form of y = beta1*x + beta2*1
for( int i = 0; i < dataSize; i++)
{
point = points[i].toPointF();
x(i, 0) = 1.0f; //beta for y-intercept
x(i, 1) = point.x(); //beta for slope (dependent on x)
y(i) = point.y();
}
//Error function (least squares of ax+b)
auto error = [](Regression reg, int b, int a)->float
{
float result = 0;
for(int i=0 ; i< reg.y.size(); i++)
{
float functionValue = a*reg.x(i, 1)+ b;
float squarederror = std::pow(reg.y(i) - functionValue, 2);
result+=squarederror;
}
return result;
};
//Perform all pairs of regressions
float lowestError = std::numeric_limits<float>::max();
float r1a, r1b;
float r2a, r2b;
int splitIndex = 0;
for( int i = 2; i < dataSize; i++)
{
Regression reg1; reg1.x = x.topRows(i); reg1.y = y.head(i);
Regression reg2; reg2.x = x.bottomRows(dataSize-i); reg2.y = y.tail(dataSize-i);
Eigen::MatrixXf reg1Result = ((reg1.x.transpose() * reg1.x).inverse() * reg1.x.transpose()) * reg1.y;
Eigen::MatrixXf reg2Result = ((reg2.x.transpose() * reg2.x).inverse() * reg2.x.transpose()) * reg2.y;
float currentError = error(reg1,reg1Result(0),reg1Result(1)) + error(reg2,reg2Result(0),reg2Result(1));
if (currentError < lowestError)
{
r1a = reg1Result(1); r1b = reg1Result(0);
r2a = reg2Result(1); r2b = reg2Result(0);
lowestError = currentError;
splitIndex = i;
}
}
std::cout << "r1:" << r1a << "x + " << r1b << std::endl;
std::cout << "r2:" << r2a << "x + " << r2b << std::endl;
std::cout << "(smallest error:" << lowestError << ")" << std::endl;
return splitIndex;
}
