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_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()) ); }
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()) ); } }
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)) ); } }
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; }
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 }
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_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_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>() ); } }
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))) ); } }
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; }
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)) ); } }
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; } }
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; } }
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>()) ); } }
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)) ); } }
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; } }
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; }
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; }
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)) ); } }
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)); }
//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); }
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 }
VectorXf DenseCRF::unaryParameters() const { if( unary_ ) return unary_->parameters(); return VectorXf(); }