Matrix2f Matrix2f::inverse( bool* pbIsSingular, float epsilon )
{
float determinant = m_elements[ 0 ] * m_elements[ 3 ] - m_elements[ 2 ] * m_elements[ 1 ];
bool isSingular = ( fabs( determinant ) < epsilon );
if( isSingular )
{
if( pbIsSingular != NULL )
{
*pbIsSingular = true;
}
return Matrix2f();
}
else
{
if( pbIsSingular != NULL )
{
*pbIsSingular = false;
}
float reciprocalDeterminant = 1.0f / determinant;
return Matrix2f
(
m_elements[ 3 ] * reciprocalDeterminant, -m_elements[ 2 ] * reciprocalDeterminant,
-m_elements[ 1 ] * reciprocalDeterminant, m_elements[ 0 ] * reciprocalDeterminant
);
}
}
void test_eigen2_array()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( array(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( array(Matrix2f()) );
CALL_SUBTEST_3( array(Matrix4d()) );
CALL_SUBTEST_4( array(MatrixXcf(3, 3)) );
CALL_SUBTEST_5( array(MatrixXf(8, 12)) );
CALL_SUBTEST_6( array(MatrixXi(8, 12)) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( comparisons(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( comparisons(Matrix2f()) );
CALL_SUBTEST_3( comparisons(Matrix4d()) );
CALL_SUBTEST_5( comparisons(MatrixXf(8, 12)) );
CALL_SUBTEST_6( comparisons(MatrixXi(8, 12)) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( lpNorm(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( lpNorm(Vector2f()) );
CALL_SUBTEST_3( lpNorm(Vector3d()) );
CALL_SUBTEST_4( lpNorm(Vector4f()) );
CALL_SUBTEST_5( lpNorm(VectorXf(16)) );
CALL_SUBTEST_7( lpNorm(VectorXcd(10)) );
}
}
void test_array_for_matrix()
{
int maxsize = 40;
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( array_for_matrix(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( array_for_matrix(Matrix2f()) );
CALL_SUBTEST_3( array_for_matrix(Matrix4d()) );
CALL_SUBTEST_4( array_for_matrix(MatrixXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( array_for_matrix(MatrixXf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( array_for_matrix(MatrixXi(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( comparisons(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( comparisons(Matrix2f()) );
CALL_SUBTEST_3( comparisons(Matrix4d()) );
CALL_SUBTEST_5( comparisons(MatrixXf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( comparisons(MatrixXi(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( lpNorm(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( lpNorm(Vector2f()) );
CALL_SUBTEST_7( lpNorm(Vector3d()) );
CALL_SUBTEST_8( lpNorm(Vector4f()) );
CALL_SUBTEST_5( lpNorm(VectorXf(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_4( lpNorm(VectorXcf(internal::random<int>(1,maxsize))) );
}
}
void test_matrix_power()
{
CALL_SUBTEST_2(test2dRotation<double>(1e-13));
CALL_SUBTEST_1(test2dRotation<float>(2e-5)); // was 1e-5, relaxed for clang 2.8 / linux / x86-64
CALL_SUBTEST_9(test2dRotation<long double>(1e-13));
CALL_SUBTEST_2(test2dHyperbolicRotation<double>(1e-14));
CALL_SUBTEST_1(test2dHyperbolicRotation<float>(1e-5));
CALL_SUBTEST_9(test2dHyperbolicRotation<long double>(1e-14));
CALL_SUBTEST_10(test3dRotation<double>(1e-13));
CALL_SUBTEST_11(test3dRotation<float>(1e-5));
CALL_SUBTEST_12(test3dRotation<long double>(1e-13));
CALL_SUBTEST_2(testGeneral(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testGeneral(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testGeneral(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testGeneral(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testGeneral(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testGeneral(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testGeneral(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testGeneral(MatrixXf(2,2), 1e-3)); // see bug 614
CALL_SUBTEST_9(testGeneral(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testGeneral(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testGeneral(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testGeneral(Matrix3e(), 1e-13));
CALL_SUBTEST_2(testSingular(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testSingular(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testSingular(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testSingular(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testSingular(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testSingular(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testSingular(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testSingular(MatrixXf(2,2), 1e-3));
CALL_SUBTEST_9(testSingular(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testSingular(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testSingular(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testSingular(Matrix3e(), 1e-13));
CALL_SUBTEST_2(testLogThenExp(Matrix2d(), 1e-13));
CALL_SUBTEST_7(testLogThenExp(Matrix3dRowMajor(), 1e-13));
CALL_SUBTEST_3(testLogThenExp(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(testLogThenExp(MatrixXd(8,8), 2e-12));
CALL_SUBTEST_1(testLogThenExp(Matrix2f(), 1e-4));
CALL_SUBTEST_5(testLogThenExp(Matrix3cf(), 1e-4));
CALL_SUBTEST_8(testLogThenExp(Matrix4f(), 1e-4));
CALL_SUBTEST_6(testLogThenExp(MatrixXf(2,2), 1e-3));
CALL_SUBTEST_9(testLogThenExp(MatrixXe(7,7), 1e-13));
CALL_SUBTEST_10(testLogThenExp(Matrix3d(), 1e-13));
CALL_SUBTEST_11(testLogThenExp(Matrix3f(), 1e-4));
CALL_SUBTEST_12(testLogThenExp(Matrix3e(), 1e-13));
}
void UKFPose2D::lineSensorUpdate(bool vertical, const Vector2f& reading, const Matrix2f& readingCov)
{
generateSigmaPoints();
// computeLineReadings
Vector2f lineReadings[7];
if(vertical)
for(int i = 0; i < 7; ++i)
lineReadings[i] = Vector2f(sigmaPoints[i].y(), sigmaPoints[i].z());
else
for(int i = 0; i < 7; ++i)
lineReadings[i] = Vector2f(sigmaPoints[i].x(), sigmaPoints[i].z());
// computeMeanOfLineReadings
Vector2f lineReadingMean = lineReadings[0];
for(int i = 1; i < 7; ++i)
lineReadingMean += lineReadings[i];
lineReadingMean *= 1.f / 7.f;
// computeCovOfLineReadingsAndSigmaPoints
Matrix2x3f lineReadingAndMeanCov = Matrix2x3f::Zero();
for(int i = 0; i < 3; ++i)
{
const Vector2f d1 = lineReadings[i * 2 + 1] - lineReadingMean;
lineReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
const Vector2f d2 = lineReadings[i * 2 + 2] - lineReadingMean;
lineReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
}
lineReadingAndMeanCov *= 0.5f;
// computeCovOfLineReadingsReadings
const Vector2f d = lineReadings[0] - lineReadingMean;
Matrix2f lineReadingCov = (Matrix2f() << d * d.x(), d * d.y()).finished();
for(int i = 1; i < 7; ++i)
{
const Vector2f d = lineReadings[i] - lineReadingMean;
lineReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
}
lineReadingCov *= 0.5f;
lineReadingMean.y() = Angle::normalize(lineReadingMean.y());
const Matrix3x2f kalmanGain = lineReadingAndMeanCov.transpose() * (lineReadingCov + readingCov).inverse();
Vector2f innovation = reading - lineReadingMean;
innovation.y() = Angle::normalize(innovation.y());
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z() = Angle::normalize(mean.z());
cov -= kalmanGain * lineReadingAndMeanCov;
Covariance::fixCovariance(cov);
}
void test_redux()
{
// the max size cannot be too large, otherwise reduxion operations obviously generate large errors.
int maxsize = (std::min)(100,EIGEN_TEST_MAX_SIZE);
EIGEN_UNUSED_VARIABLE(maxsize);
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixRedux(Matrix<float, 1, 1>()) );
CALL_SUBTEST_1( matrixRedux(Array<float, 1, 1>()) );
CALL_SUBTEST_2( matrixRedux(Matrix2f()) );
CALL_SUBTEST_2( matrixRedux(Array2f()) );
CALL_SUBTEST_3( matrixRedux(Matrix4d()) );
CALL_SUBTEST_3( matrixRedux(Array4d()) );
CALL_SUBTEST_4( matrixRedux(MatrixXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_4( matrixRedux(ArrayXXcf(internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( matrixRedux(MatrixXd (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( matrixRedux(ArrayXXd (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( matrixRedux(MatrixXi (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
CALL_SUBTEST_6( matrixRedux(ArrayXXi (internal::random<int>(1,maxsize), internal::random<int>(1,maxsize))) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_7( vectorRedux(Vector4f()) );
CALL_SUBTEST_7( vectorRedux(Array4f()) );
CALL_SUBTEST_5( vectorRedux(VectorXd(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_5( vectorRedux(ArrayXd(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_8( vectorRedux(VectorXf(internal::random<int>(1,maxsize))) );
CALL_SUBTEST_8( vectorRedux(ArrayXf(internal::random<int>(1,maxsize))) );
}
}
void test_qtvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Vector2f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Matrix2f()));
CALL_SUBTEST(check_qtvector_matrix(Vector4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_qtvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_qtvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_qtvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_qtvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_qtvector_transform(Transform2f()));
CALL_SUBTEST(check_qtvector_transform(Transform3f()));
CALL_SUBTEST(check_qtvector_transform(Transform3d()));
//CALL_SUBTEST(check_qtvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
}
void test_stdvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Projective2f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3f()));
CALL_SUBTEST_4(check_stdvector_transform(Projective3d()));
//CALL_SUBTEST(heck_stdvector_transform(Projective4d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
void test_mapstaticmethods()
{
ptr = internal::aligned_new<float>(1000);
for(int i = 0; i < 1000; i++) ptr[i] = float(i);
const_ptr = ptr;
CALL_SUBTEST_1(( mapstaticmethods(Matrix<float, 1, 1>()) ));
CALL_SUBTEST_1(( mapstaticmethods(Vector2f()) ));
CALL_SUBTEST_2(( mapstaticmethods(Vector3f()) ));
CALL_SUBTEST_2(( mapstaticmethods(Matrix2f()) ));
CALL_SUBTEST_3(( mapstaticmethods(Matrix4f()) ));
CALL_SUBTEST_3(( mapstaticmethods(Array4f()) ));
CALL_SUBTEST_4(( mapstaticmethods(Array3f()) ));
CALL_SUBTEST_4(( mapstaticmethods(Array33f()) ));
CALL_SUBTEST_5(( mapstaticmethods(Array44f()) ));
CALL_SUBTEST_5(( mapstaticmethods(VectorXf(1)) ));
CALL_SUBTEST_5(( mapstaticmethods(VectorXf(8)) ));
CALL_SUBTEST_6(( mapstaticmethods(MatrixXf(1,1)) ));
CALL_SUBTEST_6(( mapstaticmethods(MatrixXf(5,7)) ));
CALL_SUBTEST_7(( mapstaticmethods(ArrayXf(1)) ));
CALL_SUBTEST_7(( mapstaticmethods(ArrayXf(5)) ));
CALL_SUBTEST_8(( mapstaticmethods(ArrayXXf(1,1)) ));
CALL_SUBTEST_8(( mapstaticmethods(ArrayXXf(8,6)) ));
internal::aligned_delete(ptr, 1000);
}
void test_stdvector_overload()
{
// some non vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Vector2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix3f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST_1(check_stdvector_matrix(Matrix2f()));
CALL_SUBTEST_1(check_stdvector_matrix(Vector4f()));
CALL_SUBTEST_1(check_stdvector_matrix(Matrix4f()));
CALL_SUBTEST_2(check_stdvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST_3(check_stdvector_matrix(VectorXd(20)));
CALL_SUBTEST_3(check_stdvector_matrix(RowVectorXf(20)));
CALL_SUBTEST_3(check_stdvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST_4(check_stdvector_transform(Affine2f())); // does not need the specialization (2+1)^2 = 9
CALL_SUBTEST_4(check_stdvector_transform(Affine3f()));
CALL_SUBTEST_4(check_stdvector_transform(Affine3d()));
// some Quaternion
CALL_SUBTEST_5(check_stdvector_quaternion(Quaternionf()));
CALL_SUBTEST_5(check_stdvector_quaternion(Quaterniond()));
}
Matrix2f TeamPlayersLocator::rotateCovariance(const Matrix2f& matrix, const float angle)
{
const float cosine = std::cos(angle);
const float sine = std::sin(angle);
const Matrix2f rotationMatrix = (Matrix2f() << cosine, -sine, sine, cosine).finished();
return (rotationMatrix * matrix) * rotationMatrix.transpose();
}
void test_eigensolver_selfadjoint()
{
int s = 0;
for(int i = 0; i < g_repeat; i++) {
// very important to test 3x3 and 2x2 matrices since we provide special paths for them
CALL_SUBTEST_1( selfadjointeigensolver(Matrix2f()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix2d()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3f()) );
CALL_SUBTEST_1( selfadjointeigensolver(Matrix3d()) );
CALL_SUBTEST_2( selfadjointeigensolver(Matrix4d()) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_3( selfadjointeigensolver(MatrixXf(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_5( selfadjointeigensolver(MatrixXcd(s,s)) );
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_9( selfadjointeigensolver(Matrix<std::complex<double>,Dynamic,Dynamic,RowMajor>(s,s)) );
// some trivial but implementation-wise tricky cases
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(1,1)) );
CALL_SUBTEST_4( selfadjointeigensolver(MatrixXd(2,2)) );
CALL_SUBTEST_6( selfadjointeigensolver(Matrix<double,1,1>()) );
CALL_SUBTEST_7( selfadjointeigensolver(Matrix<double,2,2>()) );
}
// Test problem size constructors
s = internal::random<int>(1,EIGEN_TEST_MAX_SIZE/4);
CALL_SUBTEST_8(SelfAdjointEigenSolver<MatrixXf> tmp1(s));
CALL_SUBTEST_8(Tridiagonalization<MatrixXf> tmp2(s));
TEST_SET_BUT_UNUSED_VARIABLE(s)
}
Matrix2f sampleJacobian(const PosType &x_) {
//check bounds
if(!insideBounds(x_))
return Matrix2f().setZero();
DataType dx, dy, vHi, vLo;
PosType pHi, pLo;
{
//dx derivative
pHi[0] = x_[0] + mHalfScaleFactor[0]; pHi[1] = x_[1];
pLo[0] = x_[0] - mHalfScaleFactor[0]; pLo[1] = x_[1];
vHi = sample(pHi),
vLo = sample(pLo);
if(!insideBounds(pHi)) {
//backward difference
dx = sample(x_);
dx = (dx - vLo) / mHalfScaleFactor[0];
} else if(!insideBounds(pLo)) {
//forward difference
dx = sample(x_);
dx = (vHi - dx) / mHalfScaleFactor[0];
} else {
//central difference
dx = (vHi - vLo) / (2*mHalfScaleFactor[0]);
}
}
{
//dy derivative
pHi[0] = x_[0]; pHi[1] = x_[1] + mHalfScaleFactor[1];
pLo[0] = x_[0]; pLo[1] = x_[1] - mHalfScaleFactor[1];
vHi = sample(pHi),
vLo = sample(pLo);
if(!insideBounds(pHi)) {
//backward difference
dy = sample(x_);
dy = (dy - vLo) / mHalfScaleFactor[1];
} else if(!insideBounds(pLo)) {
//forward difference
dy = sample(x_);
dy = (vHi - dy) / mHalfScaleFactor[1];
} else {
//central difference
dy = (vHi - vLo) / (2*mHalfScaleFactor[1]);
}
}
Matrix2f out;
out[0][0] = dx[0]; out[0][1] = dx[1];
out[1][0] = dy[0]; out[1][1] = dy[1];
return out;
}
const Matrix2f Matrix2f::inverse() const{
float det = determinant();
return Matrix2f(
data[1][1]/det,
-data[1][0]/det,
-data[0][1]/det,
data[0][0]/det
);
}
Matrix2f Matrix2f::transposed() const
{
return Matrix2f
(
( *this )( 0, 0 ), ( *this )( 1, 0 ),
( *this )( 0, 1 ), ( *this )( 1, 1 )
);
}
Matrix2f trans(){
Matrix2f res = Matrix2f(0,column,row);
res.setNum(nums[0][0],0,0);
res.setNum(nums[0][1],1,0);
res.setNum(nums[1][0],0,1);
res.setNum(nums[1][1],1,1);
return res;
}
Matrix2f mul(float b){
Matrix2f res = Matrix2f(0,row,column);
for(int i=0;i<row;i++){
for(int j=0;j<column;j++){
res.setNum(nums[i][j]*b, i,j);
}
}
return res;
}
// static
Matrix2f Matrix2f::rotation( float degrees )
{
float c = cos( degrees );
float s = sin( degrees );
return Matrix2f
(
c, -s,
s, c
);
}
void TeamPlayersLocator::update(TeamPlayersModel& teamPlayersModel)
{
teamPlayersModel.obstacles.clear();
std::vector<Obstacle> ownTeam;
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOpponentGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosLeftGoal), GOALPOST);
teamPlayersModel.obstacles.emplace_back(Matrix2f::Identity(), Vector2f(theFieldDimensions.xPosOwnGoalPost, theFieldDimensions.yPosRightGoal), GOALPOST);
if(theRobotInfo.penalty == PENALTY_NONE && theGroundContactState.contact)
{
ownTeam.emplace_back(theRobotPose.covariance.topLeftCorner(2, 2), theRobotPose.translation); // pose and covariance of the robot itself
if(theFallDownState.state == theFallDownState.upright) //why is this?
for(const auto& obstacle : theObstacleModel.obstacles)
{
if(obstacle.type == GOALPOST)
continue;
// if seen robots are of the opponent team and are not detected outside the field
const Vector2f p = theRobotPose * obstacle.center;
if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
teamPlayersModel.obstacles.emplace_back(obstacle.covariance, p, obstacle.type);
}
}
for(auto const& teammate : theTeammateData.teammates)
{
if(teammate.status == Teammate::FULLY_ACTIVE)
{
if(teammate.pose.deviation < 50.f) //todo, magic number will be replaced by parameter. 50.f might be too low
ownTeam.emplace_back(teammate.pose.covariance.topLeftCorner(2, 2), teammate.pose.translation); //position of teammates
for(const auto& obstacle : teammate.obstacleModel.obstacles)
{
if(obstacle.type == GOALPOST)
continue;
// if seen robots are of the opponent team and are not detected outside the field
const Vector2f p = teammate.pose * obstacle.center;
if(std::abs(p.x()) <= theFieldDimensions.xPosOpponentFieldBorder && std::abs(p.y()) <= theFieldDimensions.yPosLeftFieldBorder)
{
Matrix2f covariance = (Matrix2f() << obstacle.covXX, obstacle.covXY, obstacle.covXY, obstacle.covYY).finished();
Obstacle converted = Obstacle(rotateCovariance(covariance, teammate.pose.rotation), p, obstacle.type);
merge(converted, teamPlayersModel.obstacles);
}
}
}
}
for(auto& teammate : ownTeam)
{
removeAround(teammate, teamPlayersModel.obstacles);
}
}
//----------------------------------------------------------------------------
void AffineTransform (int warmups, Ellipse2f& ellipse0, Ellipse2f& ellipse1)
{
float random = 0.0f;
for (int i = 0; i < warmups; ++i)
{
random = Mathf::SymmetricRandom();
}
Matrix2f A;
Vector2f B;
for (int row = 0; row < 2; ++row)
{
for (int col = 0; col < 2; ++col)
{
A[row][col] = Mathf::SymmetricRandom();
}
B[row] = Mathf::SymmetricRandom();
}
if (A.Determinant() < 0.0f)
{
A[0][0] = -A[0][0];
A[0][1] = -A[0][1];
}
Matrix2f invA = A.Inverse();
Vector2f K = invA*(ellipse0.Center - B);
Matrix2f D(
1.0f/(ellipse0.Extent[0]*ellipse0.Extent[0]),
1.0f/(ellipse0.Extent[1]*ellipse0.Extent[1]));
Matrix2f N = A.TransposeTimes(D)*A;
Matrix2f R;
N.EigenDecomposition(R, D);
ellipse0.Center = K;
ellipse0.Axis[0] = R.GetColumn(0);
ellipse0.Axis[1] = R.GetColumn(1);
ellipse0.Extent[0] = Mathf::InvSqrt(D[0][0]);
ellipse0.Extent[1] = Mathf::InvSqrt(D[1][1]);
K = invA*(ellipse1.Center - B);
D = Matrix2f(
1.0f/(ellipse1.Extent[0]*ellipse1.Extent[0]),
1.0f/(ellipse1.Extent[1]*ellipse1.Extent[1]));
N = A.TransposeTimes(D)*A;
N.EigenDecomposition(R, D);
ellipse1.Center = K;
ellipse1.Axis[0] = R.GetColumn(0);
ellipse1.Axis[1] = R.GetColumn(1);
ellipse1.Extent[0] = Mathf::InvSqrt(D[0][0]);
ellipse1.Extent[1] = Mathf::InvSqrt(D[1][1]);
}
void test_eigen2_linearstructure()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( linearStructure(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( linearStructure(Matrix2f()) );
CALL_SUBTEST_3( linearStructure(Vector3d()) );
CALL_SUBTEST_4( linearStructure(Matrix4d()) );
CALL_SUBTEST_5( linearStructure(MatrixXcf(3, 3)) );
CALL_SUBTEST_6( linearStructure(MatrixXf(8, 12)) );
CALL_SUBTEST_7( linearStructure(MatrixXi(8, 12)) );
CALL_SUBTEST_8( linearStructure(MatrixXcd(20, 20)) );
}
}
void test_linearstructure()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( linearStructure(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( linearStructure(Matrix2f()) );
CALL_SUBTEST_3( linearStructure(Vector3d()) );
CALL_SUBTEST_4( linearStructure(Matrix4d()) );
CALL_SUBTEST_5( linearStructure(MatrixXcf(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2), internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2))) );
CALL_SUBTEST_6( linearStructure(MatrixXf (internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_7( linearStructure(MatrixXi (internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
CALL_SUBTEST_8( linearStructure(MatrixXcd(internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2), internal::random<int>(1,EIGEN_TEST_MAX_SIZE/2))) );
CALL_SUBTEST_9( linearStructure(ArrayXXf (internal::random<int>(1,EIGEN_TEST_MAX_SIZE), internal::random<int>(1,EIGEN_TEST_MAX_SIZE))) );
}
}
Matrix2f sub(Matrix2f b){
if(row != b.getColumn() || column != b.getRow()){
Util::myException("Matrix error\n\r");
}
Matrix2f tmp = Matrix2f();
for(int i=0;i<row;i++){
for(int j=0;j<column;j++){
tmp.setNum(nums[i][j] - b.getNum(i,j) , i,j);
}
}
return tmp;
}
//Matrix * Matrix
const Matrix2f Matrix2f::operator*(const Matrix2f& m) const{
Matrix2f out = Matrix2f(*this);
//Columns of m.data
for (int i=0; i<2; i++){
//Rows of data
for (int j=0; j<2; j++){
//Individual entries of each
out.data[i][j] = data[0][j]*m.data[i][0];
for (int k=1; k<2; k++)
out.data[i][j] += data[k][j]*m.data[i][k];
}
}
return out;
}
void UKFPose2D::landmarkSensorUpdate(const Vector2f& landmarkPosition, const Vector2f& reading, const Matrix2f& readingCov)
{
generateSigmaPoints();
// computeLandmarkReadings
Vector2f landmarkReadings[7];
for(int i = 0; i < 7; ++i)
{
Pose2f pose(sigmaPoints[i].z(), sigmaPoints[i].head<2>());
Vector2f landmarkPosRel = pose.invert() * landmarkPosition; // TODO: optimize this
landmarkReadings[i] = Vector2f(landmarkPosRel.x(), landmarkPosRel.y());
}
// computeMeanOfLandmarkReadings
Vector2f landmarkReadingMean = landmarkReadings[0];
for(int i = 1; i < 7; ++i)
landmarkReadingMean += landmarkReadings[i];
landmarkReadingMean *= 1.f / 7.f;
// computeCovOfLandmarkReadingsAndSigmaPoints
Matrix2x3f landmarkReadingAndMeanCov = Matrix2x3f::Zero();
for(int i = 0; i < 3; ++i)
{
const Vector2f d1 = landmarkReadings[i * 2 + 1] - landmarkReadingMean;
landmarkReadingAndMeanCov += (Matrix2x3f() << d1 * l(0, i), d1 * l(1, i), d1 * l(2, i)).finished();
const Vector2f d2 = landmarkReadings[i * 2 + 2] - landmarkReadingMean;
landmarkReadingAndMeanCov += (Matrix2x3f() << d2 * -l(0, i), d2 * -l(1, i), d2 * -l(2, i)).finished();
}
landmarkReadingAndMeanCov *= 0.5f;
// computeCovOfLandmarkReadingsReadings
const Vector2f d = landmarkReadings[0] - landmarkReadingMean;
Matrix2f landmarkReadingCov = Matrix2f::Zero();
landmarkReadingCov << d * d.x(), d * d.y();
for(int i = 1; i < 7; ++i)
{
const Vector2f d = landmarkReadings[i] - landmarkReadingMean;
landmarkReadingCov += (Matrix2f() << d * d.x(), d * d.y()).finished();
}
landmarkReadingCov *= 0.5f;
const Matrix3x2f kalmanGain = landmarkReadingAndMeanCov.transpose() * (landmarkReadingCov + readingCov).inverse();
Vector2f innovation = reading - landmarkReadingMean;
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z() = Angle::normalize(mean.z());
cov -= kalmanGain * landmarkReadingAndMeanCov;
Covariance::fixCovariance(cov);
}
Matrix2f inverse(){
if(row!=2 || column != 2){
Util::myException("Matrix error at inverse\n\r");
}
Matrix2f res = Matrix2f(0,2,2);
float det = (nums[0][0]*nums[1][1] - nums[0][1]*nums[1][0]);
res.setNum( nums[1][1]/det,0,0);
res.setNum(-nums[0][1]/det,0,1);
res.setNum(-nums[1][0]/det,1,0);
res.setNum( nums[0][0]/det,1,1);
return res;
}
void test_eigen2_sum()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixSum(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( matrixSum(Matrix2f()) );
CALL_SUBTEST_3( matrixSum(Matrix4d()) );
CALL_SUBTEST_4( matrixSum(MatrixXcf(3, 3)) );
CALL_SUBTEST_5( matrixSum(MatrixXf(8, 12)) );
CALL_SUBTEST_6( matrixSum(MatrixXi(8, 12)) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_5( vectorSum(VectorXf(5)) );
CALL_SUBTEST_7( vectorSum(VectorXd(10)) );
CALL_SUBTEST_5( vectorSum(VectorXf(33)) );
}
}
void test_eigen2_visitor()
{
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_1( matrixVisitor(Matrix<float, 1, 1>()) );
CALL_SUBTEST_2( matrixVisitor(Matrix2f()) );
CALL_SUBTEST_3( matrixVisitor(Matrix4d()) );
CALL_SUBTEST_4( matrixVisitor(MatrixXd(8, 12)) );
CALL_SUBTEST_5( matrixVisitor(Matrix<double,Dynamic,Dynamic,RowMajor>(20, 20)) );
CALL_SUBTEST_6( matrixVisitor(MatrixXi(8, 12)) );
}
for(int i = 0; i < g_repeat; i++) {
CALL_SUBTEST_7( vectorVisitor(Vector4f()) );
CALL_SUBTEST_4( vectorVisitor(VectorXd(10)) );
CALL_SUBTEST_4( vectorVisitor(RowVectorXd(10)) );
CALL_SUBTEST_8( vectorVisitor(VectorXf(33)) );
}
}
void test_matrix_exponential()
{
CALL_SUBTEST_2(test2dRotation<double>(1e-13));
CALL_SUBTEST_1(test2dRotation<float>(2e-5)); // was 1e-5, relaxed for clang 2.8 / linux / x86-64
CALL_SUBTEST_2(test2dHyperbolicRotation<double>(1e-14));
CALL_SUBTEST_1(test2dHyperbolicRotation<float>(1e-5));
CALL_SUBTEST_6(testPascal<float>(1e-6));
CALL_SUBTEST_5(testPascal<double>(1e-15));
CALL_SUBTEST_2(randomTest(Matrix2d(), 1e-13));
CALL_SUBTEST_7(randomTest(Matrix<double,3,3,RowMajor>(), 1e-13));
CALL_SUBTEST_3(randomTest(Matrix4cd(), 1e-13));
CALL_SUBTEST_4(randomTest(MatrixXd(8,8), 1e-13));
CALL_SUBTEST_1(randomTest(Matrix2f(), 1e-4));
CALL_SUBTEST_5(randomTest(Matrix3cf(), 1e-4));
CALL_SUBTEST_1(randomTest(Matrix4f(), 1e-4));
CALL_SUBTEST_6(randomTest(MatrixXf(8,8), 1e-4));
}
Matrix2f mul(Matrix2f b){
if(column != b.getRow()){
Util::myException("Matrix error\n\r");
}
Matrix2f res = Matrix2f(0,row,b.getColumn());
for(int i=0;i<row;i++){
for(int j=0;j<b.getColumn();j++){
for(int k=0;k<column;k++){
res.setNum(nums[i][k]*b.getNum(k,j)+res.getNum(i,j) , i,j);
}
}
}
return res;
}
