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; }
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; }
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; }
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; }
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; }
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; }
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(); }
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 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); }
// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). void getRows(std::set<int>& rows, MatrixBase<DerivedA> const& M, // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). 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 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 angleDiff(MatrixBase<DerivedPhi1> const& phi1, MatrixBase<DerivedPhi2> const& phi2, // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references). 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 care(MatrixBase<DerivedA> const& A, MatrixBase<DerivedB> const& B, MatrixBase<DerivedQ> const& Q, MatrixBase<DerivedR> const& R, MatrixBase<DerivedX> & X) { const size_t n = A.rows(); LLT<MatrixXd> R_cholesky(R); MatrixXd H(2 * n, 2 * n); H << A, B * R_cholesky.solve(B.transpose()), Q, -A.transpose(); MatrixXd Z = H; MatrixXd Z_old; //these could be options const double tolerance = 1e-9; const double max_iterations = 100; double relative_norm; size_t iteration = 0; const double p = static_cast<double>(Z.rows()); do { Z_old = Z; //R. Byers. Solving the algebraic Riccati equation with the matrix sign function. Linear Algebra Appl., 85:267–279, 1987 //Added determinant scaling to improve convergence (converges in rough half the iterations with this) double ck = pow(abs(Z.determinant()), -1.0/p); Z *= ck; Z = Z - 0.5 * (Z - Z.inverse()); relative_norm = (Z - Z_old).norm(); iteration ++; } while(iteration < max_iterations && relative_norm > tolerance); MatrixXd W11 = Z.block(0, 0, n, n); MatrixXd W12 = Z.block(0, n, n, n); MatrixXd W21 = Z.block(n, 0, n, n); MatrixXd W22 = Z.block(n, n, n, n); MatrixXd lhs(2 * n, n); MatrixXd rhs(2 * n, n); MatrixXd eye = MatrixXd::Identity(n, n); lhs << W12, W22 + eye; rhs << W11 + eye, W21; JacobiSVD<MatrixXd> svd(lhs, ComputeThinU | ComputeThinV); X = svd.solve(rhs); }
void inverseKinBackend( RigidBodyTree* model, const int nT, const double* t, const MatrixBase<DerivedA>& q_seed, const MatrixBase<DerivedB>& q_nom, const int num_constraints, RigidBodyConstraint** const constraint_array, const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol, int* info, std::vector<std::string>* infeasible_constraint) { // Validate some basic parameters of the input. if (q_seed.rows() != model->number_of_positions() || q_seed.cols() != nT || q_nom.rows() != model->number_of_positions() || q_nom.cols() != nT) { throw std::runtime_error( "Drake::inverseKinBackend: q_seed and q_nom must be of size " "nq x nT"); } KinematicsCacheHelper<double> kin_helper(model->bodies); // TODO(sam.creasey) I really don't like rebuilding the // OptimizationProblem for every timestep, but it's not possible to // enable/disable (or even remove) a constraint from an // OptimizationProblem between calls to Solve() currently, so // there's not actually another way. for (int t_index = 0; t_index < nT; t_index++) { OptimizationProblem prog; SetIKSolverOptions(ikoptions, &prog); DecisionVariableView vars = prog.AddContinuousVariables(model->number_of_positions()); MatrixXd Q; ikoptions.getQ(Q); auto objective = std::make_shared<InverseKinObjective>(model, Q); prog.AddCost(objective, {vars}); for (int i = 0; i < num_constraints; i++) { RigidBodyConstraint* constraint = constraint_array[i]; const int constraint_category = constraint->getCategory(); if (constraint_category == RigidBodyConstraint::SingleTimeKinematicConstraintCategory) { SingleTimeKinematicConstraint* stc = static_cast<SingleTimeKinematicConstraint*>(constraint); if (!stc->isTimeValid(&t[t_index])) { continue; } auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>( stc, &kin_helper); prog.AddConstraint(wrapper, {vars}); } else if (constraint_category == RigidBodyConstraint::PostureConstraintCategory) { PostureConstraint* pc = static_cast<PostureConstraint*>(constraint); if (!pc->isTimeValid(&t[t_index])) { continue; } VectorXd lb; VectorXd ub; pc->bounds(&t[t_index], lb, ub); prog.AddBoundingBoxConstraint(lb, ub, {vars}); } else if ( constraint_category == RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) { AddSingleTimeLinearPostureConstraint( &t[t_index], constraint, model->number_of_positions(), vars, &prog); } else if (constraint_category == RigidBodyConstraint::QuasiStaticConstraintCategory) { AddQuasiStaticConstraint(&t[t_index], constraint, &kin_helper, vars, &prog); } else if (constraint_category == RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) { throw std::runtime_error( "MultipleTimeKinematicConstraint is not supported" " in pointwise mode."); } else if ( constraint_category == RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) { throw std::runtime_error( "MultipleTimeLinearPostureConstraint is not supported" " in pointwise mode."); } } // Add a bounding box constraint from the model. prog.AddBoundingBoxConstraint( model->joint_limit_min, model->joint_limit_max, {vars}); // TODO(sam.creasey) would this be faster if we stored the view // instead of copying into a VectorXd? objective->set_q_nom(q_nom.col(t_index)); if (!ikoptions.getSequentialSeedFlag() || (t_index == 0)) { prog.SetInitialGuess(vars, q_seed.col(t_index)); } else { prog.SetInitialGuess(vars, q_sol->col(t_index - 1)); } SolutionResult result = prog.Solve(); prog.PrintSolution(); q_sol->col(t_index) = vars.value(); info[t_index] = GetIKSolverInfo(prog, result); } }
void copy_shape(const MatrixBase<Derived>& src, MatrixBase<Derived>& dst) { dst.derived().resize(src.rows(), src.cols()); }
std::string dim(MatrixBase<Derived>& m) { std::stringstream ss; ss << m.rows() << " x " << m.cols(); return ss.str(); }