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]);
}
}
