Пример #1
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);
}
Пример #2
0
void test_nullary()
{
  CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
  CALL_SUBTEST_2( testMatrixType(MatrixXcf(50,50)) );
  CALL_SUBTEST_3( testMatrixType(MatrixXf(5,7)) );
  CALL_SUBTEST_4( testVectorType(VectorXd(51)) );
  CALL_SUBTEST_5( testVectorType(VectorXd(41)) );
  CALL_SUBTEST_6( testVectorType(Vector3d()) );
  CALL_SUBTEST_7( testVectorType(VectorXf(51)) );
  CALL_SUBTEST_8( testVectorType(VectorXf(41)) );
  CALL_SUBTEST_9( testVectorType(Vector3f()) );
}
Пример #3
0
void test_nullary()
{
  CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
  CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
  CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
  
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
    CALL_SUBTEST_5( testVectorType(VectorXd(internal::random<int>(1,300))) );
    CALL_SUBTEST_6( testVectorType(Vector3d()) );
    CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
    CALL_SUBTEST_8( testVectorType(VectorXf(internal::random<int>(1,300))) );
    CALL_SUBTEST_9( testVectorType(Vector3f()) );
  }
}
Пример #4
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)) );
  }
}
Пример #5
0
VectorXf AutoEncoder::Calculate(int index)
{
    /*
     *  Description:
     *  Calculate the i-th samples by feedforward and store hiddenvector
     *  in the row i of hidden matrix, output in row i of output matrix
     *
     *  @return outputVector: The output of FF
     */
    
    VectorXf HiddenVector = Weight_encode * OriginalData->row(index).transpose() + Bias_encode;
    for (int i = 0; i < HiddenVector.size(); i++)
    {
        HiddenVector(i) = sigmoid(HiddenVector(i));
    }
    HiddenMatrix.row(index) = HiddenVector.transpose();

    VectorXf output_vector = VectorXf(IO_dim);
    output_vector = Weight_decode * HiddenVector + Bias_decode;
    for (int i = 0; i < output_vector.size(); i++)
    {
        output_vector(i) = sigmoid(output_vector(i));
    }
    OutputMatrix.row(index) = output_vector.transpose();
    
    return output_vector;
}
Пример #6
0
void test_nullary()
{
  CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
  CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
  CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
  
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
    CALL_SUBTEST_5( testVectorType(Vector4d()) );  // regression test for bug 232
    CALL_SUBTEST_6( testVectorType(Vector3d()) );
    CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
    CALL_SUBTEST_8( testVectorType(Vector3f()) );
    CALL_SUBTEST_8( testVectorType(Vector4f()) );
    CALL_SUBTEST_8( testVectorType(Matrix<float,8,1>()) );
    CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );

    CALL_SUBTEST_9( testVectorType(VectorXi(internal::random<int>(1,300))) );
    CALL_SUBTEST_9( testVectorType(Matrix<int,1,1>()) );
  }

#ifdef EIGEN_TEST_PART_6
  // Assignment of a RowVectorXd to a MatrixXd (regression test for bug #79).
  VERIFY( (MatrixXd(RowVectorXd::LinSpaced(3, 0, 1)) - RowVector3d(0, 0.5, 1)).norm() < std::numeric_limits<double>::epsilon() );
#endif
}
Пример #7
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))) );
  }
}
Пример #8
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))) );
  }
}
Пример #9
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)) );
  }
}
void test_mapped_matrix()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( map_class_vector(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_1( check_const_correctness(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( map_class_vector(Vector4d()) );
    CALL_SUBTEST_2( map_class_vector(VectorXd(13)) );
    CALL_SUBTEST_2( check_const_correctness(Matrix4d()) );
    CALL_SUBTEST_3( map_class_vector(RowVector4f()) );
    CALL_SUBTEST_4( map_class_vector(VectorXcf(8)) );
    CALL_SUBTEST_5( map_class_vector(VectorXi(12)) );
    CALL_SUBTEST_5( check_const_correctness(VectorXi(12)) );

    CALL_SUBTEST_1( map_class_matrix(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( map_class_matrix(Matrix4d()) );
    CALL_SUBTEST_11( map_class_matrix(Matrix<float,3,5>()) );
    CALL_SUBTEST_4( map_class_matrix(MatrixXcf(internal::random<int>(1,10),internal::random<int>(1,10))) );
    CALL_SUBTEST_5( map_class_matrix(MatrixXi(internal::random<int>(1,10),internal::random<int>(1,10))) );

    CALL_SUBTEST_6( map_static_methods(Matrix<double, 1, 1>()) );
    CALL_SUBTEST_7( map_static_methods(Vector3f()) );
    CALL_SUBTEST_8( map_static_methods(RowVector3d()) );
    CALL_SUBTEST_9( map_static_methods(VectorXcd(8)) );
    CALL_SUBTEST_10( map_static_methods(VectorXf(12)) );
    
    CALL_SUBTEST_11( map_not_aligned_on_scalar<double>() );
  }
}
Пример #11
0
void test_stable_norm()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( stable_norm(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( stable_norm(Vector4d()) );
    CALL_SUBTEST_3( stable_norm(VectorXd(internal::random<int>(10,2000))) );
    CALL_SUBTEST_4( stable_norm(VectorXf(internal::random<int>(10,2000))) );
    CALL_SUBTEST_5( stable_norm(VectorXcd(internal::random<int>(10,2000))) );
  }
}
Пример #12
0
FeatureModel::FeatureModel(	const CamModel & _camera,
								const MotionModel & _motion,
								const FeaturesState & _featuresState,
								const Mat & frame,
								const Point2f & pf,
								int patchSize,
								float rho_0,
								float sigma_rho):
								camera(_camera),
								motion(_motion),
								features(_featuresState)
	{
    Mat newPatch(cv::Mat(frame, cv::Rect(pf.x-patchSize/2, pf.y-patchSize/2, patchSize,patchSize)));
    this->imageLocation << pf.x, pf.y;

    this->Patch = newPatch.clone();


    Vector2f hd = (Vector2f << (float)pf.x, (float)pf.y);

    // TODO: convert using motionmodel
    Vector3f r = motion->get_r();
    Quatenionf q = motion->get_q();


    Vector3f hC = this->camera.unproject(hd, true);
    Matrix3f Jac_hCHd = this->camera.getUnprojectionJacobian();

    Matrix3f Rot = quat2rot(q);

    Vector3f hW = Rot*hC;

    float hx = hW(0);
    float hy = hW(1);
    float hz = hW(2);



    float theta = atan2(hx,hz);
    float phi = atan2(-hy, sqrt(hx*hx+hz*hz));

	// Updating state and Sigma
    MatrixXf Jx = this->computeJx();
	MatrixXf Jm = this->computeJm();
	Matrix2f Sigma_mm = this->computeSigma_mm(sigma_pixel, sigma_pixel);

	this->f = (VectorXf(6) << r, theta, phi, rho_0);
	this->Sigma_ii = Jm*Sigma_mm*Jm.transpose() + Jx*motion->getSigma_xx()*Jx.transpose();

	this->motion->updateVariaceBlocks(Jx);
	this->features->updateVariaceBlocks(Jx);
	this->features.push_back((*this));
    return 1;

}
Пример #13
0
void test_array_replicate()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( replicate(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( replicate(Vector2f()) );
    CALL_SUBTEST_3( replicate(Vector3d()) );
    CALL_SUBTEST_4( replicate(Vector4f()) );
    CALL_SUBTEST_5( replicate(VectorXf(16)) );
    CALL_SUBTEST_6( replicate(VectorXcd(10)) );
  }
}
Пример #14
0
	virtual VectorXf parameters() const {
		if (ktype_ == CONST_KERNEL)
			return VectorXf();
		else if (ktype_ == DIAG_KERNEL)
			return parameters_;
		else {
			MatrixXf p = parameters_;
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}
Пример #15
0
	virtual VectorXf gradient( const MatrixXf & a, const MatrixXf & b ) const {
		if (ktype_ == CONST_KERNEL)
			return VectorXf();
		MatrixXf fg = featureGradient( a, b );
		if (ktype_ == DIAG_KERNEL)
			return (f_.array()*fg.array()).rowwise().sum();
		else {
			MatrixXf p = fg*f_.transpose();
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}
Пример #16
0
void test_nullary()
{
  CALL_SUBTEST_1( testMatrixType(Matrix2d()) );
  CALL_SUBTEST_2( testMatrixType(MatrixXcf(internal::random<int>(1,300),internal::random<int>(1,300))) );
  CALL_SUBTEST_3( testMatrixType(MatrixXf(internal::random<int>(1,300),internal::random<int>(1,300))) );
  
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_4( testVectorType(VectorXd(internal::random<int>(1,300))) );
    CALL_SUBTEST_5( testVectorType(Vector4d()) );  // regression test for bug 232
    CALL_SUBTEST_6( testVectorType(Vector3d()) );
    CALL_SUBTEST_7( testVectorType(VectorXf(internal::random<int>(1,300))) );
    CALL_SUBTEST_8( testVectorType(Vector3f()) );
    CALL_SUBTEST_8( testVectorType(Matrix<float,1,1>()) );
  }
}
Пример #17
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)) );
    }
}
Пример #18
0
SortVector::SortVector(const VectorXf& vector, const SortType& type)
{
    mSize = vector.size();
    for (int i = 0; i < mSize; i++) {
        mStdVector.push_back(ElementWithIndex(vector(i), i));
    }
    if (type == ascend) {
        sort(mStdVector.begin(), mStdVector.end(), ascendComparator);
    } else {
        sort(mStdVector.begin(), mStdVector.end(), descendComparator);
    }
    mIndices = VectorXi(mSize);
    mVector = VectorXf(mSize);
    for (int i = 0; i < mSize; i++) {
        mVector(i) = mStdVector[i].first;
        mIndices(i) = mStdVector[i].second;
    }
}
Пример #19
0
void AutoEncoder::BackPropagate(int index, double etaLearningRate)
{
    /*
     *  Description:
     *  BP in the SAE and considering the cross-entropy cost
     *
     *  @param etaLearningRate: Step to convergence
     *
     */
    
    VectorXf delta_3th = OriginalData->row(index) - OutputMatrix.row(index) ;
    for (int i = 0; i < delta_3th.size(); i++)
    {
        delta_3th(i) = DSIGMOID(OutputMatrix(index, i)) * delta_3th(i);
    }

    VectorXf diff_2th = Weight_decode.transpose() * delta_3th;

    VectorXf delta_2th = VectorXf(diff_2th.size());
    /* If need sparse restriction */
    if (hidden_units >= IO_dim)
    {
        for (int i = 0; i < delta_2th.size(); i++)
        {
            double d_sp = -st_sparsity / Sparsity_average(i) +
            (1 - st_sparsity) / (1 - Sparsity_average(i));
            
            delta_2th(i) = DSIGMOID(HiddenMatrix(index, i)) * (diff_2th(i) + d_sp);
        }
    }
    else
    {
        for (int i = 0; i < delta_2th.size(); i++)
        {
            delta_2th(i) = DSIGMOID(HiddenMatrix(index, i)) * diff_2th(i);
        }
    }

    Weight_decode += etaLearningRate * delta_3th * HiddenMatrix.row(index);
    Weight_encode += etaLearningRate * delta_2th * OriginalData->row(index);
    Bias_decode += etaLearningRate * delta_3th;
    Bias_encode += etaLearningRate * delta_2th;
}
Пример #20
0
void CMA_ES_Population::init(const Configuration& configuration)
{
  this->configuration = configuration;

  n = static_cast<unsigned>(configuration.initialization.size());
  sigma = configuration.sigma;
  stopfitness = configuration.stopFitness;
  stopeval = 1e3f * std::pow(static_cast<float>(n), 2.f);
  lambda_ = 4 + static_cast<unsigned>(floorf(3.f * std::log(static_cast<float>(n))));
  mu = lambda_ / 2.f;
  weights = VectorXf(static_cast<unsigned>(mu));
  for(int i = 1; i <= weights.size(); i++)
    weights[i - 1] = std::log(mu + 0.5f) - std::log(static_cast<float>(i));
  mu = floorf(mu);
  weights /= weights.sum();
  mueff = std::pow(weights.sum(), 2.f) / weights.array().pow(2).sum();
  cc = (4 + mueff / static_cast<float>(n)) / (static_cast<float>(n) + 4.f + 2.f * mueff / static_cast<float>(n));
  cs = (mueff + 2.f) / (static_cast<float>(n) + mueff + 5.f);
  c1 = 2.f / (std::pow(static_cast<float>(n) + 1.3f, 2.f) + mueff);
  cmu = 2.f * (mueff - 2.f + 1.f / mueff) / (std::pow(static_cast<float>(n) + 1.f, 2.f) + 2.f * mueff / 2.f);
  damps = 1.f + 2.f * std::max(0.f, std::sqrt((mueff - 1.f) / (static_cast<float>(n) + 1.f)) - 1.f) + cs;
  pc = MatrixXf::Zero(n, 1);
  ps = MatrixXf::Zero(n, 1);
  B = D = MatrixXf::Identity(n, n);
  C = B* D* (B* D).transpose();
  chiN = std::pow(static_cast<float>(n), 0.5f) * (1.f - 1.f / (4.f * static_cast<float>(n)) + 1.f / (21.f * std::pow(static_cast<float>(n), 2.f)));

  xmean = Eigen::Map<const Eigen::VectorXf>(configuration.initialization.data(), n);
  for(unsigned i = 0; i < n; i++)
  {
    xmean[i] = configuration.limits[i].limit(xmean[i]);
    xmean[i] = std::atanh((xmean[i] - configuration.limits[i].min) / (configuration.limits[i].max - configuration.limits[i].min) * 2.f - 1.f);
    xmean[i] = Rangef(-3.f, 3.f).limit(xmean[i]);
  }

  total = lambda_;
  numFeatures = n;
  fitness.resize(total);
  updateIndividuals();
  initialized = true;
  somethingChanged = true;
}
Пример #21
0
void test_eigen2_map()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( map_class_vector(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( map_class_vector(Vector4d()) );
    CALL_SUBTEST_3( map_class_vector(RowVector4f()) );
    CALL_SUBTEST_4( map_class_vector(VectorXcf(8)) );
    CALL_SUBTEST_5( map_class_vector(VectorXi(12)) );

    CALL_SUBTEST_1( map_class_matrix(Matrix<float, 1, 1>()) );
    CALL_SUBTEST_2( map_class_matrix(Matrix4d()) );
    CALL_SUBTEST_6( map_class_matrix(Matrix<float,3,5>()) );
    CALL_SUBTEST_4( map_class_matrix(MatrixXcf(ei_random<int>(1,10),ei_random<int>(1,10))) );
    CALL_SUBTEST_5( map_class_matrix(MatrixXi(ei_random<int>(1,10),ei_random<int>(1,10))) );

    CALL_SUBTEST_1( map_static_methods(Matrix<double, 1, 1>()) );
    CALL_SUBTEST_2( map_static_methods(Vector3f()) );
    CALL_SUBTEST_7( map_static_methods(RowVector3d()) );
    CALL_SUBTEST_4( map_static_methods(VectorXcd(8)) );
    CALL_SUBTEST_5( map_static_methods(VectorXf(12)) );
  }
}
Пример #22
0
void test_matrix_power()
{
  typedef Matrix<double,3,3,RowMajor>         Matrix3dRowMajor;
  typedef Matrix<long double,Dynamic,Dynamic> MatrixXe;
  typedef Matrix<long double,Dynamic,1>       VectorXe;

  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_2(testMatrixVector(Matrix2d(),         Vector2d(),    1e-13));
  CALL_SUBTEST_7(testMatrixVector(Matrix3dRowMajor(), MatrixXd(3,5), 1e-13));
  CALL_SUBTEST_3(testMatrixVector(Matrix4cd(),        Vector4cd(),   1e-13));
  CALL_SUBTEST_4(testMatrixVector(MatrixXd(8,8),      VectorXd(8),   2e-12));
  CALL_SUBTEST_1(testMatrixVector(Matrix2f(),         Vector2f(),    1e-4));
  CALL_SUBTEST_5(testMatrixVector(Matrix3cf(),        Vector3cf(),   1e-4));
  CALL_SUBTEST_8(testMatrixVector(Matrix4f(),         Vector4f(),    1e-4));
  CALL_SUBTEST_6(testMatrixVector(MatrixXf(8,8),      VectorXf(8),   1e-3));
  CALL_SUBTEST_9(testMatrixVector(MatrixXe(7,7),      VectorXe(7),   1e-13));
}
Пример #23
0
//compute proposal distribution, then sample from it, and compute new particle weight
void sample_proposal(Particle &particle, vector<VectorXf> z, vector<int> idf, MatrixXf R)
{
    assert(isfinite(particle.w()));
    VectorXf xv = VectorXf(particle.xv()); //robot position
    MatrixXf Pv = MatrixXf(particle.Pv()); //controls (motion command)

    VectorXf xv0 = VectorXf(xv);
    MatrixXf Pv0 = MatrixXf(Pv);	

    vector<MatrixXf> Hv;
    vector<MatrixXf> Hf;
    vector<MatrixXf> Sf;
    vector<VectorXf> zp;

    VectorXf zpi;
    MatrixXf Hvi;
    MatrixXf Hfi;
    MatrixXf Sfi;

    //process each feature, incrementally refine proposal distribution
    unsigned i,r;
    vector<int> j;
    for (i =0; i<idf.size(); i++) {
        j.clear();
        j.push_back(idf[i]);

        Hv.clear();
        Hf.clear();
        Sf.clear();
        zp.clear();

        compute_jacobians(particle,j,R,zp,&Hv,&Hf,&Sf);

        zpi = zp[0];
        Hvi = Hv[0];
        Hfi = Hf[0];
        Sfi = Sf[0];

        VectorXf vi = z[i] - zpi;
        vi[1] = pi_to_pi(vi[1]);

        //proposal covariance
        Pv = Hvi.transpose() * Sfi * Hvi + Pv.inverse();
        Pv = Pv.inverse();

        //proposal mean
        xv = xv + Pv * Hvi.transpose() * Sfi * vi;
        particle.setXv(xv);
        particle.setPv(Pv); 
    }

    //sample from proposal distribution
    VectorXf xvs = multivariate_gauss(xv,Pv,1); 
    particle.setXv(xvs);
    MatrixXf zeros(3,3);
    zeros.setZero();
    particle.setPv(zeros);

    //compute sample weight: w = w* p(z|xk) p(xk|xk-1) / proposal
    float like = likelihood_given_xv(particle, z, idf, R);
    float prior = gauss_evaluate(delta_xv(xv0,xvs), Pv0,0);
    float prop = gauss_evaluate(delta_xv(xv,xvs),Pv,0);
    assert(isfinite(particle.w()));

    float a = prior/prop;
    float b = particle.w() * a;
    float newW = like * b;
    //float newW = particle.w() * like * prior / prop;
    #if 0
    if (!isfinite(newW)) {
	cout<<"LIKELIHOOD GIVEN XV INPUTS"<<endl;   
	cout<<"particle.w()"<<endl;
	cout<<particle.w()<<endl;
	cout<<"particle.xv()"<<endl;
	cout<<particle.xv()<<endl;
	
	cout<<"particle.Pv()"<<endl;
	cout<<particle.Pv()<<endl;
	cout<<"particle.xf"<<endl;
	for (int i =0; i<particle.xf().size(); i++) {
	    cout<<particle.xf()[i]<<endl;
	}
	cout<<endl;
	cout<<"particle.Pf()"<<endl;
	for (int i =0; i< particle.Pf().size(); i++) {
	    cout<<particle.Pf()[i]<<endl;
	}
	cout<<endl;
	
	cout<<"z"<<endl;
	for (int i=0; i<z.size(); i++) {
	    cout<<z[i]<<endl;
	}   
	cout<<endl;
	
	cout<<"idf"<<endl;
	for (int i =0; i<idf.size(); i++){
	    cout<<idf[i]<<" ";
	}		
	cout<<endl;

	cout<<"R"<<endl;
	cout<<R<<endl;

	cout<<"GAUSS EVALUATE INPUTS"<<endl;
	cout<<"delta_xv(xv0,xvs)"<<endl;	
	cout<<delta_xv(xv0,xvs)<<endl;
	
	cout<<"Pv0"<<endl;
	cout<<Pv0<<endl;
	
	cout<<"delta_xv(xv,xvs)"<<endl;	
	cout<<delta_xv(xv,xvs)<<endl;
	
	cout<<"Pv"<<endl;
	cout<<Pv<<endl;
		
	
	cout<<"like"<<endl;
	cout<<like<<endl;
	cout<<"prior"<<endl;
	cout<<prior<<endl;
	cout<<"prop"<<endl;
	cout<<prop<<endl;
    }
    #endif
    particle.setW(newW);
} 
Пример #24
0
void test_vectorization_logic()
{

#ifdef EIGEN_VECTORIZE

#ifdef  EIGEN_DEFAULT_TO_ROW_MAJOR
  VERIFY(test_assign(Vector4f(),Vector4f(),
    LinearVectorization,CompleteUnrolling));
  VERIFY(test_assign(Vector4f(),Vector4f()+Vector4f(),
    LinearVectorization,CompleteUnrolling));
  VERIFY(test_assign(Vector4f(),Vector4f().cwise() * Vector4f(),
    LinearVectorization,CompleteUnrolling));
#else
  VERIFY(test_assign(Vector4f(),Vector4f(),
    InnerVectorization,CompleteUnrolling));
  VERIFY(test_assign(Vector4f(),Vector4f()+Vector4f(),
    InnerVectorization,CompleteUnrolling));
  VERIFY(test_assign(Vector4f(),Vector4f().cwise() * Vector4f(),
    InnerVectorization,CompleteUnrolling));
#endif

  VERIFY(test_assign(Matrix4f(),Matrix4f(),
    InnerVectorization,CompleteUnrolling));
  VERIFY(test_assign(Matrix4f(),Matrix4f()+Matrix4f(),
    InnerVectorization,CompleteUnrolling));
  VERIFY(test_assign(Matrix4f(),Matrix4f().cwise() * Matrix4f(),
    InnerVectorization,CompleteUnrolling));

  VERIFY(test_assign(Matrix<float,16,16>(),Matrix<float,16,16>()+Matrix<float,16,16>(),
    InnerVectorization,InnerUnrolling));

  VERIFY(test_assign(Matrix<float,16,16,DontAlign>(),Matrix<float,16,16>()+Matrix<float,16,16>(),
    NoVectorization,InnerUnrolling));

  VERIFY(test_assign(Matrix<float,6,2>(),Matrix<float,6,2>().cwise() / Matrix<float,6,2>(),
    LinearVectorization,CompleteUnrolling));

  VERIFY(test_assign(Matrix<float,17,17>(),Matrix<float,17,17>()+Matrix<float,17,17>(),
    NoVectorization,InnerUnrolling));

  VERIFY(test_assign(Matrix<float,4,4>(),Matrix<float,17,17>().block<4,4>(2,3)+Matrix<float,17,17>().block<4,4>(10,4),
    NoVectorization,CompleteUnrolling));

  VERIFY(test_assign(MatrixXf(10,10),MatrixXf(20,20).block(10,10,2,3),
    SliceVectorization,NoUnrolling));

  VERIFY(test_assign(VectorXf(10),VectorXf(10)+VectorXf(10),
    LinearVectorization,NoUnrolling));

  VERIFY(test_sum(VectorXf(10),
    LinearVectorization,NoUnrolling));

  VERIFY(test_sum(Matrix<float,5,2>(),
    NoVectorization,CompleteUnrolling));
  
  VERIFY(test_sum(Matrix<float,6,2>(),
    LinearVectorization,CompleteUnrolling));

  VERIFY(test_sum(Matrix<float,16,16>(),
    LinearVectorization,NoUnrolling));

  VERIFY(test_sum(Matrix<float,16,16>().block<4,4>(1,2),
    NoVectorization,CompleteUnrolling));

#ifndef EIGEN_DEFAULT_TO_ROW_MAJOR
  VERIFY(test_sum(Matrix<float,16,16>().block<8,1>(1,2),
    LinearVectorization,CompleteUnrolling));
#endif

  VERIFY(test_sum(Matrix<double,7,3>(),
    NoVectorization,CompleteUnrolling));

#endif // EIGEN_VECTORIZE

}
Пример #25
0
VectorXf DenseCRF::unaryParameters() const {
	if( unary_ )
		return unary_->parameters();
	return VectorXf();
}