Exemplo n.º 1
0
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
               );
    }
}
Exemplo n.º 2
0
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)) );
  }
}
Exemplo n.º 3
0
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));
}
Exemplo n.º 5
0
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);
}
Exemplo n.º 6
0
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))) );
  }
}
Exemplo n.º 7
0
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()));
}
Exemplo n.º 8
0
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()));
}
Exemplo n.º 9
0
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);
}
Exemplo n.º 10
0
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();
}
Exemplo n.º 12
0
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)
}
Exemplo n.º 13
0
  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;
  }
Exemplo n.º 14
0
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
	);
}
Exemplo n.º 15
0
Matrix2f Matrix2f::transposed() const
{
    return Matrix2f
           (
               ( *this )( 0, 0 ), ( *this )( 1, 0 ),
               ( *this )( 0, 1 ), ( *this )( 1, 1 )
           );

}
Exemplo n.º 16
0
	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;
	}
Exemplo n.º 17
0
	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;
	}
Exemplo n.º 18
0
// 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]);
}
Exemplo n.º 21
0
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)) );
  }
}
Exemplo n.º 22
0
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))) );
  }
}
Exemplo n.º 23
0
	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;
	}
Exemplo n.º 24
0
//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;
}
Exemplo n.º 25
0
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);
}
Exemplo n.º 26
0
	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;
	}
Exemplo n.º 27
0
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)) );
  }
}
Exemplo n.º 28
0
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)) );
    }
}
Exemplo n.º 29
0
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));
}
Exemplo n.º 30
0
	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;
	}