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