template < class ValueType > Vector < ValueType > _mult(const Matrix< ValueType > & M, const Vector < ValueType > & b) { Index cols = M.cols(); Index rows = M.rows(); Vector < ValueType > ret(rows, 0.0); //ValueType tmpval = 0; if (b.size() == cols){ for (Index i = 0; i < rows; ++i){ ret[i] = sum(M.mat_[i] * b); // for (Index j = 0; j < cols; j++){ // ret[i] += M.mat_[i][j] * b[j]; // } } } else { throwLengthError(1, WHERE_AM_I + " " + toStr(cols) + " != " + toStr(b.size())); } return ret; }
performance curvy_step(const Matrix &F, Matrix &D){ performance data={0,0,0}; Matrix FD = F * D; data.multiplies++; Matrix Eanti = FD - FD.transpose(); data.adds++; Matrix Esymm = FD + FD.transpose(); data.adds++; Matrix X = Eigen::MatrixXd::Zero(F.cols(), F.rows()); conjugategradient(F,X,D,Esymm,Eanti,data); double scale = 0.001/X.lpNorm<Eigen::Infinity>(); X = scale * X; std::size_t rotation_order = 2; BCH_rotate(D, X, rotation_order, data); Purify(D, data); return data; }
template<typename Scalar> void initSPD(double density, Matrix<Scalar,Dynamic,Dynamic>& refMat, SparseMatrix<Scalar>& sparseMat) { Matrix<Scalar,Dynamic,Dynamic> aux(refMat.rows(),refMat.cols()); initSparse(density,refMat,sparseMat); refMat = refMat * refMat.adjoint(); for (int k=0; k<2; ++k) { initSparse(density,aux,sparseMat,ForceNonZeroDiag); refMat += aux * aux.adjoint(); } sparseMat.setZero(); for (int j=0 ; j<sparseMat.cols(); ++j) for (int i=j ; i<sparseMat.rows(); ++i) if (refMat(i,j)!=Scalar(0)) sparseMat.insert(i,j) = refMat(i,j); sparseMat.finalize(); }
void Cholesky<Matrix>::doCompute( const Matrix& mat ) { if (!validation(mat)) { std::cerr<<"Warning in cholesky solver: the input matrix is not symmetric. Please confirm that!"<<std::endl; return; } clear(); __a = new scalar[mat.size()]; this->setRowNum(mat.rows()); this->setColNum(mat.cols()); this->setLDA(mat.lda()); blas::copt_blas_copy(mat.size(),mat.dataPtr(),1,__a,1); copt_lapack_potrf('U',this->rowNum(),__a,this->lda(),&__info); if( __info != 0 ) { std::cout<<__info<<std::endl; std::cerr<<"Warning in Cholesky solver: something computation is wrong!"<<std::endl; } }
TEST_P (RandomWalkerTest, GetPotentials) { Matrix p; std::map<Color, size_t> map; pcl::segmentation::randomWalker (g.graph, boost::get (boost::edge_weight, g.graph), boost::get (boost::vertex_color, g.graph), p, map); ASSERT_EQ (g.size, p.rows ()); ASSERT_EQ (g.colors.size (), p.cols ()); ASSERT_EQ (g.colors.size (), map.size ()); for (std::set<Color>::iterator it = g.colors.begin (); it != g.colors.end (); ++it) for (size_t i = 0; i < g.size; ++i) if (g.potentials.count (*it)) EXPECT_NEAR (g.potentials[*it] (i), p (i, map[*it]), 0.01); }
void Matrix <T> ::row ( const Matrix <T> &M, const unsigned int r ) { //Matrix dimemension invalid, throw exception if ( r > rows_count ) { throw ErrorIndexOutOfBound ( "ErrorIndexOutOfBound: ", " row index exceeds rows_count. " ); } //Matrix dimemension invalid, throw exception if ( M.cols() != columns_count ) { throw ErrorMathMatrixDifferentSize <Matrix <T> > ( "ErrorMathMatrixDifferentSize: ", " invalid dimension of the matrices (columns_count count). ", *this, M ); } //Copy row for ( unsigned int i = 0; i < columns_count; i++ ) { items[r][i] = M ( 0, i ); } }
Scalar minDistanceSquared( const Gaussian<Scalar, NDims>& model, const Matrix<Scalar, NDims, Dynamic>& bag, int* index ) { Scalar minDistance = numeric_limits<Scalar>::infinity(); int minIndex = -1; for (int p = 0; p < bag.cols(); p++) { Matrix<Scalar, NDims, 1> point(bag.col(p)); Scalar distance = model.distanceSquared(point); if (distance < minDistance) { minDistance = distance; minIndex = p; } } if (index) { *index = minIndex; } return minDistance; }
void getPotentials (Matrix& potentials, std::map<Color, size_t>& color_to_column_map) { using namespace boost; potentials = Matrix::Zero (num_vertices (g_), colors_.size ()); // Copy over rows from X for (int i = 0; i < X.rows (); ++i) potentials.row (L_vertex_bimap.left.at (i)).head (X.cols ()) = X.row (i); // In rows that correspond to seeds put ones in proper columns for (size_t i = 0; i < seeds_.size (); ++i) { VertexDescriptor v = seeds_[i]; insertInBimap (B_color_bimap, color_map_[v]); potentials (seeds_[i], B_color_bimap.right.at (color_map_[seeds_[i]])) = 1; } // Fill in a map that associates colors with columns in potentials matrix color_to_column_map.clear (); for (int i = 0; i < potentials.cols (); ++i) color_to_column_map[B_color_bimap.left.at (i)] = i; }
Matrix yarp::math::SE3inv(const Matrix &H) { yAssert((H.rows()==4) && (H.cols()==4)); Vector p(4); p[0]=H(0,3); p[1]=H(1,3); p[2]=H(2,3); p[3]=1.0; Matrix invH=H.transposed(); p=invH*p; invH(0,3)=-p[0]; invH(1,3)=-p[1]; invH(2,3)=-p[2]; invH(3,0)=invH(3,1)=invH(3,2)=0.0; return invH; }
Vector yarp::math::dcm2axis(const Matrix &R) { yAssert((R.rows()>=3) && (R.cols()>=3)); Vector v(4); v[0]=R(2,1)-R(1,2); v[1]=R(0,2)-R(2,0); v[2]=R(1,0)-R(0,1); v[3]=0.0; double r=yarp::math::norm(v); double theta=atan2(0.5*r,0.5*(R(0,0)+R(1,1)+R(2,2)-1)); if (r<1e-9) { // if we enter here, then // R is symmetric; this can // happen only if the rotation // angle is 0 (R=I) or 180 degrees Matrix A=R.submatrix(0,2,0,2); Matrix U(3,3), V(3,3); Vector S(3); // A=I+sin(theta)*S+(1-cos(theta))*S^2 // where S is the skew matrix. // Given a point x, A*x is the rotated one, // hence if Ax=x then x belongs to the rotation // axis. We have therefore to find the kernel of // the linear application (A-I). SVD(A-eye(3,3),U,S,V); v[0]=V(0,2); v[1]=V(1,2); v[2]=V(2,2); r=yarp::math::norm(v); } v=(1.0/r)*v; v[3]=theta; return v; }
veDouble rowStd(const Matrix& M, const veDouble& avg) { uint m = M.rows(); uint n = M.cols(); if (n < 3) return(veDouble(m, 0.0)); veDouble stddev; for (uint j=0; j<m; ++j) { double x = 0.0; for (uint i=0; i<n; ++i) { double d = M(j, i) - avg[j]; x += d*d; } stddev.push_back(sqrt(x/(n-1.0))); } return(stddev); }
void Matrix<T>::submatrix(int sr, int sc, Matrix<T> &a) { int rwz,coz,i,j; if ( this->rows() % a.rows() != 0 || this->cols() % a.cols() != 0 || this->rows() < a.rows() || this->cols() < a.cols() ) { #ifdef USE_EXCEPTION throw WrongSize2D(this->rows(),this->cols(),a.rows(),a.cols()) ; #else Error error("Matrix<T>::submatrix"); error << "Matrix and submatrix incommensurate" ; error.fatal() ; #endif } if ( sr >= this->rows()/a.rows() || sr < 0 || sc >= this->cols()/a.cols() || sc < 0 ) { #ifdef USE_EXCEPTION throw OutOfBound2D(sr,sc,0,this->rows()/a.rows()-1,0,this->cols()/a.cols()-1) ; #else Error error("Matrix<T>::submatrix"); error << "Submatrix location out of bounds.\nrowblock " << sr << ", " << rows()/a.rows() << " colblock " << sc << ", " << a.cols() << endl ; error.fatal() ; #endif } rwz = sr*a.rows(); coz = sc*a.cols(); #ifdef COLUMN_ORDER for ( i = a.rows()-1; i >= 0; --i ) for(j=a.cols()-1;j>=0;--j) this->elem(i+rwz,j+coz) = a(i,j) ; #else T *ptr, *aptr ; aptr = a.m - 1; for ( i = a.rows()-1; i >= 0; --i ) { ptr = &m[(i+rwz)*cols()+coz]-1 ; for ( j = a.cols(); j > 0; --j) *(++ptr) = *(++aptr) ; } #endif }
Matrix gramschmidt( const Matrix & A ) { Matrix Q = A; // First vector just gets normalized Q.col(0).normalize(); for(unsigned int j = 1; j < A.cols(); ++j) { // Replace inner loop over each previous vector in Q with fast matrix-vector multiplication Q.col(j) -= Q.leftCols(j) * (Q.leftCols(j).transpose() * A.col(j)); // Normalize vector if possible (othw. means colums of A almsost lin. dep. if( Q.col(j).norm() <= 10e-14 * A.col(j).norm() ) { std::cerr << "Gram-Schmidt failed because A has lin. dep columns. Bye." << std::endl; break; } else { Q.col(j).normalize(); } } return Q; }
// check *above the diagonal* for non-zero entries boost::optional<Vector> checkIfDiagonal(const Matrix M) { size_t m = M.rows(), n = M.cols(); // check all non-diagonal entries bool full = false; size_t i, j; for (i = 0; i < m; i++) if (!full) for (j = i + 1; j < n; j++) if (fabs(M(i, j)) > 1e-9) { full = true; break; } if (full) { return boost::none; } else { Vector diagonal(n); for (j = 0; j < n; j++) diagonal(j) = M(j, j); return diagonal; } }
typename PointMesher<T>::Matrix PointMesher<T>::ITMLocalMesher::delaunay2D(const Matrix matrixIn) const { typedef CGAL::Exact_predicates_inexact_constructions_kernel K; typedef CGAL::Triangulation_vertex_base_with_info_2<int, K> Vb; typedef CGAL::Triangulation_face_base_2<K> Fb; typedef CGAL::Triangulation_data_structure_2<Vb, Fb> Tds; typedef CGAL::Delaunay_triangulation_2<K, Tds> DelaunayTri; typedef DelaunayTri::Finite_faces_iterator Finite_faces_iterator; typedef DelaunayTri::Vertex_handle Vertex_handle; typedef DelaunayTri::Point Point; // Compute triangulation DelaunayTri dt; for (int i = 0; i < matrixIn.cols(); i++) { // Add point Point p = Point(matrixIn(0, i), matrixIn(1, i)); dt.push_back(Point(p)); // Add index Vertex_handle vh = dt.push_back(p); vh->info() = i; } // Iterate through faces Matrix matrixOut(3, dt.number_of_faces()); int itCol = 0; DelaunayTri::Finite_faces_iterator it; for (it = dt.finite_faces_begin(); it != dt.finite_faces_end(); it++) { for (int k = 0; k < matrixOut.rows(); k++) { // Add index of triangle vertex to list matrixOut(k, itCol) = it->vertex(k)->info(); } itCol++; } return matrixOut; }
Matrix<ValueType> evaluateLocalBasis(const Space<double>& space, const Entity<0>& element, const Matrix<double>& local, const Vector<ValueType>& localCoefficients) { if (local.rows() != space.grid()->dim()) throw std::invalid_argument("evaluate(): points in 'local' have an " "invalid number of coordinates"); const int nComponents = space.codomainDimension(); Matrix<ValueType> values(nComponents, local.cols()); values.setZero(); // Find out which basis data need to be calculated size_t basisDeps = 0, geomDeps = 0; // Find out which geometrical data need to be calculated, const Fiber::CollectionOfShapesetTransformations<double> &transformations = space.basisFunctionValue(); transformations.addDependencies(basisDeps, geomDeps); // Get basis data const Fiber::Shapeset<double> &shapeset = space.shapeset(element); Fiber::BasisData<double> basisData; shapeset.evaluate(basisDeps, local, ALL_DOFS, basisData); // Get geometrical data Fiber::GeometricalData<double> geomData; element.geometry().getData(geomDeps, local, geomData); // Get shape function values Fiber::CollectionOf3dArrays<double> functionValues; transformations.evaluate(basisData, geomData, functionValues); // Calculate grid function values for (size_t p = 0; p < functionValues[0].extent(2); ++p) for (size_t f = 0; f < functionValues[0].extent(1); ++f) for (size_t dim = 0; dim < functionValues[0].extent(0); ++dim) values(dim, p) += functionValues[0](dim, f, p) * localCoefficients(f); return values; }
bool CalibModule::getGazeParams(const string &eye, const string &type, Matrix &M) { if (((eye!="left") && (eye!="right")) || ((type!="intrinsics") && (type!="extrinsics"))) return false; Bottle info; igaze->getInfo(info); if (Bottle *pB=info.find(("camera_"+type+"_"+eye).c_str()).asList()) { M.resize((type=="intrinsics")?3:4,4); int cnt=0; for (int r=0; r<M.rows(); r++) for (int c=0; c<M.cols(); c++) M(r,c)=pB->get(cnt++).asDouble(); return true; } else return false; }
// ----------------------------------------------------------------------------------- //------------------------------------------------------------------------------------ // Compute the azimuth and nadir angle, in the satellite body frame, // of receiver Position RX as seen at the satellite Position SV. The nadir angle // is measured from the Z axis, which points to Earth center, and azimuth is // measured from the X axis. // @param Position SV Satellite position // @param Position RX Receiver position // @param Matrix<double> Rot Rotation matrix (3,3), output of SatelliteAttitude // @param double nadir Output nadir angle in degrees // @param double azimuth Output azimuth angle in degrees void SatelliteNadirAzimuthAngles(const Position& SV, const Position& RX, const Matrix<double>& Rot, double& nadir, double& azimuth) throw(Exception) { try { if(Rot.rows() != 3 || Rot.cols() != 3) { Exception e("Rotation matrix invalid"); GPSTK_THROW(e); } double d; Position RmS; // RmS points from satellite to receiver RmS = RX - SV; RmS.transformTo(Position::Cartesian); d = RmS.mag(); if(d == 0.0) { Exception e("Satellite and Receiver Positions identical"); GPSTK_THROW(e); } RmS = (1.0/d) * RmS; Vector<double> XYZ(3),Body(3); XYZ(0) = RmS.X(); XYZ(1) = RmS.Y(); XYZ(2) = RmS.Z(); Body = Rot * XYZ; nadir = ::acos(Body(2)) * RAD_TO_DEG; azimuth = ::atan2(Body(1),Body(0)) * RAD_TO_DEG; if(azimuth < 0.0) azimuth += 360.; } catch(Exception& e) { GPSTK_RETHROW(e); } catch(exception& e) {Exception E("std except: "+string(e.what())); GPSTK_THROW(E);} catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); } }
Matrix<double> Wscorerate_cox(unsigned Var, const Matrix<double> &X, const Matrix<double> &schoen, const Matrix<double> &RR, const Matrix<double> &E, const Matrix<double> &S_0, const Matrix<double> &cumhaz, const Matrix<double> &beta_iid, const Matrix<double> &It, const Matrix<unsigned> &index_dtimes, const Matrix<double> &time) { Matrix<double> dtimes = chrows(time, index_dtimes); unsigned n=X.rows(); unsigned p=X.cols(); unsigned nd=dtimes.size(); Matrix<double> schoendN(n,p); // Initialized as zero for (unsigned i=0; i<nd; i++) schoendN(index_dtimes[i],_) = schoen(i,_); Matrix<double> dMscorerate(n,nd); // component 'Var' of score for (unsigned i=0; i<nd; i++) { Matrix<double> risk = (time>=dtimes[i]); dMscorerate(_,i) = -risk%RR%(X(_,Var)-E(i,Var))/S_0[i]; dMscorerate(index_dtimes[i],i) = schoen(i,Var)+dMscorerate(index_dtimes[i],i); } Matrix<double> Mscorerate = (cumsum(t(dMscorerate))); Matrix<double> Wscorerate(n,nd); for (unsigned i=0; i<n; i++) { Matrix<double> betaiidrow = t(beta_iid(i,_)); for (unsigned j=0; j<nd; j++) { Matrix<double> Itd = It(j,_); Itd.resize(p,p); Wscorerate(i,j) = Mscorerate(j,i) - (Itd*betaiidrow)[Var]; } } return(Wscorerate); }
void RigidBodyTree::accumulateContactJacobian( const KinematicsCache<Scalar> &cache, const int bodyInd, Matrix3Xd const &bodyPoints, std::vector<size_t> const &cindA, std::vector<size_t> const &cindB, Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &J) const { const auto nq = J.cols(); const size_t numCA = cindA.size(); const size_t numCB = cindB.size(); const size_t offset = 3 * numCA; auto J_tmp = transformPointsJacobian(cache, bodyPoints, bodyInd, 0, true); // add contributions from points in xA for (size_t x = 0; x < numCA; x++) { J.block(3 * cindA[x], 0, 3, nq) += J_tmp.block(3 * x, 0, 3, nq); } // subtract contributions from points in xB for (size_t x = 0; x < numCB; x++) { J.block(3 * cindB[x], 0, 3, nq) -= J_tmp.block(offset + 3 * x, 0, 3, nq); } }
void checkScaleMinMax() { Printf("Check scale Min-Max+++++++++++++++++++++++++++\n"); Matrix m(3, {1, 5, 9, 2, 3, 39, 7, 15, 22}); print(m); double min = -1; double max = 1; Matrix testM = m; scaleMinMax(min, max, testM); print(testM); double tMin = 1000, tMax = -1000; for (int i = 0 ; i < testM.rows(); i++) { for (int j = 0; j < testM.cols(); j++) { if (tMax < testM(i, j)) { tMax = testM(i, j); } if (tMin > testM(i, j)) { tMin = testM(i, j); } } } unitpp::assert_true(spf("Expected matrix min should be greater than: %f, but was: %f", min, tMin), tMin >= min); unitpp::assert_true(spf("Expected matrix max should be less than: %f, but was: %f", max, tMax), tMax <= max); // check scale by indices VI indices = {0, 2}; Matrix testM2 = m; scaleMinMax(min, max, indices, testM2); print(testM2); Matrix checkM(3, {-1, 5, -1, -0.666666, 3, 1, 1, 15, -0.133333}); unitpp::assert_true("Scale by min/max with column indices failed", testM2.similar(checkM, 0.000001)); }
static lu_pair<scalar> lu_decomposition(const Matrix<scalar>& A) { assert(A.rows() == A.cols()); auto n = A.rows(); Matrix<scalar> L = Matrix<scalar>::Zero(n, n); Matrix<scalar> U = Matrix<scalar>::Zero(n, n); if (A.rows() == 1) { L(0, 0) = 1.0; U(0, 0) = A(0,0); } else { // first row of U is first row of A auto U12 = A.block(0, 1, 1, n-1); // first column of L is first column of A / a11 auto L21 = A.block(1, 0, n-1, 1) / A(0, 0); // remove first row and column and recurse auto A22 = A.block(1, 1, n-1, n-1); Matrix<scalar> tmp = A22 - L21 * U12; auto LU22 = lu_decomposition(tmp); L(0, 0) = 1.0; U(0, 0) = A(0, 0); L.block(1, 0, n-1, 1) = L21; U.block(0, 1, 1, n-1) = U12; L.block(1, 1, n-1, n-1) = get<0>(LU22); U.block(1, 1, n-1, n-1) = get<1>(LU22); } return lu_pair<scalar>(L, U); }
void Matrix <T> ::submat ( const Matrix <T> & M, const unsigned int row, const unsigned int col ) { const unsigned int m = M.rows(), n = M.cols(); if ( m + row > rows_count ) { throw ErrorIndexOutOfBound ( "ErrorIndexOutOfBound: a submatrix does not fit at the specified row position, ", "can not append a submatrix to the matrix. " ); } if ( n + col > columns_count ) { throw ErrorIndexOutOfBound ( "ErrorIndexOutOfBound: a submatrix does not fit at the specified col position, ", "can not append a submatrix to the matrix." ); } //Copy submatrix for ( unsigned int i = 0; i < m; i++ ) { for ( unsigned int j = 0; j < n; j++ ) { items [i + row][j + col] = M ( i, j ); } } }
Vector yarp::math::dcm2euler(const Matrix &R) { yAssert((R.rows()>=3) && (R.cols()>=3)); Vector v(3); bool singularity=false; if (R(2,2)<1.0) { if (R(2,2)>-1.0) { v[0]=atan2(R(1,2),R(0,2)); v[1]=acos(R(2,2)); v[2]=atan2(R(2,1),-R(2,0)); } else { // Not a unique solution: gamma-alpha=atan2(R10,R11) singularity=true; v[0]=-atan2(R(1,0),R(1,1)); v[1]=M_PI; v[2]=0.0; } } else { // Not a unique solution: gamma+alpha=atan2(R10,R11) singularity=true; v[0]=atan2(R(1,0),R(1,1)); v[1]=0.0; v[2]=0.0; } if (singularity) yWarning("dcm2euler() in singularity: choosing one solution among multiple"); return v; }
Vector yarp::math::dcm2rpy(const Matrix &R) { yAssert((R.rows()>=3) && (R.cols()>=3)); Vector v(3); bool singularity=false; if (R(2,0)<1.0) { if (R(2,0)>-1.0) { v[0]=atan2(R(2,1),R(2,2)); v[1]=asin(-R(2,0)); v[2]=atan2(R(1,0),R(0,0)); } else { // Not a unique solution: psi-phi=atan2(-R12,R11) singularity = true; v[0]=0.0; v[1]=M_PI/2.0; v[2]=-atan2(-R(1,2),R(1,1)); } } else { // Not a unique solution: psi+phi=atan2(-R12,R11) singularity = true; v[0]=0.0; v[1]=-M_PI/2.0; v[2]=atan2(-R(1,2),R(1,1)); } if (singularity) yWarning("dcm2rpy() in singularity: choosing one solution among multiple"); return v; }
BenchResult doBenchFLANN(const Matrix& d, const Matrix& q, const Index K, const int itCount) { BenchResult result; const int dimCount(d.rows()); const int dPtCount(d.cols()); const int qPtCount(itCount); flann::Matrix<T> dataset(new T[dPtCount*dimCount], dPtCount, dimCount); for (int point = 0; point < dPtCount; ++point) for (int dim = 0; dim < dimCount; ++dim) dataset[point][dim] = d(dim, point); flann::Matrix<T> query(new T[qPtCount*dimCount], qPtCount, dimCount); for (int point = 0; point < qPtCount; ++point) for (int dim = 0; dim < dimCount; ++dim) query[point][dim] = q(dim, point); flann::Matrix<int> indices(new int[query.rows*K], query.rows, K); flann::Matrix<float> dists(new float[query.rows*K], query.rows, K); // construct an randomized kd-tree index using 4 kd-trees boost::timer t; flann::Index<T> index(dataset, flann::KDTreeIndexParams(4) /*flann::AutotunedIndexParams(0.9)*/); // exact search index.buildIndex(); result.creationDuration = t.elapsed(); t.restart(); // do a knn search, using 128 checks index.knnSearch(query, indices, dists, int(K), flann::SearchParams(128)); // last parameter ignored because of autotuned result.executionDuration = t.elapsed(); dataset.free(); query.free(); indices.free(); dists.free(); return result; }
/* * "Chase the bulge" down J. * As a result of this method, J will remain bidiagonal, * but off-diagonals will be reduced, and U & V accumulate the rotations. * p and q are matrix dimensions of the block representation of J. */ static void chase(Matrix& J, Matrix& U, Matrix& V, int p, int q) { const int m = J.rows(), n = J.cols(); const int N = n - q; double w, cs, sn; Matrix S(m), T(n); //Compute Wilkinson shift and T2 w = computeWilkinson(sq(J.at(N - 2, N - 2)) + sq(J.at(N - 2, N - 1)), J.at(N - 2, N - 1) * J.at(N - 1, N - 1), sq(J.at(N - 1, N - 1))); rot(an2(J, p) - w, an(J, p) * bn(J, p + 1), cs, sn); T = givensCS(n, p, p + 1, cs, sn); J = J * T; V = V * T; //Perform Givens rotation to chase the values down the off-bidiagonals for (int i = p; i < N - 1; i++) { if (i > p) { //Compute Ti, annihilating values above superdiagonal values rot(J.at(i - 1, i), J.at(i - 1, i + 1), cs, sn); T = givensCS(n, i, i + 1, cs, sn); J = J * T; V = V * T; } //Compute Si^T, annihilating subdiagonal values rot(J.at(i, i), J.at(i + 1, i), cs, sn); S = givensCS(m, i, i + 1, cs, -sn); J = S * J; U = U * S.trans(); } }
int main(int argc, char *argv[]) { std::cout << "Starting a serial matrix multiplication. \n " << std::endl; // Read in the input: N if ( argv[1]== NULL ){ // check input is supplied std::cout << "ERROR: The program must be executed in the following way \n\n \t \"./serial.exe N \" \n\n where N is an integer. \n \n " << std::endl; return 1; } int N = atoi(argv[1]); // The dimensions of the matrices MUST be specified at runtime. std::cout << "The matrices are: " << N<<"x"<<N<< std::endl; // for simplicity, all matricies will be NxN int numberOfRowsA = N; int numberOfColsA = N; int numberOfRowsB = N; int numberOfColsB = N; //Declare matrices: Matix class is a 2D vetor Matrix<double> A = Matrix<double>(numberOfRowsA, numberOfColsA); Matrix<double> B = Matrix<double>(numberOfRowsB, numberOfColsB); Matrix<double> C = Matrix<double>(numberOfRowsA, numberOfColsB); FillMatricesRandomly(A, B); /* Excluded from timing!!! */ struct timespec start, end; clock_gettime(CLOCK_MONOTONIC, &start); // start timing // Matrix Multiplication for (int i = 0; i < A.rows(); i++) {//iterate through rows of A for (int j = 0; j < B.cols(); j++) {//iterate through columns of B for (int k = 0; k < B.rows(); k++) {//iterate through rows of B C(i,j) += (A(i,k) * B(k,j)); } } } clock_gettime(CLOCK_MONOTONIC, &end); // end timing double matrixCalculationTime = (end.tv_sec - start.tv_sec) + (end.tv_nsec - start.tv_nsec)*1e-9; std::cout << "\nTotal multplication time = " << matrixCalculationTime << std::endl; // PrintMatrices(A, B, C); return 0; }
std::shared_ptr<Matrix> NearestNeighborNoiseEstimation(const Matrix &data) { // TODO: Outlier exclusion Index d = data.cols(); Index n = data.rows(); std::shared_ptr<Matrix> result = std::make_shared<Matrix>(d, d); Matrix noise_values(n, d); auto l2_dist = L2Distance(data, data, true); // Find the second smallest distance (first is 0, and should be forced, so it is smallest non-zero), so no need to fully sort the matrix for (Index i = 0; i < n; ++i) { Index smallest_non_zero_index = 0; Scalar smallest_non_zero_value = (*l2_dist)(i, 0); if (i == 0) { smallest_non_zero_value = std::numeric_limits<Scalar>::max(); } for (Index j = 1; j < n; ++j) { if (j == i) continue; if ((*l2_dist)(i, j) < smallest_non_zero_value) { smallest_non_zero_index = j; smallest_non_zero_value = (*l2_dist)(i, j); } } for (Index k = 0; k < d; ++k) { noise_values(i, k) = data(i, k) - data(smallest_non_zero_index, k); } } gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, noise_values.m_, noise_values.m_, 0.0, result->m_); return result; }
YARP_END_PACK /// network stuff #include <yarp/os/NetInt32.h> bool yarp::sig::removeCols(const Matrix &in, Matrix &out, int first_col, int how_many) { int nrows = in.rows(); int ncols = in.cols(); Matrix ret(nrows, ncols-how_many); for(int r=0; r<nrows; r++) for(int c_in=0,c_out=0;c_in<ncols; c_in++) { if (c_in==first_col) { c_in=c_in+(how_many-1); continue; } ret[r][c_out]=(in)[r][c_in]; c_out++; } out=ret; return true; }