void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
T a1[N], b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
b[i] = bs::frac(a1[i]) ;
}
p_t aa1(&a1[0], &a1[N]);
p_t bb (&b[0], &b[N]);
STF_IEEE_EQUAL(bs::frac(aa1), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
T a1[N], a2[N], b[N];
for(int i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(1) : T(0);
a2[i] = (i%2) ? T(i+N) : T(-(i+N));
b[i] = bs::negifnot(a1[i], a2[i]);
}
p_t aa1(&a1[0], &a1[N]);
p_t aa2(&a2[0], &a2[N]);
p_t bb(&b[0], &b[N]);
STF_IEEE_EQUAL(bs::negifnot(aa1, aa2), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
T a1[N], a2[N], b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(2*i);
a2[i] = (i%2) ? T(i+N) : T(1);
b[i] = bs::bitwise_andnot(a1[i], a2[i]);
}
p_t aa1(&a1[0], &a1[N]);
p_t aa2(&a2[0], &a2[N]);
p_t bb(&b[0], &b[N]);
STF_IEEE_EQUAL(bs::bitwise_andnot(aa1, aa2), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using lT = bs::logical<T>;
using p_t = bs::pack<T, N>;
using pl_t = bs::pack<lT, N>;
T a1[N];
lT b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
b[i] = bs::is_denormal(a1[i]);
}
p_t aa1(&a1[0], &a1[N]);
pl_t bb(&b[0], &b[N]);
STF_IEEE_EQUAL(bs::is_denormal(aa1), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
namespace bs = boost::simd;
namespace bd = boost::dispatch;
T a1[N], b[N];
bd::as_integer_t<T> a2 = 2;
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
b[i] = bs::pow2(a1[i], a2);
}
p_t aa1(&a1[0], &a1[N]);
p_t bb(&b[0], &b[N]);//logical
STF_IEEE_EQUAL(bs::pow2(aa1, a2), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
namespace bd = boost::dispatch;
using p_t = bs::pack<T, N>;
using iT = bd::as_integer_t<T>;
using pi_t = bs::pack<iT, N>;
T a1[N];
iT b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = T(i+1);
b[i] = bs::firstbitset(a1[i]);
}
p_t aa1(&a1[0], &a1[N]);
pi_t bb(&b[0], &b[N]);
STF_EQUAL(bs::firstbitset(aa1), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
using pl_t = bs::pack<bs::logical<T>, N>;
T a1[N], a2[N];
bs::logical<T> b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(2*i);
a2[i] = (i%2) ? T(i) : T(2*i+1);
b[i] = bs::is_less(a1[i], a2[i]);
}
p_t aa1(&a1[0], &a1[0]+N);
p_t aa2(&a2[0], &a2[0]+N);
pl_t bb(&b[0], &b[0]+N);
STF_EQUAL(bs::is_less(aa1, aa2), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
namespace bs = boost::simd;
namespace bd = boost::dispatch;
T a1[N], a2[N], b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
a2[i] = (i%2) ? T(i+N) : T(-(i+N));
b[i] = bs::nextafter(a1[i], a2[i]);
}
p_t aa1(&a1[0], &a1[N]);
p_t aa2(&a2[0], &a2[N]);
p_t bb(&b[0], &b[N]);
STF_IEEE_EQUAL(bs::nextafter(aa1, aa2), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
namespace bs = boost::simd;
namespace bd = boost::dispatch;
T a1[N], a2[N];
bool b = true; ;
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
a2[i] = (i%2) ? T(i+N) : T(-(i+N));
b = b && bs::is_included_c(a1[i], a2[i]);
}
p_t aa1(&a1[0], &a1[N]);
p_t aa2(&a2[0], &a2[N]);
STF_IEEE_EQUAL(bs::is_included_c(aa1, aa2), b);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
T a1[N], a2[N];
bs::logical<T> b = true, c = true;
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
a2[i] = (i%2) ? T(i+1) : T(-i);
b = b && a1[i]!= 0;
c = c && a2[i]!= 0;
}
p_t aa1(&a1[0], &a1[N]);
p_t aa2(&a2[0], &a2[N]);
STF_EQUAL(bs::all(aa1), b);
STF_EQUAL(bs::all(aa2), c);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
namespace bs = boost::simd;
namespace bd = boost::dispatch;
T a1[N], b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = T(N-i-1);
b[i] = T(i);
}
p_t aa1(&a1[0], &a1[N]);
p_t bb (&b[0], &b[N]);
std::cout << aa1 << std::endl;
std::cout << bb << std::endl;
std::cout << bs::sort(aa1) << std::endl;
STF_IEEE_EQUAL(bs::sort(aa1), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
namespace bd = boost::dispatch;
using p_t = bs::pack<T, N>;
using fT = bd::as_floating_t<T>;
using f_t =bs::pack<fT, N>;
T a1[N];
fT b[N];
for(int i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
b[i] = bs::bitfloating(a1[i]) ;
}
p_t aa1(&a1[0], &a1[N]);
f_t bb (&b[0], &b[N]);
STF_IEEE_EQUAL(bs::bitfloating(aa1), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
namespace bd = boost::dispatch;
using p_t = bs::pack<T, N>;
using iT = bd::as_integer_t<T>;
using i_t = bs::pack<iT, N>;
T a1[N], b[N];
iT a2[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
a2[i] = i;
b[i] = bs::predecessor(a1[i], a2[i]);
}
p_t aa1(&a1[0], &a1[N]);
i_t aa2(&a2[0], &a2[N]);
p_t bb(&b[0], &b[N]);//logical
STF_IEEE_EQUAL(bs::predecessor(aa1, aa2), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
namespace bd = boost::dispatch;
using p_t = bs::pack<T, N>;
using iT = bd::as_integer_t<T, unsigned>;
using i_t = bs::pack<iT, N>;
T a1[N];
iT b[N];
for(std::size_t i = 0; i < N; ++i)
{
a1[i] = T(N+i+1);
b[i] = bs::hi(a1[i]);
}
p_t aa1(&a1[0], &a1[N]);
i_t bb(&b[0], &b[N]);
std::cout << aa1 << std::endl;
std::cout << bb << std::endl;
STF_EQUAL(bs::hi(aa1), bb);
}
void test(Env& $)
{
namespace bs = boost::simd;
using p_t = bs::pack<T, N>;
namespace bs = boost::simd;
namespace bd = boost::dispatch;
T a1[N], a2[N], b[N];
for(int i = 0; i < N; ++i)
{
a1[i] = (i%2) ? T(i) : T(-i);
a2[i] = (i%2) ? T(i+N) : T(-(i+N));
}
for(int i = 0; i < N; ++i)
{
b[i] = (i%2) ? a1[i] : a2[i-1];
}
p_t aa1(&a1[0], &a1[N]);
p_t aa2(&a2[0], &a2[N]);
p_t bb(&b[0], &b[N]);
STF_IEEE_EQUAL(bs::interleave_even(aa1, aa2), bb);
}
/** Incomplete beta function for variable objects.
Evaluates the continued fraction for imcomplete beta function.
\param _a \f$a\f$
\param _b \f$b\f$
\param _x \f$x\f$
\param MAXIT Maximum number of iterations for the continued fraction approximation in betacf.
\return Incomplete beta function \f$I_x(a,b)\f$
\n\n The implementation of this algorithm was inspired by
"Numerical Recipes in C", 2nd edition,
Press, Teukolsky, Vetterling, Flannery, chapter 2
*/
dvariable betacf(const dvariable& _a, const dvariable& _b, const dvariable& _x,
int MAXIT)
{
double qab,qam,qap;
double a=value(_a);
double b=value(_b);
double x=value(_x);
qab=a+b;
qap=a+1.0;
qam=a-1.0;
dvector c1(0,MAXIT);
dvector c(1,MAXIT);
dvector d1(0,MAXIT);
dvector d(1,MAXIT);
dvector del(1,MAXIT);
dvector h1(0,MAXIT);
dvector h(1,MAXIT);
dvector aa(1,MAXIT);
dvector aa1(1,MAXIT);
c1(0)=1.0;
d1(0)=1.0/(1.0-qab*x/qap);
h1(0)=d1(0);
int m = 1;
for (; m <= MAXIT; m++)
{
int i=m;
int m2=2*m;
aa(i)=m*(b-m)*x/((qam+m2)*(a+m2));
d(i)=1.0/(1.0+aa(i)*d1(i-1));
c(i)=1.0+aa(i)/c1(i-1);
h(i) = h1(i-1)*d(i)*c(i);
aa1(i) = -(a+m)*(qab+m)*x/((a+m2)*(qap+m2));
d1(i)=1.0/(1.0+aa1(i)*d(i));
c1(i)=1.0+aa1(i)/c(i);
del(i)=d1(i)*c1(i);
h1(i) = h(i)*del(i);
if (fabs(del(i)-1.0) < EPS) break;
}
if (m > MAXIT)
{
cerr << "a or b too big, or MAXIT too small in cumulative beta function"
" routine" << endl;
m=MAXIT;
}
int mmax=m;
dvariable hh;
value(hh)=h1(mmax);
dvector dfc1(0,MAXIT);
dvector dfc(1,MAXIT);
dvector dfd1(0,MAXIT);
dvector dfd(1,MAXIT);
dvector dfh1(0,MAXIT);
dvector dfh(1,MAXIT);
dvector dfaa(1,MAXIT);
dvector dfaa1(1,MAXIT);
dvector dfdel(1,MAXIT);
dfc1.initialize();
dfc.initialize();
dfaa1.initialize();
dfaa.initialize();
dfd1.initialize();
dfd.initialize();
dfh1.initialize();
dfh.initialize();
dfdel.initialize();
dfh1(mmax)=1.0;
double dfqab=0.0;
double dfqam=0.0;
double dfqap=0.0;
double dfa=0.0;
double dfb=0.0;
double dfx=0.0;
for (m=mmax;m>=1;m--)
{
/*
int i=m;
m2=2*m;
aa(i)=m*(b-m)*x/((qam+m2)*(a+m2));
d(i)=1.0/(1.0+aa(i)*d1(i-1));
c(i)=1.0+aa(i)/c1(i-1);
h(i) = h1(i-1)*d(i)*c(i);
aa1(i) = -(a+m)*(qab+m)*x/((a+m2)*(qap+m2));
d1(i)=1.0/(1.0+aa1(i)*d(i));
c1(i)=1.0+aa1(i)/c(i);
del(i)=d1(i)*c1(i);
h1(i) = h(i)*del(i);
*/
int i=m;
int m2=2*m;
//h1(i) = h(i)*del(i);
dfh(i)+=dfh1(i)*del(i);
dfdel(i)+=dfh1(i)*h(i);
dfh1(i)=0.0;
//del(i)=d1(i)*c1(i);
dfd1(i)+=dfdel(i)*c1(i);
dfc1(i)+=dfdel(i)*d1(i);
dfdel(i)=0.0;
//c1(i)=1.0+aa1(i)/c(i);
dfaa1(i)+=dfc1(i)/c(i);
dfc(i)-=dfc1(i)*aa1(i)/(c(i)*c(i));
dfc1(i)=0.0;
//d1(i)=1.0/(1.0+aa1(i)*d(i));
double sq=square(d1(i));
dfaa1(i)-=dfd1(i)*sq*d(i);
dfd(i)-=dfd1(i)*sq*aa1(i);
dfd1(i)=0.0;
//aa1(i) = -(a+m)*(qab+m)*x/((a+m2)*(qap+m2));
dfx -= dfaa1(i) *
(a+m)*(qab+m)/((a+m2)*(qap+m2));
dfa += dfaa1(i) * aa1(i)* (1.0/(a+m) - 1.0/(a+m2));
dfqab += dfaa1(i) * aa1(i)/(qab+m);
dfqap += dfaa1(i) * aa1(i)* (-1.0/(qap+m2));
dfaa1(i)=0.0;
//h(i) = h1(i-1)*d(i)*c(i);
dfh1(i-1)+=dfh(i)*d(i)*c(i);
dfd(i)+=dfh(i)*h1(i-1)*c(i);
dfc(i)+=dfh(i)*h1(i-1)*d(i);
dfh(i)=0.0;
//c(i)=1.0+aa(i)/c1(i-1);
dfaa(i)+=dfc(i)/c1(i-1);
dfc1(i-1)-=dfc(i)*aa(i)/square(c1(i-1));
dfc(i)=0.0;
//d(i)=1.0/(1.0+aa(i)*d1(i-1));
dfaa(i)-=dfd(i)*square(d(i))*d1(i-1);
dfd1(i-1)-=dfd(i)*square(d(i))*aa(i);
dfd(i)=0.0;
//aa(i)=m*(b-m)*x/((qam+m2)*(a+m2));
dfx+=dfaa(i)*
m*(b-m)/((qam+m2)*(a+m2));
dfb+=dfaa(i)*
m*x/((qam+m2)*(a+m2));
dfa-=dfaa(i)*aa(i)/(a+m2);
dfqam-=dfaa(i)*aa(i)/(qam+m2);
dfaa(i)=0.0;
}
/*
c1(0)=1.0;
d1(0)=1.0/(1.0-qab*x/qap);
h1(0)=d1(0);
*/
//h1(0)=d1(0);
dfd1(0)+=dfh1(0);
dfh1(0)=0.0;
//d1(0)=1.0/(1.0-qab*x/qap);
double sq1=square(d1(0))/qap;
dfx+=dfd1(0)*sq1*qab;
dfqab+=dfd1(0)*sq1*x;
dfqap-=dfd1(0)*sq1*qab*x/qap;
dfd1(0)=0.0;
/*
qab=a+b;
qap=a+1.0;
qam=a-1.0;
*/
//qam=a-1.0;
dfa+=dfqam;
//qap=a+1.0;
dfa+=dfqap;
//qab=a+b;
dfa+=dfqab;
dfb+=dfqab;
gradient_structure::GRAD_STACK1->set_gradient_stack(default_evaluation3ind,
&(value(hh)) ,&(value(_a)),dfa ,&(value(_b)),dfb ,&(value(_x)),dfx);
return hh;
}
void
ExtendedHandEyeCalibration::solve(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& H1,
const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> >& H2,
Eigen::Matrix4d& H_12,
double& s) const
{
int motionCount = H1.size();
Eigen::MatrixXd T(motionCount * 6, 8);
T.setZero();
for (int i = 0; i < motionCount; ++i)
{
Eigen::AngleAxisd aa1(H1.at(i).block<3,3>(0,0));
Eigen::Vector3d rvec1 = aa1.angle() * aa1.axis();
Eigen::Vector3d tvec1 = H1.at(i).block<3,1>(0,3);
Eigen::AngleAxisd aa2(H2.at(i).block<3,3>(0,0));
Eigen::Vector3d rvec2 = aa2.angle() * aa2.axis();
Eigen::Vector3d tvec2 = H2.at(i).block<3,1>(0,3);
double theta1, d1;
Eigen::Vector3d l1, m1;
AngleAxisAndTranslationToScrew(rvec1, tvec1, theta1, d1, l1, m1);
double theta2, d2;
Eigen::Vector3d l2, m2;
AngleAxisAndTranslationToScrew(rvec2, tvec2, theta2, d2, l2, m2);
Eigen::Vector3d a = l1;
Eigen::Vector3d a_prime = m1;
Eigen::Vector3d b = l2;
Eigen::Vector3d b_prime = m2;
T.block<3,1>(i * 6, 0) = a - b;
T.block<3,3>(i * 6, 1) = skew(Eigen::Vector3d(a + b));
T.block<3,1>(i * 6 + 3, 0) = a_prime - b_prime;
T.block<3,3>(i * 6 + 3, 1) = skew(Eigen::Vector3d(a_prime + b_prime));
T.block<3,1>(i * 6 + 3, 4) = a - b;
T.block<3,3>(i * 6 + 3, 5) = skew(Eigen::Vector3d(a + b));
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeFullU | Eigen::ComputeFullV);
// v7 and v8 span the null space of T, v6 may also be one
// if rank = 5.
Eigen::Matrix<double, 8, 1> v6 = svd.matrixV().block<8,1>(0, 5);
Eigen::Matrix<double, 8, 1> v7 = svd.matrixV().block<8,1>(0,6);
Eigen::Matrix<double, 8, 1> v8 = svd.matrixV().block<8,1>(0,7);
Eigen::Vector4d u1 = v7.block<4,1>(0,0);
Eigen::Vector4d v1 = v7.block<4,1>(4,0);
Eigen::Vector4d u2 = v8.block<4,1>(0,0);
Eigen::Vector4d v2 = v8.block<4,1>(4,0);
double lambda1 = 0;
double lambda2 = 0.0;
if (u1.dot(v1) == 0.0)
{
std::swap(u1, u2);
std::swap(v1, v2);
}
if (u1.dot(v1) != 0.0)
{
double s[2];
solveQuadraticEquation(u1.dot(v1), u1.dot(v2) + u2.dot(v1), u2.dot(v2), s[0], s[1]);
// find better solution for s
double t[2];
for (int i = 0; i < 2; ++i)
{
t[i] = s[i] * s[i] * u1.dot(u1) + 2 * s[i] * u1.dot(u2) + u2.dot(u2);
}
int idx;
if (t[0] > t[1])
{
idx = 0;
}
else
{
idx = 1;
}
lambda2 = sqrt(1.0 / t[idx]);
lambda1 = s[idx] * lambda2;
}
else
{
if (u1.norm() == 0.0 && u2.norm() > 0.0)
{
lambda1 = 0.0;
lambda2 = 1.0 / u2.norm();
}
else if (u2.norm() == 0.0 && u1.norm() > 0.0)
{
lambda1 = 1.0 / u1.norm();
lambda2 = 0.0;
}
}
// rotation
Eigen::Vector4d q_coeffs = lambda1 * u1 + lambda2 * u2;
Eigen::Vector4d q_prime_coeffs = lambda1 * v1 + lambda2 * v2;
Eigen::Quaterniond q(q_coeffs(0), q_coeffs(1), q_coeffs(2), q_coeffs(3));
Eigen::Quaterniond d(q_prime_coeffs(0), q_prime_coeffs(1), q_prime_coeffs(2), q_prime_coeffs(3));
DualQuaterniond dq(q, d);
H_12 = dq.toMatrix().inverse();
s = 1.0;
refine(H1, H2, H_12, s);
}
