// --------------------------------------------------------------------------------
// F is defined as the exclusive-or sum of the first c iterates of the underlying
// pseudorandom function PRF applied to the password P and the concatenation of the
// salt S and the block index i:
//
// F (P, S, c, i) = U_1 \xor U_2 \xor ... \xor U_c
//
// where
//
// U_1 = PRF (P, S || INT (i)) ,
// U_2 = PRF (P, U_1) ,
// ...
// U_c = PRF (P, U_{c-1}) .
//
// Here, INT (i) is a four-octet encoding of the integer i, most
// significant octet first.
// --------------------------------------------------------------------------------
static BYTE * F(const MemBuf &P, const MemBuf &S, int count, int index, MemBuf &out)
{
// Create a working salt that is the inputted salt appended with the index.
MemBuf SI( S.size() + 4 );
memcpy(SI, S, S.size() );
SI[ S.size() ] = (BYTE)( (index>>24) );
SI[ S.size() + 1 ] = (BYTE)( (index>>16) & 0xff );
SI[ S.size() + 2 ] = (BYTE)( (index>> 8) & 0xff );
SI[ S.size() + 3 ] = (BYTE)( index & 0xff );
// U_1:
MemBuf U_i( SHA1_LEN ); hmac_sha1( SI, SI.size(), P, P.size(), U_i );
out.copy(U_i);
// U_2 thru U_c:
for (int i=2; i<=count; i++) {
MemBuf U_tmp( U_i );
hmac_sha1( U_tmp, U_tmp.size(), P, P.size(), U_i );
for (UINT j=0; j<out.size(); j++) {
out[j] ^= U_i[j];
}
}
return out;
}
void PR2EihMappingNBV::next_best_view(int max_iters, const MatrixJ& j_sigma0,
const std::vector<Gaussian3d>& obj_gaussians, const std::vector<geometry3d::Triangle>& obstacles,
StdVectorJ& J, StdVectorU& U) {
VectorJ j_t = arm->get_joints();
const double alpha = 10;
J = StdVectorJ(T);
StdVectorJ J_i(T);
U = StdVectorU(T-1);
StdVectorU U_i(T-1);
double min_cost = INFINITY;
for(int i=0; i < max_iters; ++i) {
sample_joint_trajectory(j_t, J_i, U_i);
double cost = sys->cost(J_i, j_sigma0, U_i, obj_gaussians, alpha, obstacles);
if (cost < min_cost) {
min_cost = cost;
J = J_i;
U = U_i;
}
}
for(const VectorJ& j : J) {
sim->plot_transform(sim->transform_from_to(arm_sim->fk(j), "base_link", "world"));
}
}
