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; }
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; }
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; }
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); };
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); }
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; }
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; } };
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; } } }
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 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); }
// 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); }
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; } }
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; } } } }
Matrix<Scalar, Dynamic, Dynamic> forwardKinJacobianTemp( const RigidBodyTreed& tree, const KinematicsCache<Scalar>& cache, const MatrixBase<DerivedPoints>& points, int current_body_or_frame_ind, int new_body_or_frame_ind, int rotation_type, bool in_terms_of_qdot) { auto Jtrans = tree.transformPointsJacobian(cache, points, current_body_or_frame_ind, new_body_or_frame_ind, in_terms_of_qdot); if (rotation_type == 0) { return Jtrans; } else { Matrix<Scalar, Dynamic, Dynamic> Jrot( rotationRepresentationSize(rotation_type), Jtrans.cols()); if (rotation_type == 1) Jrot = tree.relativeRollPitchYawJacobian(cache, current_body_or_frame_ind, new_body_or_frame_ind, in_terms_of_qdot); else if (rotation_type == 2) Jrot = tree.relativeQuaternionJacobian(cache, current_body_or_frame_ind, new_body_or_frame_ind, in_terms_of_qdot); else throw runtime_error("rotation_type not recognized"); Matrix<Scalar, Dynamic, Dynamic> J((3 + Jrot.rows()) * points.cols(), Jtrans.cols()); int row_start = 0; for (int i = 0; i < points.cols(); i++) { J.template middleRows<3>(row_start) = Jtrans.template middleRows<3>(3 * i); row_start += 3; J.middleRows(row_start, Jrot.rows()) = Jrot; row_start += Jrot.rows(); } return J; } }
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 setConstraintMatrixPart(double time, int derivative_order, MatrixBase<DerivedM> & constraint_matrix, double scaling = 1.0) { double time_power = 1.0; Eigen::Index num_coefficients = constraint_matrix.cols(); for (int col = derivative_order; col < num_coefficients; col++) { double column_power = 1.0; for (int i = 0; i< derivative_order; i++) { column_power *= (col - i); } constraint_matrix(0, col) = scaling * time_power * column_power; time_power *= time; } }
Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> forwardKinTemp( const RigidBodyTreed& tree, const KinematicsCache<Scalar>& cache, const MatrixBase<DerivedPoints>& points, int current_body_or_frame_ind, int new_body_or_frame_ind, int rotation_type) { Matrix<Scalar, Dynamic, DerivedPoints::ColsAtCompileTime> ret( 3 + rotationRepresentationSize(rotation_type), points.cols()); ret.template topRows<3>() = tree.transformPoints( cache, points, current_body_or_frame_ind, new_body_or_frame_ind); if (rotation_type == 1) { ret.template bottomRows<3>().colwise() = tree.relativeRollPitchYaw( cache, current_body_or_frame_ind, new_body_or_frame_ind); } else if (rotation_type == 2) { ret.template bottomRows<4>().colwise() = tree.relativeQuaternion( cache, current_body_or_frame_ind, new_body_or_frame_ind); } return ret; }
bool checkPointsInOOBB(const MatrixBase<Derived>& points, OOBB oobb) { Matrix33 A_KI = oobb.m_q_KI.matrix(); A_KI.transposeInPlace(); Vector3 p; bool allInside = true; auto size = points.cols(); decltype(size) i = 0; while (i < size && allInside) { p = A_KI * points.col(i); allInside &= (p(0) >= oobb.m_minPoint(0) && p(0) <= oobb.m_maxPoint(0) && p(1) >= oobb.m_minPoint(1) && p(1) <= oobb.m_maxPoint(1) && p(2) >= oobb.m_minPoint(2) && p(2) <= oobb.m_maxPoint(2)); ++i; } return allInside; }
APPROXMVBB_EXPORT void samplePointsGrid(Matrix3Dyn & newPoints, const MatrixBase<Derived> & points, const unsigned int nPoints, OOBB & oobb) { if(nPoints >= points.cols() || nPoints < 2) { ApproxMVBB_ERRORMSG("Wrong arguements!") } newPoints.resize(3,nPoints); std::random_device rd; std::mt19937 gen(rd()); std::uniform_int_distribution<unsigned int> dis(0, points.cols()-1); //total points = bottomPoints=gridSize^2 + topPoints=gridSize^2 unsigned int gridSize = std::max( static_cast<unsigned int>( std::sqrt( static_cast<double>(nPoints) / 2.0 )) , 1U ); // Set z-Axis to longest dimension //std::cout << oobb.m_minPoint.transpose() << std::endl; oobb.setZAxisLongest(); unsigned int halfSampleSize = gridSize*gridSize; std::vector< std::pair<unsigned int , PREC > > topPoints(halfSampleSize, std::pair<unsigned int,PREC>{} ); // grid of indices of the top points (indexed from 1 ) std::vector< std::pair<unsigned int , PREC > > bottomPoints(halfSampleSize, std::pair<unsigned int,PREC>{} ); // grid of indices of the bottom points (indexed from 1 ) using LongInt = long long int; MyMatrix<LongInt>::Array2 idx; // Normalized P //std::cout << oobb.extent() << std::endl; //std::cout << oobb.m_minPoint.transpose() << std::endl; Array2 dxdyInv = Array2(gridSize,gridSize) / oobb.extent().head<2>(); // in K Frame; Vector3 K_p; Matrix33 A_KI(oobb.m_q_KI.matrix().transpose()); // Register points in grid auto size = points.cols(); for(unsigned int i=0; i<size; ++i) { K_p = A_KI * points.col(i); // get x index in grid idx = ( (K_p - oobb.m_minPoint).head<2>().array() * dxdyInv ).template cast<LongInt>(); // map to grid idx(0) = std::max( std::min( LongInt(gridSize-1), idx(0)), 0LL ); idx(1) = std::max( std::min( LongInt(gridSize-1), idx(1)), 0LL ); //std::cout << idx.transpose() << std::endl; unsigned int pos = idx(0) + idx(1)*gridSize; // Register points in grid // if z axis of p is > topPoints[pos] -> set new top point at pos // if z axis of p is < bottom[pos] -> set new bottom point at pos if( topPoints[pos].first == 0) { topPoints[pos].first = bottomPoints[pos].first = i+1; topPoints[pos].second = bottomPoints[pos].second = K_p(2); } else { if( topPoints[pos].second < K_p(2) ) { topPoints[pos].first = i+1; topPoints[pos].second = K_p(2); } if( bottomPoints[pos].second > K_p(2) ) { bottomPoints[pos].first = i+1; bottomPoints[pos].second = K_p(2); } } } // Copy top and bottom points unsigned int k=0; // k does not overflow -> 2* halfSampleSize = 2*gridSize*gridSize <= nPoints; for(unsigned int i=0; i<halfSampleSize; ++i) { if( topPoints[i].first != 0 ) { // comment in if you want the points top points of the grid // Array3 a(i % gridSize,i/gridSize,oobb.m_maxPoint(2)-oobb.m_minPoint(2)); // a.head<2>()*=dxdyInv.inverse(); newPoints.col(k++) = points.col(topPoints[i].first-1); // A_KI.transpose()*(oobb.m_minPoint + a.matrix()).eval() ; if(topPoints[i].first != bottomPoints[i].first) { // comment in if you want the bottom points of the grid // Array3 a(i % gridSize,i/gridSize,0); // a.head<2>()*=dxdyInv.inverse(); newPoints.col(k++) = points.col(bottomPoints[i].first-1); // A_KI.transpose()*(oobb.m_minPoint + a.matrix()).eval() ; } } } // Add random points! while( k < nPoints) { newPoints.col(k++) = points.col( dis(gen) ); //= Vector3(0,0,0);// } }
void copy_shape(const MatrixBase<Derived>& src, MatrixBase<Derived>& dst) { dst.derived().resize(src.rows(), src.cols()); }
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); } }
std::string dim(MatrixBase<Derived>& m) { std::stringstream ss; ss << m.rows() << " x " << m.cols(); return ss.str(); }