int myGRBaddconstrs(GRBmodel* model, MatrixBase<DerivedA> const& A, MatrixBase<DerivedB> const& b, char sense, double sparseness_threshold = 1e-14) { int i, j, nnz, error = 0; /* // todo: it seems like I should just be able to do something like this: SparseMatrix<double, RowMajor> sparseAeq(Aeq.sparseView()); sparseAeq.makeCompressed(); error = GRBaddconstrs(model, nq_con, sparseAeq.nonZeros(), sparseAeq.InnerIndices(), sparseAeq.OuterStarts(), sparseAeq.Values(), beq.data(), NULL); */ int* cind = new int[A.cols()]; double* cval = new double[A.cols()]; for (i = 0; i < A.rows(); i++) { nnz = 0; for (j = 0; j < A.cols(); j++) { if (abs(A(i, j)) > sparseness_threshold) { cval[nnz] = A(i, j); cind[nnz++] = j; } } error = GRBaddconstr(model, nnz, cind, cval, sense, b(i), NULL); if (error) break; } delete[] cind; delete[] cval; return error; }
bool CSRMatrix::eq(const MatrixBase &other) const { unsigned row = this->nrows(); if (row != other.nrows() or this->ncols() != other.ncols()) return false; if (is_a<CSRMatrix>(other)) { const CSRMatrix &o = down_cast<const CSRMatrix &>(other); if (this->p_[row] != o.p_[row]) return false; for (unsigned i = 0; i <= row; i++) if (this->p_[i] != o.p_[i]) return false; for (unsigned i = 0; i < this->p_[row]; i++) if ((this->j_[i] != o.j_[i]) or neq(*this->x_[i], *(o.x_[i]))) return false; return true; } else { return this->MatrixBase::eq(other); } }
Derived lidarBoostEngine::apply_optical_flow(const MatrixBase<Derived>& I, std::vector< Derived > uv) { Derived moved_I(beta*n, beta*m); // W = SparseMatrix<double>( beta*n, beta*m ); // W.reserve(VectorXi::Constant(n, m)); // T = SparseMatrix<double>( beta*n, beta*m ); // MatrixXi W( beta*n, beta*m ), T( beta*n, beta*m ); W = MatrixXd( beta*n, beta*m ); int new_i, new_j; for( int i = 0; i < I.rows(); i++ ) { for( int j = 0; j < I.cols(); j++ ) { new_i = round( beta * (i + uv[0](i, j)) ); new_j = round( beta * (j + uv[1](i, j)) ); if(new_i > 0 && new_i < beta*n && new_j > 0 && new_j < beta*m) { moved_I(new_i, new_j) = I(i ,j); W(new_i, new_j) = 1; } } } return moved_I; }
inline auto convolution( MatrixBase< DerivedM > const& m, MatrixBase< DerivedK > const& k ){ using ScalarK = typename DerivedK::Scalar; using ResultType = Matrix< ScalarK, Eigen::Dynamic, Eigen::Dynamic >; int kr = k.rows(); int kc = k.cols(); int res_r = m.rows() - kr + 1; int res_c = m.cols() - kc + 1; ResultType res(res_r, res_c); for(int my = 0; my < res_r; ++my){ for(int mx = 0; mx < res_c; ++mx){ ScalarK b = 0; for(int ky = 0; ky < kr; ++ky){ for(int kx = 0; kx < kc; ++kx){ b += m(my + ky, mx + kx) * k(ky, kx); } } res.coeffRef(my, mx) = b; } } return res; }
Vector<T> GaussianElimination<T>::operator()(MatrixBase<T>& A, const Vector<T>& b) { Vector<T> x( b.getSize() ); Matrix<T> newA(A.getNumRows(),A.getNumCols()); newA = A; newA.addColumn(b); // Forward Elimination for(int i=1; i < newA.getNumRows(); i++) { reduceDown(newA, i); } // Backward Elimination for(int i=newA.getNumRows()-2; i >= 0; i--) { reduceUp(newA, i); } // Solve for x for(int i=0; i < x.getSize(); i++) { x[i] = newA(i,newA.getNumCols()-1) / newA(i,i); } return x; }
void setLinearIndices(MatrixBase<Derived>& A) { for (int col = 0; col < A.cols(); col++) { for (int row = 0; row < A.rows(); row++) { A(row, col) = row + col * A.rows() + 1; } } }
std::vector < Derived > lidarBoostEngine::lk_optical_flow( const MatrixBase<Derived>& I1, const MatrixBase<Derived> &I2, int win_sz) { //Instantiate optical flow matrices std::vector < Derived > uv(2); //Create masks Matrix2d robX, robY, robT; robX << -1, 1, -1, 1; robY << -1, -1, 1, 1; robT << -1, -1, -1, -1; Derived Ix, Iy, It, A, solutions, x_block, y_block, t_block; //Apply masks to images and average the result Ix = 0.5 * ( conv2d( I1, robX ) + conv2d( I2, robX ) ); Iy = 0.5 * ( conv2d( I1, robY ) + conv2d( I2, robY ) ); It = 0.5 * ( conv2d( I1, robT ) + conv2d( I2, robT ) ); uv[0] = Derived::Zero( I1.rows(), I1.cols() ); uv[1] = Derived::Zero( I1.rows(), I1.cols() ); int hw = win_sz/2; for( int i = hw+1; i < I1.rows()-hw; i++ ) { for ( int j = hw+1; j < I1.cols()-hw; j++ ) { //Take a small block of window size in the filtered images x_block = Ix.block( i-hw, j-hw, win_sz, win_sz); y_block = Iy.block( i-hw, j-hw, win_sz, win_sz); t_block = It.block( i-hw, j-hw, win_sz, win_sz); //Convert these blocks in vectors Map<Derived> A1( x_block.data(), win_sz*win_sz, 1); Map<Derived> A2( y_block.data(), win_sz*win_sz, 1); Map<Derived> B( t_block.data(), win_sz*win_sz, 1); //Organize the vectors in a matrix A = Derived( win_sz*win_sz, 2 ); A.block(0, 0, win_sz*win_sz, 1) = A1; A.block(0, 1, win_sz*win_sz, 1) = A2; //Solve the linear least square system solutions = (A.transpose() * A).ldlt().solve(A.transpose() * B); //Insert the solutions in the optical flow matrices uv[0](i, j) = solutions(0); uv[1](i, j) = solutions(1); } } return uv; }
void save(const char *filename, const MatrixBase<Derived>& m) { unsigned int rows = m.rows(), cols = m.cols(); std::ofstream f(filename, std::ios::out | std::ios::binary); f.write((char *)&rows, sizeof(rows)); f.write((char *)&cols, sizeof(cols)); f.write((char *)m.derived().data(), sizeof(typename Derived::Scalar) * rows * cols); f.close(); }
void vFSMNLayer::Compute( const MatrixBase& feature, MatrixBase* output ) { m_linear.Compute( feature, output ); m_memory.Reshape( feature.Rows(), feature.Columns() ); m_memory.VfsmnMemory( feature, m_filter, m_position ); output->Sgemm( 1.0f, 1.0f, m_memory, CUBLAS_OP_N, m_weight, CUBLAS_OP_N ); output->ReLU( *output ); }
void load(const char *filename, MatrixBase<Derived>& m) { int nrows, ncols; std::ifstream f(filename, std::ios::binary); f.read(reinterpret_cast<char*>(&nrows), sizeof(int)); f.read(reinterpret_cast<char*>(&ncols), sizeof(int)); m.derived().resize(nrows, ncols); f.read((char*)m.derived().data(), sizeof(typename Derived::Scalar) * nrows * ncols); }
bool hasInf(const MatrixBase& expr) { for (int i = 0; i != expr.cols(); ++i) { for (int j = 0; j != expr.rows(); ++j) { if (std::isinf(expr.coeff(j, i))) return true; } } return false; }
bool hasNaN(const MatrixBase& expr) { for (int j = 0; j != expr.cols(); ++j) { for (int i = 0; i != expr.rows(); ++i) { if (std::isnan(expr.coeff(i, j))) return true; } } return false; }
void getCols(std::set<int> &cols, MatrixBase<DerivedA> const &M, MatrixBase<DerivedB> &Msub) { if (static_cast<int>(cols.size()) == M.cols()) { Msub = M; return; } int i = 0; for (std::set<int>::iterator iter = cols.begin(); iter != cols.end(); iter++) Msub.col(i++) = M.col(*iter); }
void DenseMatrix::add_matrix(const MatrixBase &other, MatrixBase &result) const { SYMENGINE_ASSERT(row_ == result.nrows() and col_ == result.ncols()); if (is_a<DenseMatrix>(other) and is_a<DenseMatrix>(result)) { const DenseMatrix &o = static_cast<const DenseMatrix &>(other); DenseMatrix &r = static_cast<DenseMatrix &>(result); add_dense_dense(*this, o, r); } }
void Matrix<T>::copy(const MatrixBase<T>& a) { setSize(a.getNumRows(), a.getNumCols()); for(int i=0; i < a.getNumRows(); i++) { for(int j=0; j < a.getNumCols(); j++) { head[i][j] = a(i,j); } } }
void getRows(std::set<int> &rows, MatrixBase<DerivedA> const &M, MatrixBase<DerivedB> &Msub) { if (static_cast<int>(rows.size()) == M.rows()) { Msub = M; return; } int i = 0; for (std::set<int>::iterator iter = rows.begin(); iter != rows.end(); iter++) Msub.row(i++) = M.row(*iter); }
void fillSubMatrix ( MatrixBase<T> & fillMatrix, MatrixBase<T> & subMatrix, unsigned int startRow, unsigned int startCol ) { for ( unsigned int i = 0; i < subMatrix.size(); i++ ) { for ( unsigned int j = 0; j < subMatrix.size(); j++ ) { fillMatrix( startRow + i , startCol + j ) = subMatrix ( i , j ); } } return; }
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). void getCols(std::set<int>& cols, MatrixBase<DerivedA> const& M, // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). MatrixBase<DerivedB>& Msub) { if (static_cast<int>(cols.size()) == M.cols()) { Msub = M; return; } int i = 0; for (std::set<int>::iterator iter = cols.begin(); iter != cols.end(); iter++) Msub.col(i++) = M.col(*iter); }
MatrixXd stack( const MatrixBase<D1>& m1, const MatrixBase<D2>& m2 ) { assert( m1.cols() == m2.cols() ); const int m1r=m1.rows(), m2r=m2.rows(), mr=m1r+m2r, mc=m1.cols(); MatrixXd res( mr,mc ); for( int i=0;i<m1r;++i ) for( int j=0;j<mc;++j ) res(i,j) = m1(i,j); for( int i=0;i<m2r;++i ) for( int j=0;j<mc;++j ) res(m1r+i,j) = m2(i,j); return res; }
bool MatrixBase::eq(const MatrixBase &other) const { if (this->nrows() != other.nrows() || this->ncols() != other.ncols()) return false; for (unsigned i = 0; i < this->nrows(); i++) for (unsigned j = 0; j < this->ncols(); j++) if(neq(this->get(i, j), other.get(i, j))) return false; return true; }
void sFSMNLayer::Compute( const MatrixBase& feature, MatrixBase* output ) { m_linear.Compute( feature, output ); m_memory.Reshape( feature.Rows(), feature.Columns() ); m_memory.Strmm( 1.0f, m_block_diagonal, CUBLAS_SIDE_LEFT, CUBLAS_FILL_MODE_LOWER, feature, CUBLAS_OP_N ); // @xmb20160226 // m_memory.Sgemm( 0.0f, 1.0f, m_block_diagonal, CUBLAS_OP_N, feature, CUBLAS_OP_N ); output->Sgemm( 1.0f, 1.0f, m_memory, CUBLAS_OP_N, m_weight, CUBLAS_OP_N ); output->ReLU( *output ); }
void angleDiff(MatrixBase<DerivedPhi1> const &phi1, MatrixBase<DerivedPhi2> const &phi2, MatrixBase<DerivedD> &d) { d = phi2 - phi1; for (int i = 0; i < phi1.rows(); i++) { for (int j = 0; j < phi1.cols(); j++) { if (d(i,j) < -M_PI) { d(i,j) = fmod(d(i,j) + M_PI, 2*M_PI) + M_PI; } else { d(i,j) = fmod(d(i,j) + M_PI, 2*M_PI) - M_PI; } } } }
void dumpPointsMatrix(std::string filePath, const MatrixBase<Derived>& v) { std::fstream l(filePath.c_str()); if (!l.good()) { GRSF_ERRORMSG("Could not open file: " << filePath << std::endl) } for (unsigned int i = 0; i < v.cols(); i++) { l << v.col(i).transpose().format(MyMatrixIOFormat::SpaceSep) << std::endl; } }
Matrix<Scalar, Dynamic, 1> dynamicsBiasTermTemp(const RigidBodyTree &model, KinematicsCache<Scalar> &cache, const MatrixBase<DerivedF> &f_ext_value) { // temporary solution. eigen_aligned_unordered_map<const RigidBody *, Matrix<Scalar, 6, 1> > f_ext; if (f_ext_value.size() > 0) { assert(f_ext_value.cols() == model.bodies.size()); for (DenseIndex i = 0; i < f_ext_value.cols(); i++) { f_ext.insert({model.bodies[i].get(), f_ext_value.col(i)}); } } return model.dynamicsBiasTerm(cache, f_ext); };
Derived lidarBoostEngine::conv2d(const MatrixBase<Derived>& I, const MatrixBase<Derived2> &kernel ) { Derived O = Derived::Zero(I.rows(),I.cols()); typedef typename Derived::Scalar Scalar; typedef typename Derived2::Scalar Scalar2; int col=0,row=0; int KSizeX = kernel.rows(); int KSizeY = kernel.cols(); int limitRow = I.rows()-KSizeX; int limitCol = I.cols()-KSizeY; Derived2 block ; Scalar normalization = kernel.sum(); if ( normalization < 1E-6 ) { normalization=1; } for ( row = KSizeX; row < limitRow; row++ ) { for ( col = KSizeY; col < limitCol; col++ ) { Scalar b=(static_cast<Derived2>( I.block(row,col,KSizeX,KSizeY ) ).cwiseProduct(kernel)).sum(); O.coeffRef(row,col) = b; } } return O/normalization; }
Matrix<Scalar, Dynamic, 1> dynamicsBiasTermTemp( const RigidBodyTreed& model, KinematicsCache<Scalar>& cache, const MatrixBase<DerivedF>& f_ext_value) { // temporary solution. typename RigidBodyTree<Scalar>::BodyToWrenchMap external_wrenches; if (f_ext_value.size() > 0) { DRAKE_ASSERT(f_ext_value.cols() == model.bodies.size()); for (Eigen::Index i = 0; i < f_ext_value.cols(); i++) { external_wrenches.insert({model.bodies[i].get(), f_ext_value.col(i)}); } } return model.dynamicsBiasTerm(cache, external_wrenches); }
MatrixBase<T>& Matrix<T>::operator*=(const MatrixBase<T>& rhs) { if( getNumCols() != rhs.getNumRows() ) throw SizeError(getNumCols(), "operator*= matrix"); MatrixBase<T>* retVal = new Matrix<T>(getNumRows(), rhs.getNumCols()); for(int i=0; i < getNumRows(); i++) { for(int j=0; j < rhs.getNumCols(); j++) { retVal->operator()(i,j) = getRow(i) * rhs.getColumn(j); } } *this = *retVal; delete retVal; return *this; }
// print the features (debugging) void FeatureFile::print(MatrixBase<float> &mFeatures, unsigned int iDelta) { assert((mFeatures.getCols()%iDelta) == 0); cout << endl; for(unsigned int i=0 ; i < mFeatures.getRows() ; ++i) { for(unsigned int j=0 ; j < iDelta ; ++j) { for(unsigned int h=0 ; h < mFeatures.getCols()/iDelta ; ++h) { cout << " " << FLT(9,6) << mFeatures(i,h+j*(mFeatures.getCols()/iDelta)); } cout << endl; } cout << endl; } }
void vFSMNLayer::BackPropagate( MatrixBase& outDiff, const MatrixBase& in, const MatrixBase& out, float learningRate, MatrixBase* inDiff ) { outDiff.DiffReLU( out, outDiff ); m_linear.BackPropagate( outDiff, in, Matrix(), learningRate, inDiff ); if ( m_w_momentum.Rows() != m_weight.Rows() || m_w_momentum.Columns() != m_weight.Columns() ) { m_w_momentum.Reshape( m_weight.Rows(), m_weight.Columns(), kSetZero ); } float avg_lr = -learningRate / (float)outDiff.Rows(); m_w_momentum.Sgemm( m_momentum, avg_lr, m_memory, CUBLAS_OP_T, outDiff, CUBLAS_OP_N ); if ( m_weight_decay != 0.0f ) { m_w_momentum.Add( 1.0f, -learningRate * m_weight_decay, m_weight ); } m_memory_diff.Reshape( m_memory.Rows(), m_memory.Columns() ); m_memory_diff.Sgemm( 0.0f, 1.0f, outDiff, CUBLAS_OP_N, m_weight, CUBLAS_OP_T ); if ( NULL != inDiff ) { inDiff->ComputeVfsmnHiddenDiff( m_memory_diff, m_filter, m_position ); } if ( m_norm_type == "Clip" ) { float range = m_norm_param * (-avg_lr); m_w_momentum.Clip( -range, range ); } if ( m_norm_type == "L2Norm" ) { float w_norm = m_w_momentum.L2Norm() * (-avg_lr); if ( w_norm > m_norm_param ) { m_w_momentum.Scale( 1.0f / w_norm ); } } m_filter.UpdateVfsmnFilter( m_memory_diff, in, m_position, avg_lr ); m_weight.Add( 1.0f, 1.0f, m_w_momentum ); }
Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> forwardJacDotTimesVTemp( const RigidBodyTree &tree, const KinematicsCache<Scalar> &cache, const MatrixBase<DerivedPoints> &points, int current_body_or_frame_ind, int new_body_or_frame_ind, int rotation_type) { auto Jtransdot_times_v = tree.transformPointsJacobianDotTimesV(cache, points, current_body_or_frame_ind, new_body_or_frame_ind); if (rotation_type == 0) { return Jtransdot_times_v; } else { Matrix<Scalar, Dynamic, 1> Jrotdot_times_v(rotationRepresentationSize(rotation_type)); if (rotation_type == 1) { Jrotdot_times_v = tree.relativeRollPitchYawJacobianDotTimesV(cache, current_body_or_frame_ind, new_body_or_frame_ind); } else if (rotation_type == 2) { Jrotdot_times_v = tree.relativeQuaternionJacobianDotTimesV(cache, current_body_or_frame_ind, new_body_or_frame_ind); } else { throw runtime_error("rotation type not recognized"); } Matrix<Scalar, Dynamic, 1> Jdot_times_v((3 + rotationRepresentationSize(rotation_type)) * points.cols(), 1); int row_start = 0; for (int i = 0; i < points.cols(); i++) { Jdot_times_v.template middleRows<3>(row_start) = Jtransdot_times_v.template middleRows<3>(3 * i); row_start += 3; Jdot_times_v.middleRows(row_start, Jrotdot_times_v.rows()) = Jrotdot_times_v; row_start += Jrotdot_times_v.rows(); } return Jdot_times_v; } };