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();
}
