void igl::sort_triangles( const Eigen::PlainObjectBase<DerivedV> & V, const Eigen::PlainObjectBase<DerivedF> & F, Eigen::PlainObjectBase<DerivedFF> & FF, Eigen::PlainObjectBase<DerivedI> & I) { using namespace Eigen; using namespace igl; using namespace std; // Put model, projection, and viewport matrices into double arrays Matrix4d MV; Matrix4d P; glGetDoublev(GL_MODELVIEW_MATRIX, MV.data()); glGetDoublev(GL_PROJECTION_MATRIX, P.data()); if(V.cols() == 3) { Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime,4> hV; hV.resize(V.rows(),4); hV.block(0,0,V.rows(),V.cols()) = V; hV.col(3).setConstant(1); return sort_triangles(hV,F,MV,P,FF,I); }else { return sort_triangles(V,F,MV,P,FF,I); } }

void dmz::WeaponPluginGravityBullet::_store_speed ( const Handle ObjectHandle, const ObjectType &Type) { Float64 *ptr (_speedTable.lookup (Type.get_handle ())); if (!ptr) { ptr = new Float64 (_defaultSpeed); if (ptr && !_speedTable.store (Type.get_handle (), ptr)) { delete ptr; ptr = 0; } } if (ptr) { _objectTable.store (ObjectHandle, ptr); ObjectModule *objMod (get_object_module ()); if (objMod) { Matrix ori; objMod->lookup_orientation (ObjectHandle, _defaultHandle, ori); Vector vel (0.0, 0.0, -(*ptr)); ori.transform_vector (vel); objMod->store_velocity (ObjectHandle, _defaultHandle, vel); } } }

bool SVGLineElement::GetGeometryBounds( Rect* aBounds, const StrokeOptions& aStrokeOptions, const Matrix& aTransform) { float x1, y1, x2, y2; GetAnimatedLengthValues(&x1, &y1, &x2, &y2, nullptr); if (aStrokeOptions.mLineWidth <= 0) { *aBounds = Rect(aTransform * Point(x1, y1), Size()); aBounds->ExpandToEnclose(aTransform * Point(x2, y2)); return true; } if (aStrokeOptions.mLineCap == CapStyle::ROUND) { if (!aTransform.IsRectilinear()) { // TODO: handle this case. return false; } Rect bounds(Point(x1, y1), Size()); bounds.ExpandToEnclose(Point(x2, y2)); bounds.Inflate(aStrokeOptions.mLineWidth / 2.f); *aBounds = aTransform.TransformBounds(bounds); return true; } Float length = Float(NS_hypot(x2 - x1, y2 - y1)); Float xDelta; Float yDelta; if (aStrokeOptions.mLineCap == CapStyle::BUTT) { if (length == 0.f) { xDelta = yDelta = 0.f; } else { Float ratio = aStrokeOptions.mLineWidth / 2.f / length; xDelta = ratio * (y2 - y1); yDelta = ratio * (x2 - x1); } } else { MOZ_ASSERT(aStrokeOptions.mLineCap == CapStyle::SQUARE); if (length == 0.f) { xDelta = yDelta = aStrokeOptions.mLineWidth / 2.f; } else { Float ratio = aStrokeOptions.mLineWidth / 2.f / length; xDelta = yDelta = ratio * (fabs(y2 - y1) + fabs(x2 - x1)); } } Point points[4]; points[0] = Point(x1 - xDelta, y1 - yDelta); points[1] = Point(x1 + xDelta, y1 + yDelta); points[2] = Point(x2 + xDelta, y2 + yDelta); points[3] = Point(x2 - xDelta, y2 - yDelta); *aBounds = Rect(aTransform * points[0], Size()); for (uint32_t i = 1; i < 4; ++i) { aBounds->ExpandToEnclose(aTransform * points[i]); } return true; }

//--------------------------------------------------------------------------- Matrix invert(const Matrix& A) { Matrix y(A.numRows(), A.numRows(), false); // Make sure A is square! if (A.numRows() != A.numCols()) { std::cerr << "ERROR! A must be square matrix!\n"; return y; } const int N = A.numRows(); // Make a copy of A since it gets modified Matrix Acopy(A); // y.setSize(N,N); Matrix col(N,1); Matrix indx(N,1); double d; int i,j; ludcmp(Acopy,indx,d); for (j = 0; j < N; j++) { for (i = 0; i < N; i++) col(i,0) = 0.0; col(j,0) = 1.0; lubksb(Acopy,indx,col); for (i = 0; i < N; i++) y(i,j) = col(i,0); } std::cout.flush(); // Return result return y; }

void SegmentorOTSU::NdGrid (const VectorDouble& x, const VectorDouble& y, Matrix& xm, Matrix& ym ) { kkuint32 xLen = (kkuint32)x.size (); kkuint32 yLen = (kkuint32)y.size (); xm.ReSize (xLen, xLen); ym.ReSize (yLen, yLen); kkuint32 row, col; for (row = 0; row < xLen; ++row) { for (col = 0; col < yLen; ++col) xm[row][col] = x[row]; } for (row = 0; row < xLen; ++row) { for (col = 0; col < yLen; ++col) ym[row][col] = y[col]; } }

double BLSSS::log_model_prob(const Selector &g)const{ // borrowed from MLVS.cpp double num = vpri_->logp(g); if(num==BOOM::negative_infinity() || g.nvars() == 0) { // If num == -infinity then it is in a zero support point in the // prior. If g.nvars()==0 then all coefficients are zero // because of the point mass. The only entries remaining in the // likelihood are sums of squares of y[i] that are independent // of g. They need to be omitted here because they are omitted // in the non-empty case below. return num; } SpdMatrix ivar = g.select(pri_->siginv()); num += .5*ivar.logdet(); if(num == BOOM::negative_infinity()) return num; Vector mu = g.select(pri_->mu()); Vector ivar_mu = ivar * mu; num -= .5*mu.dot(ivar_mu); bool ok=true; ivar += g.select(suf().xtx()); Matrix L = ivar.chol(ok); if(!ok) return BOOM::negative_infinity(); double denom = sum(log(L.diag())); // = .5 log |ivar| Vector S = g.select(suf().xty()) + ivar_mu; Lsolve_inplace(L,S); denom-= .5*S.normsq(); // S.normsq = beta_tilde ^T V_tilde beta_tilde return num-denom; }

/* Saves matrix to file @param filename filename for the txt output @param mat matrix to be outputted */ void saveMatrix(const std::string filename, const Matrix& mat){ std::ofstream fOut(filename); if (!fOut) { std::cout << "Error opening file: " << filename << std::endl; exit(EXIT_FAILURE); } if(!(fOut << mat.getDimM() << " " << mat.getDimN() << std::endl)) { std::cout << "Error in writing matrix entries!" << std::endl; exit(EXIT_FAILURE); } for (int m = 0; m < mat.getDimM(); m++) { for (int n = 0; n < mat.getDimN(); n++) { if(!(fOut << mat(m, n) << std::endl)) { std::cout << "Error in writing matrix entries!" << std::endl; exit(EXIT_FAILURE); } } } fOut.close(); }

int main(int argc,char* argv[]) { Matrix mat(2,5); mat(1,0) = 2; mat(0,4) = 1; std::set<size_t> r; std::set<size_t> c; r.insert(0); // r.insert(1); c.insert(0); c.insert(4); Matrix m; m.blockFromMatrix(mat,r.begin(),r.end(),c.begin(),c.end()); std::cout<<"blocking test one:"<<std::endl<<m<<std::endl; m.rowBlockFromMatrix(mat,r.begin(),r.end()); std::cout<<"blocking test two:"<<std::endl<<m<<std::endl; m.columnBlockFromMatrix(mat,c.begin(),c.end()); std::cout<<"blocking test three :"<<std::endl<<m<<std::endl; Vector vec(5); vec[4] = 2.0; Vector v; v.blockFromVector(vec,c); std::cout<<vec.block(c)<<std::endl; std::cout<<v<<std::endl; }

//C = A^trA B^trB int MatMul(Matrix &C, int trA, Matrix &A, int trB, Matrix &B) { CBLAS_TRANSPOSE_t TransA = ((trA == 0) ? CblasNoTrans : CblasTrans); CBLAS_TRANSPOSE_t TransB = ((trB == 0) ? CblasNoTrans : CblasTrans); return gsl_blas_dgemm( TransA, TransB, 1.0, A.Ma(), B.Ma(), 0.0, C.Ma()); }

Matrix* Matrix::transpose(){ Matrix *rezult = new Matrix(size); for (int i = 0; i < size; i++) for (int j = 0; j < size; j++) rezult->setElement(i, j, matrix[j][i]); return rezult; }

const Matrix& ActuatorCorot::getInitialStiff(void) { // zero the matrix theMatrix->Zero(); // local stiffness matrix static Matrix kl(3,3); // material stiffness portion kl.Zero(); kl(0,0) = EA/L; // compute R'*kl*R static Matrix kg(3,3); kg.addMatrixTripleProduct(0.0, R, kl, 1.0); // copy stiffness into appropriate blocks in element stiffness int numDOF2 = numDOF/2; for (int i=0; i<numDIM; i++) { for (int j=0; j<numDIM; j++) { (*theMatrix)(i,j) = kg(i,j); (*theMatrix)(i,j+numDOF2) = -kg(i,j); (*theMatrix)(i+numDOF2,j) = -kg(i,j); (*theMatrix)(i+numDOF2,j+numDOF2) = kg(i,j); } } return *theMatrix; }

Matrix* Matrix::operator+(Matrix &matr){ Matrix *rezult = new Matrix(size); for (int i = 0; i < size; i++) for (int j = 0; j < size; j++) rezult->setElement(i, j, matrix[i][j] + matr.getElement(i, j)); return rezult; }

Matrix Assignment::RandomGenerate(size_t nrows, size_t ncols, int MAX, unsigned int _seed){ //accept new seed for random generator if(_seed != SEED) srand(_seed); else srand(this->seed); //update data members this->num_agents = nrows; this->num_tasks = ncols; //define a matrix Matrix matrix; matrix.resize(nrows); for(unsigned int i=0; i<nrows; i++) matrix[i].resize(ncols); //randomly generate for(unsigned int i=0; i<nrows; i++) for(unsigned int j=0; j<ncols; j++){ int rdm=rand()%MAX-1; if(rdm<0) rdm = 0; matrix[i][j].SetWeight(rdm); } return matrix; }

void DrawTargetSkia::Mask(const Pattern &aSource, const Pattern &aMask, const DrawOptions &aOptions) { MarkChanged(); AutoPaintSetup paint(mCanvas.get(), aOptions, aSource); SkPaint maskPaint; SetPaintPattern(maskPaint, aMask); SkLayerRasterizer *raster = new SkLayerRasterizer(); raster->addLayer(maskPaint); SkSafeUnref(paint.mPaint.setRasterizer(raster)); // Skia only uses the mask rasterizer when we are drawing a path/rect. // Take our destination bounds and convert them into user space to use // as the path to draw. SkPath path; path.addRect(SkRect::MakeWH(SkScalar(mSize.width), SkScalar(mSize.height))); Matrix temp = mTransform; temp.Invert(); SkMatrix mat; GfxMatrixToSkiaMatrix(temp, mat); path.transform(mat); mCanvas->drawPath(path, paint.mPaint); }

void CRSMatrix::full2CRS(Matrix &A){ int nz = 0; //count non-zero elements int idx = 0; //data index /* *** count number of non-zero entries */ for(int i=0;i<A.numRows();i++) for(int j=0;j<A.numCols();j++) if(A(i,j)!=0) nz++; /* *** initialize private variables */ _rows = A.numRows(); _cols = A.numCols(); _nonZeros = nz; delete[] _data; delete []_colindex; delete[]_rowptr; _data = new double[nz]; _colindex = new int[nz]; _rowptr = new int[_rows+1]; /* *** fill _rowptr,_colindex,_data */ for(int i=0;i<_rows;i++){ _rowptr[i] = idx; for(int j=0; j<_cols;j++){ if(A(i,j)!=0){ _data[idx] = A(i,j); _colindex[idx]= j; idx++; } } } _rowptr[_rows]= nz; }

void LinearSolver_LU::solve_(Matrix* A, Matrix* B, LinearSolverRecord *record, int incrImprov, bool preserveMatrix) { const int nbRows = B->getNbRows(), nbCols = B->getNbCols(); Matrix *X = record->sol; int* p = new int[nbRows]; double* v = new double[nbRows]; Matrix *LU; if (preserveMatrix) LU = new Matrix(A); else LU = A; double* const*const lu = LU->getMatrix(R_W_ACCESS); double* const*const b = B->getMatrix(R_W_ACCESS); double* const*const x = X->getMatrix(R_W_ACCESS); record->status = factorize_(nbRows, lu, p, v); forwardSubstitution_(nbRows, nbCols, lu, b, p, x); backSubstitution_(nbRows, nbCols, lu, x, p, x); delete []p; delete []v; if (preserveMatrix) delete LU; }

bool SRUKF::measurementUpdate(const Matrix& measurement, const Matrix& measurementNoise, const Matrix& predictedMeasurementSigmas, const Matrix& stateEstimateSigmas) { int numberOfSigmaPoints = stateEstimateSigmas.getn(); debug << "Predicted measurement sigmas:" << std::endl << predictedMeasurementSigmas; Matrix predictedMeasurement = CalculateMeanFromSigmas(predictedMeasurementSigmas); Matrix Mz(m_numStates,numberOfSigmaPoints, false); Matrix Mx(m_numStates,numberOfSigmaPoints, false); for(int i = 0; i < numberOfSigmaPoints; i++) { Mz.setCol(i, m_sqrtSigmaWeights[0][i] * (predictedMeasurementSigmas.getCol(i) - predictedMeasurement)); Mx.setCol(i, m_sqrtSigmaWeights[0][i] * (stateEstimateSigmas.getCol(i) - m_mean)); } Matrix Sz = horzcat(Mz,measurementNoise); Matrix Pxz = Mx*Mz.transp(); Matrix K = Pxz * Invert22(Sz*Sz.transp()); debug << "K:" << std::endl << K; m_mean = m_mean + K * (measurement - predictedMeasurement); m_sqrtCovariance = HT(horzcat(Mx-K*Mz,K*measurementNoise)); //m_covariance = HT(horzcat(sigmaPoints-m_mean*m_sigmaWeights - K*predictedObservationSigmas + // K*predictedObservation*m_sigmaWeights,K*measurementNoise)); return true; }

void CDiamond::Draw(CDC* pDC,CRect& rcInvalid) { if(m_nPtCount <= 0) return; Pen penDraw(m_crColor,(float)m_nWidth); penDraw.SetLineJoin(LineJoinRound); penDraw.SetDashStyle(m_nDashStyle); Graphics graph(pDC->m_hDC); graph.SetSmoothingMode(SmoothingModeAntiAlias); Matrix mat; mat.RotateAt(m_fAngle,m_ptCenter); graph.SetTransform(&mat); if(m_nPtCount <= 1) { return; } else { if(m_bFinish && (m_bSelected || m_bEdit)) { DrawHotShape(graph); } graph.ExcludeClip(&m_RgnErase); PointF* pPt = m_ptary.GetData(); graph.DrawPolygon(&penDraw,pPt,4); } }

bool solve(const_Matrix<T, Block0> b, Matrix<T, Block1> x) { typedef typename Block_layout<Block0>::order_type order_type; typedef typename Block_layout<Block0>::complex_type complex_type; typedef Layout<2, order_type, Stride_unit_dense, complex_type> data_LP; typedef Strided<2, T, data_LP, Local_map> block_type; assert(b.size(0) == length_); assert(b.size(0) == x.size(0) && b.size(1) == x.size(1)); Matrix<T, block_type> b_int(b.size(0), b.size(1)); assign_local(b_int, b); if (tr == mat_conj || (tr == mat_trans && Is_complex<T>::value) || (tr == mat_herm && !Is_complex<T>::value)) VSIP_IMPL_THROW(unimplemented( "LU solver (CVSIP backend) does not implement this transformation")); { Ext_data<block_type> b_ext(b_int.block()); cvsip::View<2,T,true> cvsip_b_int(b_ext.data(),0,b_ext.stride(0),b_ext.size(0), b_ext.stride(1),b_ext.size(1)); cvsip_b_int.block().admit(true); traits::lu_solve(lu_, tr, cvsip_b_int.ptr()); cvsip_b_int.block().release(true); } assign_local(x, b_int); return true; }

// Inliers: Detected corners are in green, reprojected ones are in red // Outliers: Detected corners are in yellow, reprojected ones are in blue vector<Image> visualizeReprojection(const Image &im1, const Image &im2, Matrix H, vector<Correspondance> & corr, const vector<bool> & ins) { // Initialize colors vector<float> red(3,0); vector<float> green(3,0); vector<float> blue(3,0); vector<float> yellow(3,0); red[0] = 1.0f; green[1]= 1.0f; blue[2] = 1.0f; yellow[0] = 1.0f; yellow[1] = 1.0f; vector<Point> detectedPts1In; vector<Point> projectedPts1In; vector<Point> detectedPts1Out; vector<Point> projectedPts1Out; vector<Point> detectedPts2In; vector<Point> projectedPts2In; vector<Point> detectedPts2Out; vector<Point> projectedPts2Out; for (int i = 0 ; i < (int) corr.size(); i++) { Point pt1 = corr[i].feature(0).point(); Point pt2 = corr[i].feature(1).point(); Matrix P1 = pt1.toHomogenousCoords(); Matrix P2 = pt2.toHomogenousCoords(); Matrix P2_proj = H.multiply(P1); Matrix P1_proj = H.inverse().multiply(P2); Point reproj1 = Point(P1_proj(0,0)/P1_proj(0,2), P1_proj(0,1)/P1_proj(0,2)); Point reproj2 = Point(P2_proj(0,0)/P2_proj(0,2), P2_proj(0,1)/P2_proj(0,2)); if (ins[i]) { // Inlier detectedPts1In.push_back(pt1); projectedPts1In.push_back(reproj1); detectedPts2In.push_back(pt2); projectedPts2In.push_back(reproj2); } else { // Outlier detectedPts1Out.push_back(pt1); projectedPts1Out.push_back(reproj1); detectedPts2Out.push_back(pt2); projectedPts2Out.push_back(reproj2); } } vector<Image> output; Image vim1(im1); Image vim2(im2); vim1 = visualizeCorners(im1, detectedPts1In,2, green); vim1 = visualizeCorners(vim1, projectedPts1In,1, red); vim1 = visualizeCorners(vim1, detectedPts1Out,2, yellow); vim1 = visualizeCorners(vim1, projectedPts1Out,1, blue); vim2 = visualizeCorners(im2, detectedPts2In,2, green); vim2 = visualizeCorners(vim2, projectedPts2In,1, red); vim2 = visualizeCorners(vim2, detectedPts2Out,2, yellow); vim2 = visualizeCorners(vim2, projectedPts2Out,1, blue); output.push_back(vim1); output.push_back(vim2); return output; }

//--------------------------------------------------------------------------- Matrix solveLinearSystem(const Matrix& A, const Matrix& b) { Matrix x; // Make sure A is square! if (A.numRows() != A.numCols()) { std::cerr << "ERROR! A must be square matrix!\n"; return x; } // Make sure b is a column vector with the same dimensions // as A if (b.numRows() != A.numRows()) { std::cerr << "ERROR! b must be a column vector with the same dimensions as square matrix A!\n"; return x; } // Make a copy of A since it gets modified Matrix Acopy(A); const int N = Acopy.numRows(); Matrix indx(N,1); double d; ludcmp(Acopy,indx,d); x = b; // x will contain solution lubksb(Acopy,indx,x); // Return solution column vector return x; }

ActionBase::ResultE Joint::intersectEnter(Action *action) { // Use parent class for trivial reject if(Inherited::intersect(action) == Action::Skip) return Action::Skip; // Need to check children IntersectAction *ia = dynamic_cast<IntersectAction *>(action); Matrix m = this->getMatrix(); m.mult(this->getJointTransformation()); m.invert(); Pnt3f pos; Vec3f dir; m.multFull(ia->getLine().getPosition (), pos); m.mult (ia->getLine().getDirection(), dir); Real32 length = dir.length(); if(length < TypeTraits<Real32>::getDefaultEps()) SWARNING << "Joint::intersectEnter: Near-zero scale!" << std::endl; ia->setLine(Line(pos, dir), ia->getMaxDist()); ia->scale (length ); return ActionBase::Continue; }

int main(int argc, char **argv) { if(argc<=1) throw std::runtime_error(TOSTRING("Usage : " << argv[0] << " <file> <indices_subset_file> <outfile>")); file = argv[1]; Matrix X; X.read(file); std::cout << X.height << "x" << X.width << "\n"; int nbrows = 0; FILE* f = fopen(argv[2], "r"); while(!feof(f)) { if(fgetc(f)=='\n') nbrows++; } fclose(f); Matrix Y(nbrows, X.width); std::cout << Y.height << "x" << Y.width << "\n"; f = fopen(argv[2], "r"); int i=0; while(!feof(f)) { int ind; fscanf(f, "%u", &ind); ind--; memcpy(&Y[X.width*(i++)],&X[X.width*ind],sizeof(float)*X.width); if(i==nbrows) break; } fclose(f); Y.write(argv[3]); std::cout << "ok\n"; }

NewActionTypes::ResultE Joint::intersectActorEnter( ActorBase::FunctorArgumentType &funcArg) { IntersectActor *pIA = dynamic_cast<IntersectActor *>( funcArg.getActor()); Matrix matrix = this->getMatrix(); Line transLine; Pnt3f pos; Vec3f dir; matrix.mult(this->getJointTransformation()); matrix.invert(); matrix.multFull(pIA->getRay().getPosition (), pos); matrix.mult (pIA->getRay().getDirection(), dir); transLine.setValue(pos, dir); pIA->beginEditState(); { pIA->setRay (transLine ); pIA->setScaleFactor(pIA->getScaleFactor() / dir.length()); } pIA->endEditState (); pIA->setupChildrenPriorities(); return NewActionTypes::Continue; }

// virtual Matrix* NominalToCategorical::filterLabels(Matrix& labels) { if(labels.cols() != 1) ThrowError("unexpected number of label dims"); Matrix* pOut = new Matrix(); pOut->setSize(0, m_labelVals); if(m_labelVals == 1) { for(size_t i = 0; i < labels.rows(); i++) { vector<double> row; row.push_back(labels[i][0]); pOut->copyRow(row); } } else { for(size_t i = 0; i < labels.rows(); i++) { vector<double> row; row.resize(m_labelVals, 0.0); row[(size_t)labels[i][0]] = 1.0; pOut->copyRow(row); } } return pOut; }

double fwd_1(Vector &pi, Matrix &P, const Matrix &logQ, const Vector &logd, const Vector &one) { /*---------------------------------------------------------------------- * Input: pi[0..S-1] is the conditionl distribution of h[t-1]|Y[t-1] * one[0..S-1] is a vector of 1's * logd[s] is logp(y[t] | model[s]) * logQ[0..S-1] is the log of the square transition * probability matrix (rows of Q sum to 1) * * Output: pi[0..S-1] is prob(h[t] | Y[t]) * P[0..S-1]^2 is prob(h[t-1], h[t] | Y[t]) * * Return: logp(y[t] | y[1]..y[t-1]) * --------------------------------------------------------------------*/ uint S = pi.size(); P = logQ; pi = log(pi); for (uint r = 0; r < S; ++r) P.row(r) += logd; // P(r,s) += logd[s] for (uint s = 0; s < S; ++s) P.col(s) += pi; // P(r,s) += pi[r] double m = max(P); P -= m; P.exp(); double nc = P.abs_norm(); P /= nc; pi = one * P; return m + log(nc); }

bool LinearRegression::FitLinearModel(Matrix & X, Vector & y){ Matrix Xt; Xt.Transpose(X); Matrix XtX; XtX.Product(Xt, X); if (!this->chol.TryDecompose(XtX)) return false; chol.Decompose(XtX); chol.Invert(); this->XtXinv = chol.inv; Vector tmp = y; tmp.Product(Xt, y); this->B.Product(this->XtXinv, tmp); // beta = (XtX)^{-1} Xt Y this->predict.Product(X, this->B); this->residuals = y; this->residuals.Subtract(this->predict); this->sigma2 = 0.0; for (int i = 0; i < this->residuals.Length(); i++){ sigma2 += (this->residuals[i]) * (this->residuals[i]); } sigma2 /= y.Length(); // MLE estimates of sigma2 this->covB = this->XtXinv; this->covB.Multiply(sigma2); return true; };

//--------------------------------------------------------------------------- void TEstimation::calculateMarginalDensitiesOfRetained(){ //ofstream fmfile; //fmfile.open("fmfile.txt"); if(calcObsPValue>mySimData->numUsedSims) calcObsPValue=mySimData->numUsedSims; cout << " - calculating the marginal density for " << calcObsPValue << " used sims to get the p-value of the observed data." << endl; fmFromRetained=new float[mySimData->numUsedSims]; Matrix D = SigmaS+CHat*SigmaTheta*CHat.t(); Matrix D_inv=D.i(); ColumnVector theta_j, m_j, simStats; for(int s=1; s<=calcObsPValue;++s){ simStats=mySimData->statMatrix.column(s); double f_M=0.0; //loop over all dirac peaks for(int d=1; d<=mySimData->numUsedSims;++d){ theta_j=mySimData->paramMatrix.submatrix(d,d,2,mySimData->numParams+1).as_column(); m_j=c_zero+CHat*theta_j; Matrix temp=-0.5*(simStats-m_j).t()*D_inv*(simStats-m_j); f_M+=exp(temp(1,1)); } f_M=f_M/(mySimData->numUsedSims*sqrt((2*3.14159265358979323846*D).determinant())); fmFromRetained[s-1]=f_M*((double)mySimData->numUsedSims/(double)mySimData->numReadSims); // fmfile << fmFromRetained[s-1] << endl; } //fmfile.close(); }

bool GyroRatePerceptor::Percept(boost::shared_ptr<PredicateList> predList) { Predicate &predicate = predList->AddPredicate(); predicate.name = "GYR"; predicate.parameter.Clear(); ParameterList &nameElement = predicate.parameter.AddList(); nameElement.AddValue(std::string("n")); nameElement.AddValue(GetName()); Matrix invRot = mBody->GetRotation(); invRot.InvertRotationMatrix(); Vector3f rate = invRot * mBody->GetAngularVelocity(); ParameterList &ratesElement = predicate.parameter.AddList(); ratesElement.AddValue(std::string("rt")); ratesElement.AddValue(gRadToDeg(rate.x())); ratesElement.AddValue(gRadToDeg(rate.y())); ratesElement.AddValue(gRadToDeg(rate.z())); // What should be done when yrotate is around 90? in that case, the parameters of the atan2 are 0! // const dReal* q = dBodyGetQuaternion(mBody->GetODEBody()); // float xrotate = gArcTan2(2*(q[0]*q[1]+q[2]*q[3]), 1-2*(q[1]*q[1]+q[2]*q[2])); // float yrotate = gArcSin(2*(q[0]*q[2] - q[3]*q[1])); // float zrotate = gArcTan2(2*(q[0]*q[3] + q[1]*q[2]), 1-2*(q[2]*q[2]+q[3]*q[3])); // // ParameterList & anglesElement = predicate.parameter.AddList(); // anglesElement.AddValue(std::string("ang")); // anglesElement.AddValue(gRadToDeg(xrotate)); // anglesElement.AddValue(gRadToDeg(yrotate)); // anglesElement.AddValue(gRadToDeg(zrotate)); return true; }

void FixedContactEKFFlexEstimatorIMU::setFlexibilityGuess(const Matrix & x) { bool bstate =ekf_.checkStateVector(x); bool b6= (x.rows()==6 && x.cols()==1); bool bhomogeneous = (x.rows()==4 && x.cols()==4); (void)b6;//avoid warning BOOST_ASSERT((bstate||b6||bhomogeneous) && "ERROR: The flexibility state has incorrect size \ must be 18x1 vector, 6x1 vector or 4x4 matrix"); Vector x0=x; if (bstate) { ekf_.setState(x0,k_); } else { if (bhomogeneous) x0=kine::homogeneousMatrixToVector6(x); Vector x_s = ekf_.stateVectorZero(); x_s.segment(indexes::pos,3)=x0.head(3); x_s.segment(indexes::ori,3)=x0.tail(3); ekf_.setState(x_s,k_); ekf_.setQ(Q_); } }