float LinearInterpolation (VectorXf X , VectorXf Y, float X_PointOfInterest){ //Produce Y_point_of_interest given X,Y and target X_point_of_interest //X : vector containing the X variables of the interpolant //Y : vector containing the Y variables of the interpolant //PointOfInterest : Point of X to estimate the new point of Y float xk, xkp1, yk, ykp1; //Points adjecent to the point of interpolation if ( X.size() != Y.size() ){cout << "Problem with vector sizes" << endl; return(-1);} //cout << " X(0): " << X(0) <<" X(Y.size()-1): " <<X(Y.size()-1) << " Point of interest: " << X_PointOfInterest<< endl; if ( X_PointOfInterest < X(0) || X_PointOfInterest > X(Y.size()-1) ){cout << "You interpolate out of the curve boundaries" << endl; return(-1);} //Find the points right before and right after the point of interest for (int i=1; i<X.size() ; i++){ if (X(i)>= X_PointOfInterest){ xkp1 = X(i); xk = X(i-1); ykp1 = Y(i); yk = Y(i-1); break;} } //point-slope form for a line formula float t = (X_PointOfInterest -xk)/(xkp1 -xk); float yPOI = (1-t) * yk + t * ykp1; // estimate point of interest // cout << "(" << xk << ", " << X_PointOfInterest << " , " << xkp1 << ") & (" << yk << ", " << yPOI << ", " << ykp1 << ")"<< endl; return (yPOI); }
void Layouter::laplacianEigMap( const MatrixXd& laplacian, const VectorXf& radiusVec, const VectorXi& hashID, MatrixXf& finalPos2D, float& finalRadius, float sparseFactor /*= 1.f*/ ) { if (laplacian.rows() <= 0 || laplacian.cols() <= 0 || radiusVec.size() <= 0) { finalPos2D = MatrixXf::Zero(1,2); finalRadius = 0; return; } if (laplacian.rows() == 1 && laplacian.cols() == 1 && radiusVec.size() == 1) { finalPos2D = MatrixXf::Zero(1,2); finalRadius = radiusVec[0]; return; } MatrixXd pos2D, finalPosd; LaplacianSolver::compute(laplacian, pos2D); VectorXd minPos, maxPos; MDSPostProcesser m_postProcessor(500, 1.0f, 1.0, 1.02, radiusVec.minCoeff()); m_postProcessor.setSparseFactor(sparseFactor); m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID); m_postProcessor.compute(); m_postProcessor.getFinalPos(finalPosd); m_postProcessor.getFinalBoundingRect(minPos, maxPos); finalPos2D = finalPosd.cast<float>(); // finalPos2D = pos2D.cast<float>(); // minPos = pos2D.colwise().minCoeff(); // maxPos = pos2D.colwise().maxCoeff(); // VectorXd size = maxPos - minPos; finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f; }
void NonRigid::computeInflateDir(VectorXf &p_vec, VectorXf &g_vec) { VectorXf arap_g = g_vec; VectorXf inflate_g = g_vec; VectorXf userCrsp_g = g_vec; double f_e = 0; computeArap(p_vec, arap_g); computeUserCrsp(p_vec, userCrsp_g); Vector3f cur_v(0,0,0); Vector3f cur_g(0,0,0); size_t P_Num = p_vec.size()/3; for (size_t i = 0; i < P_Num; ++i) { cur_v << p_vec(i + 0*P_Num), p_vec(i + 1*P_Num), p_vec(i + 2*P_Num); if (computeVertexGrad(cur_v, cur_g) > 0.0) { inflate_g(i + 0*P_Num) = cur_g(0); inflate_g(i + 1*P_Num) = cur_g(1); inflate_g(i + 2*P_Num) = cur_g(2); } else { inflate_g(i + 0*P_Num) = -P_Prime_N(0, i); inflate_g(i + 1*P_Num) = -P_Prime_N(1, i); inflate_g(i + 2*P_Num) = -P_Prime_N(2, i); } } g_vec = lamd_inflate*inflate_g + lamd_arap*arap_g + lamd_userCrsp*userCrsp_g; g_vec.normalize(); }
VectorXf param_sensitivity_widget::computeSensitivity( MatrixXf ¶meterMatrix, VectorXf &responseVector) { MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix; MatrixXf C; C = Ctemp.inverse(); VectorXf b = C*parameterMatrix.transpose()*responseVector; VectorXf Y_hat = parameterMatrix*b; int p = b.rows(); VectorXf sigma2Vec = responseVector-Y_hat; float sigma2 = sigma2Vec.squaredNorm(); sigma2= sigma2/(parameterMatrix.rows() - p); Ctemp = C*sigma2; MatrixXf denominator = Ctemp.diagonal(); // Do element-wise division VectorXf t = b; for (int i = 0; i < b.rows(); i++) { t(i) = abs(b(i)/sqrt(denominator(i))); } return t; }
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 PlaneFittingCloudOrienter::_compute() { assert(input_cloud_); assert(input_intensity_); assert(!output_cloud_); // -- Fit a plane. VectorXf a = fitPlane(*input_cloud_); // -- Rotate the points so that the direction of the best plane is the x axis. assert(fabs(a.norm() - 1) < 1e-4); double theta = M_PI/2. - atan2(a(1), a(0)); MatrixXf rotation = MatrixXf::Identity(3, 3); rotation(0,0) = cos(theta); rotation(1,1) = cos(theta); rotation(0,1) = sin(theta); rotation(1,0) = -sin(theta); output_cloud_ = shared_ptr<MatrixXf>(new MatrixXf()); *output_cloud_ = *input_cloud_ * rotation; VectorXf foo = fitPlane(*output_cloud_); //cout << "New plane: " << foo.transpose() << endl; // -- Subtract off the mean of the points. MatrixXf& points = *output_cloud_; VectorXf pt_mean = points.colwise().sum() / (float)points.rows(); for(int i=0; i<points.rows(); ++i) points.row(i) -= pt_mean.transpose(); }
void D3DRandomProjector::doComputation(const sensor_msgs::PointCloud& data, cloud_kdtree::KdTree& data_kdtree, const vector<const std::vector<int>*>& interest_region_indices, vector<vector<float> >& results) { assert(results.size() == interest_region_indices.size()); vvf orig_results; descriptor_->compute(data, data_kdtree, interest_region_indices, orig_results); //TODO: Make descriptors cache their results. results = vvf(orig_results.size()); for(size_t i=0; i<orig_results.size(); ++i) { if(orig_results[i].empty()) { results[i] = vector<float>(0); continue; } // -- Project the feature. VectorXf vec; floatToEigen(orig_results[i], &vec); VectorXf projected = projector_ * vec; // -- Put into results. results[i] = vector<float>(projected.rows()); for(size_t j=0; j<results[i].size(); ++j) { results[i][j] = projected(j); } } }
void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H) { VectorXf PHt = P*H.transpose(); MatrixXf S = H*PHt; S(0,0) += R; MatrixXf Si = S.inverse(); Si = make_symmetric(Si); MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt PSD_check.transpose(); PSD_check.conjugate(); VectorXf W = PHt*Si; x = x+W*v; //Joseph-form covariance update MatrixXf eye(P.rows(), P.cols()); eye.setIdentity(); MatrixXf C = eye - W*H; P = C*P*C.transpose() + W*R*W.transpose(); float eps = 2.2204*pow(10.0,-16); //numerical safety P = P+eye*eps; PSD_check = P.llt().matrixL(); PSD_check.transpose(); PSD_check.conjugate(); //for upper tri }
void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) { out.resize( in.rows(), in.cols() ); for( int i=0; i<in.cols(); i++ ){ VectorXf b = in.col(i); VectorXf q = Q.col(i); out.col(i) = b.array().sum()*q - b; } }
SeedFeature::SeedFeature( const ImageOverSegmentation & ios, const VectorXf & obj_param ) { Image rgb_im = ios.image(); const RMatrixXs & s = ios.s(); const int Ns = ios.Ns(), W = rgb_im.W(), H = rgb_im.H(); // Initialize various values VectorXf area = bin( s, 1, [&](int x, int y){ return 1.f; } ); VectorXf norm = (area.array()+1e-10).cwiseInverse(); pos_ = norm.asDiagonal() * bin( s, 6, [&](int i, int j){ float x=1.0*i/(W-1)-0.5,y=1.0*j/(H-1)-0.5; return makeArray<6>( x, y, x*x, y*y, fabs(x), fabs(y) ); } ); if (N_DYNAMIC_COL) { Image lab_im; rgb2lab( lab_im, rgb_im ); col_ = norm.asDiagonal() * bin( s, 6, [&](int x, int y){ return makeArray<6>( rgb_im(y,x, 0), rgb_im(y,x,1), rgb_im(y,x,2), lab_im(y,x,0), lab_im(y,x,1), lab_im(y,x,2) ); } ); } const int N_GEO = sizeof(EDGE_P)/sizeof(EDGE_P[0]); for( int i=0; i<N_GEO; i++ ) gdist_.push_back( GeodesicDistance(ios.edges(),ios.edgeWeights().array().pow(EDGE_P[i])+1e-3) ); // Compute the static features static_f_ = RMatrixXf::Zero( Ns, N_STATIC_F ); int o=0; // Add the position features static_f_.block( 0, o, Ns, N_STATIC_POS ) = pos_.leftCols( N_STATIC_POS ); o += N_STATIC_POS; // Add the geodesic features if( N_STATIC_GEO >= N_GEO ) { RMatrixXu8 bnd = findBoundary( s ); RMatrixXf mask = (bnd.array() == 0).cast<float>()*1e10; for( int i=0; i<N_GEO; i++ ) static_f_.col( o++ ) = gdist_[i].compute( mask ); for( int j=1; (j+1)*N_GEO<=N_STATIC_GEO; j++ ) { mask = (bnd.array() != j).cast<float>()*1e10; for( int i=0; i<N_GEO; i++ ) static_f_.col( o++ ) = gdist_[i].compute( mask ); } } if( N_STATIC_EDGE ) { RMatrixXf edge_map = DirectedSobel().detect( ios.image() ); for( int j=0; j<s.rows(); j++ ) for( int i=0; i<s.cols(); i++ ) { const int id = s(j,i); int bin = edge_map(j,i)*N_STATIC_EDGE; if ( bin < 0 ) bin = 0; if ( bin >= N_STATIC_EDGE ) bin = N_STATIC_EDGE-1; static_f_(id,o+bin) += norm[id]; } o += N_STATIC_EDGE; } if( N_OBJ_F>1 ) static_f_.col(o++) = (computeObjFeatures(ios)*obj_param).transpose(); // Initialize the dynamic features dynamic_f_ = RMatrixXf::Zero( Ns, N_DYNAMIC_F ); n_ = 0; min_dist_ = RMatrixXf::Ones(Ns,5)*10; var_ = RMatrixXf::Zero(Ns,6); }
int main(int, char**) { cout.precision(3); VectorXf v; v.setConstant(3, 5); cout << v << endl; return 0; }
int main(int, char**) { cout.precision(3); VectorXf v; v.setOnes(3); cout << v << endl; return 0; }
/////////////////////// ///// Inference ///// /////////////////////// void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) { out.resize( in.rows(), in.cols() ); for( int i=0; i<out.cols(); i++ ){ VectorXf b = in.col(i); b.array() -= b.maxCoeff(); b = b.array().exp(); out.col(i) = b / b.array().sum(); } }
Hamming::Hamming( const VectorXs & gt, float class_weight_pow ):gt_( gt ) { int M=0,N=gt.rows();; for( int i=0; i<N; i++ ) if( gt[i] >= M ) M = gt[i]+1; VectorXf cnt = VectorXf::Zero( M ); for( int i=0; i<N; i++ ) if( gt[i] >= 0 ) cnt[gt[i]] += 1; class_weight_ = cnt.array() / cnt.array().sum(); class_weight_ = class_weight_.array().pow( -class_weight_pow ); class_weight_ = class_weight_.array() / (cnt.array()*class_weight_.array()).sum(); }
VectorXf PBSeqWeightEstimator::calc_residue_weight(const vector<string>& msa, const int& idx) const { VectorXf s = VectorXf::Zero(dim); // number of times a particular residue appears char c; for (vector<string>::const_iterator pos = msa.begin(); pos != msa.end(); ++pos) { c = (*pos)[idx]; if (is_allowed(c)) s(abc_idx(c)) += 1; } double r = (double) (s.array() > 0).count(); // number of different residues VectorXf wt(dim); for (int k = 0; k < dim; ++k) { if (s(k) > 0) wt(k) = 1. / (r * s(k)); else wt(k) = 0; } return wt; }
VectorXi searchsorted(const VectorXf& x, const VectorXf& y) { // y(i-1) <= x(out(i)) < y(i) int nX = x.size(); int nY = y.size(); VectorXi out(nX); int iY=0; for (int iX=0; iX < nX; iX++) { while (iY < nY && x(iX) > y(iY)) iY++; out(iX) = iY; } return out; }
size_t random_index (const VectorXf & likes) { float total = likes.sum(); ASSERT_LT(0, total); while (true) { float t = random_unif(0, total); for (int i = 0, I = likes.size(); i < I; ++i) { t -= likes(i); if (t < 0) return i; } } }
MatrixX3f Surface::compute_normals(const MatrixX3f& rr, const MatrixX3i& tris) { printf("\tcomputing normals\n"); // first, compute triangle normals MatrixX3f r1(tris.rows(),3); MatrixX3f r2(tris.rows(),3); MatrixX3f r3(tris.rows(),3); for(qint32 i = 0; i < tris.rows(); ++i) { r1.row(i) = rr.row(tris(i, 0)); r2.row(i) = rr.row(tris(i, 1)); r3.row(i) = rr.row(tris(i, 2)); } MatrixX3f x = r2 - r1; MatrixX3f y = r3 - r1; MatrixX3f tri_nn(x.rows(),y.cols()); tri_nn.col(0) = x.col(1).cwiseProduct(y.col(2)) - x.col(2).cwiseProduct(y.col(1)); tri_nn.col(1) = x.col(2).cwiseProduct(y.col(0)) - x.col(0).cwiseProduct(y.col(2)); tri_nn.col(2) = x.col(0).cwiseProduct(y.col(1)) - x.col(1).cwiseProduct(y.col(0)); // Triangle normals and areas MatrixX3f tmp = tri_nn.cwiseProduct(tri_nn); VectorXf normSize = tmp.rowwise().sum(); normSize = normSize.cwiseSqrt(); for(qint32 i = 0; i < normSize.size(); ++i) if(normSize(i) != 0) tri_nn.row(i) /= normSize(i); MatrixX3f nn = MatrixX3f::Zero(rr.rows(), 3); for(qint32 p = 0; p < tris.rows(); ++p) { Vector3i verts = tris.row(p); for(qint32 j = 0; j < verts.size(); ++j) nn.row(verts(j)) = tri_nn.row(p); } tmp = nn.cwiseProduct(nn); normSize = tmp.rowwise().sum(); normSize = normSize.cwiseSqrt(); for(qint32 i = 0; i < normSize.size(); ++i) if(normSize(i) != 0) nn.row(i) /= normSize(i); return nn; }
QStringList param_sensitivity_widget::sortSensitivity(VectorXf &senVal, QStringList ¶mNames) { // We need to sort senVal and use the corresponding indices to sort // paramNames // Step 1: Convert senVal to vector of pairs std::vector<std::pair<float,int> > sortedSenVals; std::pair<float,int> senValuePair; for (unsigned int i = 1; i < senVal.rows(); ++i) { senValuePair.first = senVal(i); senValuePair.second = i-1; // We are ignoring first element sortedSenVals.push_back(senValuePair); } // Step 2: Sort this vector std::sort(sortedSenVals.begin(), sortedSenVals.end()); QStringList sortedParamNames; // Step 3: Rewrite the senVal and paramNames for (unsigned int i = 0; i < sortedSenVals.size(); ++i) { sortedParamNames.push_back(paramNames.at(sortedSenVals.at(i).second)); senVal[i] = sortedSenVals.at(i).first; } return sortedParamNames; }
int MonotonicityCheck(VectorXf X){ // evaluate vector monotonicity of vector X int N = X.size(); for (int i=0; i<(N-1) ; i++){ if ( X(i) >X (1+i) ) return (-1); } return (1); }
void test_return_by_value(int len) { VectorXf in; VectorXf in1; in.setRandom( len ); VectorXcf out1,out2; FFT<float> fft; fft.SetFlag(fft.HalfSpectrum ); fft.fwd(out1,in); out2 = fft.fwd(in); VERIFY( (out1-out2).norm() < test_precision<float>() ); in1 = fft.inv(out1); VERIFY( (in1-in).norm() < test_precision<float>() ); }
vector<int> Util::descending_order(VectorXf& values) { vector<int> indices(values.size()); for (int i = 0; i != indices.size(); ++i) indices[i] = i; DescentCompareIndicesByAnotherVectorValues comp(values); sort(indices.begin(), indices.end(), comp); return indices; }
float sparsify_absolute ( const VectorXf & dense, VectorSf & sparse, const Vector<float> & thresh) { ASSERT1_LE(0, min(thresh)); const int I = dense.size(); sparse.resize(I); float loss = 0; for (int i = 0; i < I; ++i) { const float dense_i = dense(i); const float abs_i = fabsf(dense_i); if (abs_i > thresh[i]) sparse.insert(i) = dense_i; else loss += abs_i; } sparse.finalize(); return loss; }
vector<T> interp(const VectorXf& xNew, const VectorXf& xOld, const vector<T>& yOld, T (*blend)(T,T,float)) { int nNew = xNew.size(); int nOld = xOld.size(); vector<T> yNew(nNew); VectorXi new2old = searchsorted(xNew, xOld); for (int iNew=0; iNew < nNew; iNew++) { int iOldAbove = new2old(iNew); if (iOldAbove == 0) yNew[iNew] = yOld[0]; else if (iOldAbove == nOld) yNew[iNew] = yOld[nOld-1]; else yNew[iNew] = blend(yOld[iOldAbove-1], yOld[iOldAbove], (xNew(iNew) - xOld(iOldAbove-1)) / (xOld(iOldAbove) - xOld(iOldAbove-1))); } return yNew; }
double NonRigid::computeArap(VectorXf &p_vec, VectorXf &g_vec) { VectorXf d_vec(VectorXf::Map(d_cur.data(), d_cur.cols()*d_cur.rows())); g_vec = L*p_vec - d_vec; return (0.5*p_vec.transpose()*L*p_vec - d_vec.transpose()*p_vec)(0, 0); }
MatrixXf Sphere::make_initial_simplex( const VectorXf &pars, float size ) { /* * Make the initial tetrahedron */ int npar = pars.size(); MatrixXf simplex = MatrixXf::Zero(npar+1,npar); simplex.rowwise() += pars.transpose(); for (int k = 1; k < npar+1; k++) { simplex(k,k-1) += size; } return simplex; }
void filter( MatrixXf & out, const MatrixXf & in, bool transpose ) const { // Read in the values if( ntype_ == NORMALIZE_SYMMETRIC || (ntype_ == NORMALIZE_BEFORE && !transpose) || (ntype_ == NORMALIZE_AFTER && transpose)) out = in*norm_.asDiagonal(); else out = in; // Filter if( transpose ) lattice_.compute( out, out, true ); else lattice_.compute( out, out ); // lattice_.compute( out.data(), out.data(), out.rows() ); // Normalize again if( ntype_ == NORMALIZE_SYMMETRIC || (ntype_ == NORMALIZE_BEFORE && transpose) || (ntype_ == NORMALIZE_AFTER && !transpose)) out = out*norm_.asDiagonal(); }
VectorXf fnvalspapi(VectorXf X, VectorXf Y, VectorXf X_target){ //evaluate Y_target for X_target given X and Y int N = X_target.size(); VectorXf rr(N); for (int i=0; i<N ; i++){ rr(i) = LinearInterpolation(X, Y, X_target(i)) ; } return(rr); }
double density (const VectorXf & x) { const size_t I = x.size(); double sum_x1 = 0; double sum_x2 = 0; const float * restrict x_ = x.data(); for (size_t i = 0; i < I; ++i) { double xi = x_[i]; sum_x1 += max(-xi,xi); sum_x2 += xi * xi; } return sqr(sum_x1) / sum_x2 / I; }
void LayoutSetting::getChildrenRadius( const QList<SymbolNode::Ptr>& childList, VectorXf& radiusVec ) { radiusVec.resize(childList.size()); for (int ithChild = 0; ithChild < childList.size(); ++ithChild) { const SymbolNode::Ptr& child = childList[ithChild]; getNodeRadius(child, radiusVec[ithChild]); } }