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 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;
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;
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);
}
int main()
{
double w,x,y,z;
double ha, angle(M_PI/180.0*60.0);
ha = 0.5*angle;
w = cos(ha);
x = 0.0;
y = sin(ha);
z = 0.0;
creek::Quaternion q0(w,x,y,z);
std::cout << q0.w() << ", " << q0.x() << ", " << q0.y() << ", " << q0.z() << std::endl;
split();
// q1 -> q0
creek::Quaternion q1(1,0,0,0);
creek::Quaternion qs;
qs = q1.slerp(0.9, q0);
std::cout << qs.w() << ", " << qs.x() << ", " << qs.y() << ", " << qs.z() << std::endl;
split();
// Quaternion -> Rotation Matrix (3x3)
creek::Matrix3 mat;
mat = qs.toRotationMatrix();
for(int i=0; i<3; i++) {
for(int j=0; j<3; j++) {
std::cout << " " << mat(i,j);
}
std::cout << std::endl;
}
split();
// Rotation Matrix -> Quaternion
creek::Quaternion qm(mat);
std::cout << qm.w() << ", " << qm.x() << ", " << qm.y() << ", " << qm.z() << std::endl;
split();
// check convert Quaternion <-> Rotation Matrix
mat = qm.toRotationMatrix();
for(int i=0; i<3; i++) {
for(int j=0; j<3; j++) {
std::cout << " " << mat(i,j);
}
std::cout << std::endl;
}
split();
// 逆クオータニオン
creek::Quaternion q_inv = qm.inverse();
// 共役クオータニオン
creek::Quaternion q_con = qm.conjugate();
// 回転ベクトルの長さ
creek::Quaternion::Scalar norm = qm.norm();
// 正規化
creek::Quaternion qn(2,1,3,4);
//qn.normalize();
std::cout << qn.w() << ", " << qn.x() << ", " << qn.y() << ", " << qn.z() << std::endl;
creek::Quaternion q_norm = qn.normalized();
std::cout << q_norm.w() << ", " << q_norm.x() << ", " << q_norm.y() << ", " << q_norm.z() << std::endl;
split();
// template float <-> double
#ifdef USE_CNOID_MODEL
Eigen::Quaternion<float> qf(1,2,3,4);
#elif defined USE_HRP_MODEL
creek_tvmet::Quaternion<float> qf(1,2,3,4);
#endif
creek::Quaternion qfd(qf);
std::cout << qfd.w() << ", " << qfd.x() << ", " << qfd.y() << ", " << qfd.z() << std::endl;
#ifdef USE_HRP_MODEL
split();
qn.normalize();
qf.normalize();
std::cout << qn.w() << ", " << qn.x() << ", " << qn.y() << ", " << qn.z() << std::endl;
std::cout << qf.w() << ", " << qf.x() << ", " << qf.y() << ", " << qf.z() << std::endl;
qn = qf.slerp(0.5, qn);
std::cout << qn.w() << ", " << qn.x() << ", " << qn.y() << ", " << qn.z() << std::endl;
#endif
split();
creek::Vector3 vec; vec << 0,1,0;
creek::AngleAxis aa(0.6, vec);
creek::Quaternion qaa(aa);
std::cout << qaa.w() << ", " << qaa.x() << ", " << qaa.y() << ", " << qaa.z() << std::endl;
split();
aa = qaa;
//aa = mat;
std::cout << aa.angle() << std::endl;
std::cout << aa.axis()(0) << ", " << aa.axis()(1) << ", " << aa.axis()(2) << std::endl;
split();
mat = aa.toRotationMatrix();
std::cout << mat << std::endl;
split();
creek::AngleAxis aa2(0.4, creek::Vector3::UnitX());
std::cout << aa2.angle() << std::endl;
std::cout << aa2.axis()(0) << ", " << aa2.axis()(1) << ", " << aa2.axis()(2) << std::endl;
creek::Vector3 veca, vecb;
veca << 1,2,3;
vecb << 2,3,4;
double dot = veca.dot(vecb);
std::cout << dot << std::endl;
creek::Vector3 cross;
cross = veca.cross(vecb);
//cross = veca.normalized();
std::cout << cross << std::endl;
std::cout << creek::Vector3::UnitY() << std::endl;
split();
creek::Position pos;
pos.translation() = creek::Vector3::Zero();
pos.linear() = creek::Matrix3::Identity();
std::cout << pos.translation() << std::endl << pos.linear() << std::endl;
return 0;
}
/** unittest for oapackage
*
* Returns UNITTEST_SUCCESS if all tests are ok.
*
*/
int oaunittest (int verbose, int writetests = 0, int randval = 0) {
double t0 = get_time_ms ();
const char *bstr = "OA unittest";
cprintf (verbose, "%s: start\n", bstr);
srand (randval);
int allgood = UNITTEST_SUCCESS;
Combinations::initialize_number_combinations (20);
/* constructors */
{
cprintf (verbose, "%s: interaction matrices\n", bstr);
array_link al = exampleArray (2);
Eigen::MatrixXd m1 = array2xfeigen (al);
Eigen::MatrixXd m2 = arraylink2eigen (array2xf (al));
Eigen::MatrixXd dm = m1 - m2;
int sum = dm.sum ();
myassert (sum == 0, "unittest error: construction of interaction matrices\n");
}
cprintf(verbose, "%s: reduceConferenceTransformation\n", bstr);
myassert(unittest_reduceConferenceTransformation()==0, "unittest unittest_reduceConferenceTransformation failed");
/* constructors */
{
cprintf (verbose, "%s: array manipulation operations\n", bstr);
test_array_manipulation (verbose);
}
/* double conference matrices */
{
cprintf (verbose, "%s: double conference matrices\n", bstr);
array_link al = exampleArray (36, verbose);
myassert (al.is_conference (2), "check on double conference design type");
myassert (testLMC0checkDC (al, verbose >= 2), "testLMC0checkDC");
}
/* conference matrices */
{
cprintf (verbose, "%s: conference matrices\n", bstr);
int N = 4;
conference_t ctype (N, N, 0);
arraylist_t kk;
array_link al = ctype.create_root ();
kk.push_back (al);
for (int extcol = 2; extcol < N; extcol++) {
kk = extend_conference (kk, ctype, 0);
}
myassert (kk.size () == 1, "unittest error: conference matrices for N=4\n");
}
{
cprintf (verbose, "%s: generators for conference matrix extensions\n", bstr);
test_conference_candidate_generators (verbose);
}
{
cprintf (verbose, "%s: conference matrix Fvalues\n", bstr);
array_link al = exampleArray (22, 0);
if (verbose >= 2)
al.show ();
if (0) {
std::vector< int > f3 = al.FvaluesConference (3);
if (verbose >= 2) {
printf ("F3: ");
display_vector (f3);
printf ("\n");
}
}
const int N = al.n_rows;
jstructconference_t js (N, 4);
std::vector< int > f4 = al.FvaluesConference (4);
std::vector< int > j4 = js.Jvalues ();
if (verbose >= 2) {
printf ("j4: ");
display_vector (j4);
printf ("\n");
printf ("F4: ");
display_vector (f4);
printf ("\n");
}
myassert (j4[0] == 28, "unittest error: conference matricex F values: j4[0]\n");
myassert (f4[0] == 0, "unittest error: conference matricex F values: f4[0] \n");
myassert (f4[1] == 0, "unittest error: conference matricex F values: j4[1]\n");
}
{
cprintf (verbose, "%s: LMC0 check for arrays in C(4, 3)\n", bstr);
array_link al = exampleArray (28, 1);
if (verbose >= 2)
al.showarray ();
lmc_t r = LMC0check (al, verbose);
if (verbose >= 2)
printf ("LMC0check: result %d\n", r);
myassert (r >= LMC_EQUAL, "LMC0 check\n");
al = exampleArray (29, 1);
if (verbose >= 2)
al.showarray ();
r = LMC0check (al, verbose);
if (verbose >= 2)
printf ("LMC0check: result %d (LMC_LESS %d)\n", r, LMC_LESS);
myassert (r == LMC_LESS, "LMC0 check of example array 29\n");
}
{
cprintf (verbose, "%s: LMC0 check\n", bstr);
array_link al = exampleArray (31, 1);
if (verbose >= 2)
al.showarray ();
conference_transformation_t T (al);
for (int i = 0; i < 80; i++) {
T.randomize ();
array_link alx = T.apply (al);
lmc_t r = LMC0check (alx, verbose);
if (verbose >= 2) {
printfd ("%d: transformed array: r %d\n", i, r);
alx.showarray ();
}
if (alx == al)
myassert (r >= LMC_EQUAL, "result should be LMC_MORE\n");
else {
myassert (r == LMC_LESS, "result should be LMC_LESS\n");
}
}
}
{
cprintf (verbose, "%s: random transformation for conference matrices\n", bstr);
array_link al = exampleArray (19, 1);
conference_transformation_t T (al);
// T.randomizerowflips();
T.randomize ();
conference_transformation_t Ti = T.inverse ();
array_link alx = Ti.apply (T.apply (al));
if (0) {
printf ("input array:\n");
al.showarray ();
T.show ();
printf ("transformed array:\n");
T.apply (al).showarray ();
Ti.show ();
alx.showarray ();
}
myassert (alx == al, "transformation of conference matrix\n");
}
/* constructors */
{
cprintf (verbose, "%s: constructors\n", bstr);
array_transformation_t t;
conference_transformation_t ct;
}
/* J-characteristics */
{
cprintf (verbose, "%s: J-characteristics\n", bstr);
array_link al = exampleArray (8, 1);
const int mm[] = {-1, -1, 0, 0, 8, 16, 0, -1};
for (int jj = 2; jj < 7; jj++) {
std::vector< int > jx = al.Jcharacteristics (jj);
int j5max = vectormax (jx, 0);
if (verbose >= 2) {
printf ("oaunittest: jj %d: j5max %d\n", jj, j5max);
}
if (j5max != mm[jj]) {
printfd ("j5max %d (should be %d)\n", j5max, mm[jj]);
allgood = UNITTEST_FAIL;
return allgood;
}
}
}
{
cprintf (verbose, "%s: array transformations\n", bstr);
const int N = 9;
const int t = 3;
arraydata_t adataX (3, N, t, 4);
array_link al (adataX.N, adataX.ncols, -1);
al.create_root (adataX);
if (checkTransformationInverse (al))
allgood = UNITTEST_FAIL;
if (checkTransformationComposition (al, verbose >= 2))
allgood = UNITTEST_FAIL;
al = exampleArray (5, 1);
if (checkTransformationInverse (al))
allgood = UNITTEST_FAIL;
if (checkTransformationComposition (al))
allgood = UNITTEST_FAIL;
for (int i = 0; i < 15; i++) {
al = exampleArray (18, 0);
if (checkConferenceComposition (al))
allgood = UNITTEST_FAIL;
if (checkConferenceInverse (al))
allgood = UNITTEST_FAIL;
al = exampleArray (19, 0);
if (checkConferenceComposition (al))
allgood = UNITTEST_FAIL;
if (checkConferenceInverse (al))
allgood = UNITTEST_FAIL;
}
}
{
cprintf (verbose, "%s: rank \n", bstr);
const int idx[10] = {0, 1, 2, 3, 4, 6, 7, 8, 9};
const int rr[10] = {4, 11, 13, 18, 16, 4, 4, 29, 29};
for (int ii = 0; ii < 9; ii++) {
array_link al = exampleArray (idx[ii], 0);
myassert (al.is2level (), "unittest error: input array is not 2-level\n");
int r = arrayrankColPivQR (array2xf (al));
int r3 = (array2xf (al)).rank ();
myassert (r == r3, "unittest error: rank of array");
if (verbose >= 2) {
al.showarray ();
printf ("unittest: rank of array %d: %d\n", idx[ii], r);
}
myassert (rr[ii] == r, "unittest error: rank of example matrix\n");
}
}
{
cprintf (verbose, "%s: Doptimize \n", bstr);
const int N = 40;
const int t = 0;
arraydata_t arrayclass (2, N, t, 6);
std::vector< double > alpha (3);
alpha[0] = 1;
alpha[1] = 1;
alpha[2] = 0;
int niter = 5000;
double t00 = get_time_ms ();
DoptimReturn rr = Doptimize (arrayclass, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);
array_t ss[7] = {3, 3, 2, 2, 2, 2, 2};
arraydata_t arrayclassmixed (ss, 36, t, 5);
rr = Doptimize (arrayclassmixed, 10, alpha, 0, DOPTIM_AUTOMATIC, niter);
cprintf (verbose, "%s: Doptimize time %.3f [s] \n", bstr, get_time_ms () - t00);
}
{
cprintf (verbose, "%s: J-characteristics for conference matrix\n", bstr);
array_link al = exampleArray (19, 0);
std::vector< int > j2 = Jcharacteristics_conference (al, 2);
std::vector< int > j3 = Jcharacteristics_conference (al, 3);
myassert (j2[0] == 0, "j2 value incorrect");
myassert (j2[1] == 0, "j2 value incorrect");
myassert (std::abs (j3[0]) == 1, "j3 value incorrect");
if (verbose >= 2) {
al.showarray ();
printf ("j2: ");
display_vector (j2);
printf ("\n");
printf ("j3: ");
display_vector (j3);
printf ("\n");
}
}
{
// test PEC sequence
cprintf (verbose, "%s: PEC sequence\n", bstr);
for (int ii = 0; ii < 5; ii++) {
array_link al = exampleArray (ii, 0);
std::vector< double > pec = PECsequence (al);
printf ("oaunittest: PEC for array %d: ", ii);
display_vector (pec);
printf (" \n");
}
}
{
cprintf (verbose, "%s: D-efficiency test\n", bstr);
// D-efficiency near-zero test
{
array_link al = exampleArray (14);
double D = al.Defficiency ();
std::vector< double > dd = al.Defficiencies ();
printf ("D %f, D (method 2) %f\n", D, dd[0]);
assert (fabs (D - dd[0]) < 1e-4);
}
{
array_link al = exampleArray (15);
double D = al.Defficiency ();
std::vector< double > dd = al.Defficiencies ();
printf ("D %f, D (method 2) %f\n", D, dd[0]);
assert (fabs (D - dd[0]) < 1e-4);
assert (fabs (D - 0.335063) < 1e-3);
}
}
arraydata_t adata (2, 20, 2, 6);
OAextend oaextendx;
oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);
std::vector< arraylist_t > aa (adata.ncols + 1);
printf ("OA unittest: create root array\n");
create_root (&adata, aa[adata.strength]);
/** Test extend of arrays **/
{
cprintf (verbose, "%s: extend arrays\n", bstr);
setloglevel (SYSTEM);
for (int kk = adata.strength; kk < adata.ncols; kk++) {
aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
printf (" extend: column %d->%d: %ld->%ld arrays\n", kk, kk + 1, aa[kk].size (),
aa[kk + 1].size ());
}
if (aa[adata.ncols].size () != 75) {
printf ("extended ?? to %d arrays\n", (int)aa[adata.ncols].size ());
}
myassert (aa[adata.ncols].size () == 75, "number of arrays is incorrect");
aa[adata.ncols].size ();
setloglevel (QUIET);
}
{
cprintf (verbose, "%s: test LMC check\n", bstr);
array_link al = exampleArray (1, 1);
lmc_t r = LMCcheckOriginal (al);
myassert (r != LMC_LESS, "LMC check of array in normal form");
for (int i = 0; i < 20; i++) {
array_link alx = al.randomperm ();
if (alx == al)
continue;
lmc_t r = LMCcheckOriginal (alx);
myassert (r == LMC_LESS, "randomized array cannot be in minimal form");
}
}
{
/** Test dof **/
cprintf (verbose, "%s: test delete-one-factor reduction\n", bstr);
array_link al = exampleArray (4);
cprintf (verbose >= 2, "LMC: \n");
al.reduceLMC ();
cprintf (verbose >= 2, "DOP: \n");
al.reduceDOP ();
}
arraylist_t lst;
{
/** Test different methods **/
cprintf (verbose, "%s: test 2 different methods\n", bstr);
const int s = 2;
arraydata_t adata (s, 32, 3, 10);
arraydata_t adata2 (s, 32, 3, 10);
OAextend oaextendx;
oaextendx.setAlgorithm ((algorithm_t)MODE_ORIGINAL, &adata);
OAextend oaextendx2;
oaextendx2.setAlgorithm ((algorithm_t)MODE_LMC_2LEVEL, &adata2);
printf ("OA unittest: test 2-level algorithm on %s\n", adata.showstr ().c_str ());
std::vector< arraylist_t > aa (adata.ncols + 1);
create_root (&adata, aa[adata.strength]);
std::vector< arraylist_t > aa2 (adata.ncols + 1);
create_root (&adata, aa2[adata.strength]);
setloglevel (SYSTEM);
for (int kk = adata.strength; kk < adata.ncols; kk++) {
aa[kk + 1] = extend_arraylist (aa[kk], adata, oaextendx);
aa2[kk + 1] = extend_arraylist (aa2[kk], adata2, oaextendx2);
printf (" extend: column %d->%d: %ld->%ld arrays, 2-level method %ld->%ld arrays\n", kk,
kk + 1, (long) aa[kk].size (), (long)aa[kk + 1].size (), aa2[kk].size (), aa2[kk + 1].size ());
if (aa[kk + 1] != aa2[kk + 1]) {
printf ("oaunittest: error: 2-level algorithm unequal to original algorithm\n");
exit (1);
}
}
setloglevel (QUIET);
lst = aa[8];
}
{
cprintf (verbose, "%s: rank calculation using rankStructure\n", bstr);
for (int i = 0; i < 27; i++) {
array_link al = exampleArray (i, 0);
if (al.n_columns < 5)
continue;
al = exampleArray (i, 1);
rankStructure rs;
rs.verbose = 0;
int r = array2xf (al).rank ();
int rc = rs.rankxf (al);
if (verbose >= 2) {
printf ("rank of example array %d: %d %d\n", i, r, rc);
if (verbose >= 3) {
al.showproperties ();
}
}
myassert (r == rc, "rank calculations");
}
}
{
cprintf (verbose, "%s: test dtable creation\n", bstr);
for (int i = 0; i < 4; i++) {
array_link al = exampleArray (5);
array_link dtable = createJdtable (al);
}
}
{
cprintf (verbose, "%s: test Pareto calculation\n", bstr);
double t0x = get_time_ms ();
int nn = lst.size ();
for (int k = 0; k < 5; k++) {
for (int i = 0; i < nn; i++) {
lst.push_back (lst[i]);
}
}
Pareto< mvalue_t< long >, long > r = parsePareto (lst, 1);
cprintf (verbose, "%s: test Pareto %d/%d: %.3f [s]\n", bstr, r.number (), r.numberindices (),
(get_time_ms () - t0x));
}
{
cprintf (verbose, "%s: check reduction transformation\n", bstr);
array_link al = exampleArray (6).reduceLMC ();
arraydata_t adata = arraylink2arraydata (al);
LMCreduction_t reduction (&adata);
reduction.mode = OA_REDUCE;
reduction.init_state = COPY;
OAextend oaextend;
oaextend.setAlgorithm (MODE_ORIGINAL, &adata);
array_link alr = al.randomperm ();
array_link al2 = reduction.transformation->apply (al);
lmc_t tmp = LMCcheck (alr, adata, oaextend, reduction);
array_link alx = reduction.transformation->apply (alr);
bool c = alx == al;
if (!c) {
printf ("oaunittest: error: reduction of randomized array failed!\n");
printf ("-- al \n");
al.showarraycompact ();
printf ("-- alr \n");
alr.showarraycompact ();
printf ("-- alx \n");
alx.showarraycompact ();
allgood = UNITTEST_FAIL;
}
}
{
cprintf (verbose, "%s: reduce randomized array\n", bstr);
array_link al = exampleArray (3);
arraydata_t adata = arraylink2arraydata (al);
LMCreduction_t reduction (&adata);
for (int ii = 0; ii < 50; ii++) {
reduction.transformation->randomize ();
array_link al2 = reduction.transformation->apply (al);
array_link alr = al2.reduceLMC ();
if (0) {
printf ("\n reduction complete:\n");
al2.showarray ();
printf (" --->\n");
alr.showarray ();
}
bool c = (al == alr);
if (!c) {
printf ("oaunittest: error: reduction of randomized array failed!\n");
allgood = UNITTEST_FAIL;
}
}
}
/* Calculate symmetry group */
{
cprintf (verbose, "%s: calculate symmetry group\n", bstr);
array_link al = exampleArray (2);
symmetry_group sg = al.row_symmetry_group ();
assert (sg.permsize () == sg.permsize_large ().toLong ());
// symmetry_group
std::vector< int > vv;
vv.push_back (0);
vv.push_back (0);
vv.push_back (1);
symmetry_group sg2 (vv);
assert (sg2.permsize () == 2);
if (verbose >= 2)
printf ("sg2: %ld\n", sg2.permsize ());
assert (sg2.ngroups == 2);
}
/* Test efficiencies */
{
cprintf (verbose, "%s: efficiencies\n", bstr);
std::vector< double > d;
int vb = 1;
array_link al;
if (1) {
al = exampleArray (9, vb);
al.showproperties ();
d = al.Defficiencies (0, 1);
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);
if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
printf ("oaunittest: error: Defficiency not good!\n");
allgood = UNITTEST_FAIL;
}
}
al = exampleArray (8, vb);
al.showproperties ();
d = al.Defficiencies ();
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
if (fabs (d[0] - al.Defficiency ()) > 1e-10) {
printf ("oaunittest: error: Defficiency of examlple array 8 not good!\n");
}
al = exampleArray (13, vb);
if (verbose >= 3) {
al.showarray ();
al.showproperties ();
}
d = al.Defficiencies (0, 1);
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
if ((fabs (d[0] - 0.939014) > 1e-4) || (fabs (d[3] - 0.896812) > 1e-4) || (fabs (d[2] - 1) > 1e-4)) {
printf ("ERROR: D-efficiencies of example array 13 incorrect! \n");
d = al.Defficiencies (2, 1);
printf (" efficiencies: D %f Ds %f D1 %f Ds0 %f\n", d[0], d[1], d[2], d[3]);
allgood = UNITTEST_FAIL;
exit (1);
}
for (int ii = 11; ii < 11; ii++) {
printf ("ii %d: ", ii);
al = exampleArray (ii, vb);
al.showarray ();
al.showproperties ();
d = al.Defficiencies ();
if (verbose >= 2)
printf (" efficiencies: D %f Ds %f D1 %f\n", d[0], d[1], d[2]);
}
}
{
cprintf (verbose, "%s: test robustness\n", bstr);
array_link A (0, 8, 0);
printf ("should return an error\n ");
A.Defficiencies ();
A = array_link (1, 8, 0);
printf ("should return an error\n ");
A.at (0, 0) = -2;
A.Defficiencies ();
}
{
cprintf (verbose, "%s: test nauty\n", bstr);
array_link alr = exampleArray (7, 0);
if (unittest_nautynormalform (alr, 1) == 0) {
printf ("oaunittest: error: unittest_nautynormalform returns an error!\n");
}
}
#ifdef HAVE_BOOST
if (writetests) {
cprintf (verbose, "OA unittest: reading and writing of files\n");
boost::filesystem::path tmpdir = boost::filesystem::temp_directory_path ();
boost::filesystem::path temp = boost::filesystem::unique_path ("test-%%%%%%%.oa");
const std::string tempstr = (tmpdir / temp).native ();
if (verbose >= 2)
printf ("generate text OA file: %s\n", tempstr.c_str ());
int nrows = 16;
int ncols = 8;
int narrays = 10;
arrayfile_t afile (tempstr.c_str (), nrows, ncols, narrays, ATEXT);
for (int i = 0; i < narrays; i++) {
array_link al (nrows, ncols, array_link::INDEX_DEFAULT);
afile.append_array (al);
}
afile.closefile ();
arrayfile_t af (tempstr.c_str (), 0);
std::cout << " " << af.showstr () << std::endl;
af.closefile ();
// check read/write of binary file
arraylist_t ll0;
ll0.push_back (exampleArray (7));
ll0.push_back (exampleArray (7).randomcolperm ());
writearrayfile (tempstr.c_str (), ll0, ABINARY);
arraylist_t ll = readarrayfile (tempstr.c_str ());
myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
for (size_t i = 0; i < ll0.size (); i++) {
myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
}
ll0.resize (0);
ll0.push_back (exampleArray (24));
writearrayfile (tempstr.c_str (), ll0, ABINARY_DIFFZERO);
ll = readarrayfile (tempstr.c_str ());
myassert (ll0.size () == ll.size (), "read and write of arrays: size of list");
for (size_t i = 0; i < ll0.size (); i++) {
myassert (ll0[i] == ll[i], "read and write of arrays: array unequal");
}
}
#endif
{
cprintf (verbose, "OA unittest: test nauty\n");
array_link al = exampleArray (5, 2);
arraydata_t arrayclass = arraylink2arraydata (al);
for (int i = 0; i < 20; i++) {
array_link alx = al;
alx.randomperm ();
array_transformation_t t1 = reduceOAnauty (al);
array_link alr1 = t1.apply (al);
array_transformation_t t2 = reduceOAnauty (alx);
array_link alr2 = t2.apply (alx);
if (alr1 != alr2)
printf ("oaunittest: error: Nauty reductions unequal!\n");
allgood = UNITTEST_FAIL;
}
}
cprintf (verbose, "OA unittest: complete %.3f [s]!\n", (get_time_ms () - t0));
cprintf (verbose, "OA unittest: also run ptest.py to perform checks!\n");
if (allgood) {
printf ("OA unittest: all tests ok\n");
return UNITTEST_SUCCESS;
} else {
printf ("OA unittest: ERROR!\n");
return UNITTEST_FAIL;
}
}
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);
}
