Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
	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;
	}
Exemplo n.º 3
0
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;
    }
  }
}
Exemplo n.º 4
0
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;

}
Exemplo n.º 5
0
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;
}
Exemplo n.º 6
0
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;
}
Exemplo n.º 7
0
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;
}
Exemplo n.º 8
0
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();
}
Exemplo n.º 9
0
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;
}
Exemplo n.º 10
0
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;
}
Exemplo n.º 11
0
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);
}
Exemplo n.º 12
0
// 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);
}
Exemplo n.º 13
0
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;
      }
    }
  }
}
Exemplo n.º 14
0
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;
      }
    }
  }
}
Exemplo n.º 15
0
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);
}
Exemplo n.º 16
0
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);
  }
}
Exemplo n.º 17
0
 void copy_shape(const MatrixBase<Derived>& src, MatrixBase<Derived>& dst) {
   dst.derived().resize(src.rows(), src.cols());
 }
Exemplo n.º 18
0
std::string dim(MatrixBase<Derived>& m)
{
   std::stringstream ss;
   ss << m.rows() << " x " << m.cols();
   return ss.str();
}