void lcd_reset(){ for(int i = 0 ; i < 3; i++){ D7(1);D6(1);D5(1);D4(1); _delay_ms(20); D7(0);D6(0);D5(1);D4(1); lcd_new_data(); } D7(0);D6(0);D5(1);D4(0); lcd_new_data(); }
double SpinAdapted::StackCreCreDesDes::redMatrixElement(Csf c1, vector<Csf>& ladder, const StackSpinBlock* b) { assert( build_pattern == "(((CC)(D))(D))" ); double element = 0.0; int I = get_orbs()[0]; int J = get_orbs()[1]; int K = get_orbs()[2]; int L = get_orbs()[3]; int Slaterlength = c1.det_rep.begin()->first.size(); vector<bool> backupSlater1(Slaterlength,0), backupSlater2(Slaterlength,0); // Must take into account how the 4-index is built from a combination of the 2-index ops std::vector<SpinQuantum> quantum_ladder = get_quantum_ladder().at("(((CC)(D))(D))"); assert( quantum_ladder.size() == 3 ); SpinQuantum deltaQuantum12 = quantum_ladder.at(0); SpinQuantum deltaQuantum123 = quantum_ladder.at(1); SpinQuantum deltaQuantum1234 = quantum_ladder.at(2); //FIXME components != 0 deltaQuantum[0] = deltaQuantum1234; // Spin quantum data for CC IrrepSpace sym12 = deltaQuantum12.get_symm(); int irrep12 = deltaQuantum12.get_symm().getirrep(); int spin12 = deltaQuantum12.get_s().getirrep(); // Spin quantum data for (CC)D IrrepSpace sym123 = deltaQuantum123.get_symm(); int irrep123 = deltaQuantum123.get_symm().getirrep(); int spin123= deltaQuantum123.get_s().getirrep(); // Spin quantum data for total operator IrrepSpace sym1234 = deltaQuantum1234.get_symm(); int irrep1234 = deltaQuantum1234.get_symm().getirrep(); int spin1234 = deltaQuantum1234.get_s().getirrep(); TensorOp C1(I, 1); TensorOp C2(J, 1); TensorOp D3(K,-1); TensorOp D4(L,-1); TensorOp CC = C1.product(C2, spin12, irrep12); TensorOp CCD = CC.product(D3, spin123, irrep123); TensorOp CCDD = CCD.product(D4, spin1234, irrep1234); //FIXME loop over deltaQuantum components int j = 0; for (int i=0; i<ladder.size(); i++) { int index = 0; double cleb=0.0; if (nonZeroTensorComponent(c1, deltaQuantum[j], ladder[i], index, cleb)) { std::vector<double> MatElements = calcMatrixElements(c1, CCDD, ladder[i], backupSlater1, backupSlater2) ; element = MatElements[index]/cleb; break; } else continue; } return element; }
void lcd_read(unsigned char cmd){ // read first 4 bit D7(0); D6(0);D5(0); D4(0); D7(rd_bit(cmd,7)); D6(rd_bit(cmd,6)); D5(rd_bit(cmd,5)); D4(rd_bit(cmd,4)); lcd_new_data(); // read second 4 bit D7(0); D6(0);D5(0); D4(0); D7(rd_bit(cmd,3)); D6(rd_bit(cmd,2)); D5(rd_bit(cmd,1)); D4(rd_bit(cmd,0)); lcd_new_data(); }
int main () { #if defined (__GXX_ABI_VERSION) && __GXX_ABI_VERSION >= 100 if (Check<B1, C1> ()) return 1; if (Check<B2, C2> ()) return 2; if (Check<B3, C3> ()) return 3; if (Check<B4, C4> ()) return 4; if (Check<B5, C5> ()) return 5; if (Check<B6, C6> ()) return 6; if (Check<B7, C7> ()) return 7; if (Check<B8, C8> ()) return 8; if (Check<C1> (D1 ())) return 11; if (Check<C2> (D2 ())) return 12; if (Check<C3> (D3 ())) return 13; if (Check<C4> (D4 ())) return 14; if (Check<C5> (D5 ())) return 15; if (Check<C6> (D6 ())) return 16; if (Check<C7> (D7 ())) return 17; if (Check<C8> (D8 ())) return 18; if (sizeof (C2) == nearly_empty_size) return 22; if (sizeof (C3) == nearly_empty_size) return 23; if (sizeof (C4) == nearly_empty_size) return 24; if (sizeof (C5) == nearly_empty_size) return 25; if (sizeof (C6) == nearly_empty_size) return 26; if (sizeof (C7) == nearly_empty_size) return 27; if (sizeof (C8) == nearly_empty_size) return 28; #endif return 0; }
double SpinAdapted::CreCreCreDes::redMatrixElement(Csf c1, vector<Csf>& ladder, const SpinBlock* b) { assert( build_pattern == "(((CC)(C))(D))" ); double element = 0.0; int I = get_orbs()[0]; int J = get_orbs()[1]; int K = get_orbs()[2]; int L = get_orbs()[3]; // Must take into account how the 4-index is built from a combination of the 2-index ops std::vector<SpinQuantum> quantum_ladder = get_quantum_ladder().at("(((CC)(C))(D))"); assert( quantum_ladder.size() == 3 ); SpinQuantum deltaQuantum12 = quantum_ladder.at(0); SpinQuantum deltaQuantum123 = quantum_ladder.at(1); SpinQuantum deltaQuantum1234 = quantum_ladder.at(2); deltaQuantum[0] = deltaQuantum1234; // Spin quantum data for CC IrrepSpace sym12 = deltaQuantum12.get_symm(); int irrep12 = deltaQuantum12.get_symm().getirrep(); int spin12 = deltaQuantum12.get_s().getirrep(); // Spin quantum data for (CC)C IrrepSpace sym123 = deltaQuantum123.get_symm(); int irrep123 = deltaQuantum123.get_symm().getirrep(); int spin123= deltaQuantum123.get_s().getirrep(); // Spin quantum data for total operator IrrepSpace sym1234 = deltaQuantum1234.get_symm(); int irrep1234 = deltaQuantum1234.get_symm().getirrep(); int spin1234 = deltaQuantum1234.get_s().getirrep(); TensorOp C1(I, 1); TensorOp C2(J, 1); TensorOp C3(K, 1); TensorOp D4(L,-1); TensorOp CC = C1.product(C2, spin12, irrep12); TensorOp CCC = CC.product(C3, spin123, irrep123); TensorOp CCCD = CCC.product(D4, spin1234, irrep1234); for (int i=0; i<ladder.size(); i++) { int index = 0; double cleb=0.0; if (nonZeroTensorComponent(c1, deltaQuantum[0], ladder[i], index, cleb)) { std::vector<double> MatElements = calcMatrixElements(c1, CCCD, ladder[i]) ; element = MatElements[index]/cleb; break; } else continue; } return element; }
bool mrpt::vision::pnp::rpnp::compute_pose(Eigen::Ref<Eigen::Matrix3d> R_, Eigen::Ref<Eigen::Vector3d> t_) { // selecting an edge $P_{ i1 }P_{ i2 }$ by n random sampling int i1 = 0, i2 = 1; double lmin = Q(0, i1)*Q(0, i2) + Q(1, i1)*Q(1, i2) + Q(2, i1)*Q(2, i2); Eigen::MatrixXi rij (n,2); R_=Eigen::MatrixXd::Identity(3,3); t_=Eigen::Vector3d::Zero(); for (int i = 0; i < n; i++) for (int j = 0; j < 2; j++) rij(i, j) = rand() % n; for (int ii = 0; ii < n; ii++) { int i = rij(ii, 0), j = rij(ii,1); if (i == j) continue; double l = Q(0, i)*Q(0, j) + Q(1, i)*Q(1, j) + Q(2, i)*Q(2, j); if (l < lmin) { i1 = i; i2 = j; lmin = l; } } // calculating the rotation matrix of $O_aX_aY_aZ_a$. Eigen::Vector3d p1, p2, p0, x, y, z, dum_vec; p1 = P.col(i1); p2 = P.col(i2); p0 = (p1 + p2) / 2; x = p2 - p0; x /= x.norm(); if (abs(x(1)) < abs(x(2)) ) { dum_vec << 0, 1, 0; z = x.cross(dum_vec); z /= z.norm(); y = z.cross(x); y /= y.norm(); } else { dum_vec << 0, 0, 1; y = dum_vec.cross(x); y /= y.norm(); z = x.cross(y); x /= x.norm(); } Eigen::Matrix3d R0; R0.col(0) = x; R0.col(1) =y; R0.col(2) = z; for (int i = 0; i < n; i++) P.col(i) = R0.transpose() * (P.col(i) - p0); // Dividing the n - point set into(n - 2) 3 - point subsets // and setting up the P3P equations Eigen::Vector3d v1 = Q.col(i1), v2 = Q.col(i2); double cg1 = v1.dot(v2); double sg1 = sqrt(1 - cg1*cg1); double D1 = (P.col(i1) - P.col(i2)).norm(); Eigen::MatrixXd D4(n - 2, 5); int j = 0; Eigen::Vector3d vi; Eigen::VectorXd rowvec(5); for (int i = 0; i < n; i++) { if (i == i1 || i == i2) continue; vi = Q.col(i); double cg2 = v1.dot(vi); double cg3 = v2.dot(vi); double sg2 = sqrt(1 - cg2*cg2); double D2 = (P.col(i1) - P.col(i)).norm(); double D3 = (P.col(i) - P.col(i2)).norm(); // get the coefficients of the P3P equation from each subset. rowvec = getp3p(cg1, cg2, cg3, sg1, sg2, D1, D2, D3); D4.row(j) = rowvec; j += 1; if(j>n-3) break; } Eigen::VectorXd D7(8), dumvec(8), dumvec1(5); D7.setZero(); for (int i = 0; i < n-2; i++) { dumvec1 = D4.row(i); dumvec= getpoly7(dumvec1); D7 += dumvec; } Eigen::PolynomialSolver<double, 7> psolve(D7.reverse()); Eigen::VectorXcd comp_roots = psolve.roots().transpose(); Eigen::VectorXd real_comp, imag_comp; real_comp = comp_roots.real(); imag_comp = comp_roots.imag(); Eigen::VectorXd::Index max_index; double max_real= real_comp.cwiseAbs().maxCoeff(&max_index); std::vector<double> act_roots_; int cnt=0; for (int i=0; i<imag_comp.size(); i++ ) { if(abs(imag_comp(i))/max_real<0.001) { act_roots_.push_back(real_comp(i)); cnt++; } } double* ptr = &act_roots_[0]; Eigen::Map<Eigen::VectorXd> act_roots(ptr, cnt); if (cnt==0) { return false; } Eigen::VectorXd act_roots1(cnt); act_roots1 << act_roots.segment(0,cnt); std::vector<Eigen::Matrix3d> R_cum(cnt); std::vector<Eigen::Vector3d> t_cum(cnt); std::vector<double> err_cum(cnt); for(int i=0; i<cnt; i++) { double root = act_roots(i); // Compute the rotation matrix double d2 = cg1 + root; Eigen::Vector3d unitx, unity, unitz; unitx << 1,0,0; unity << 0,1,0; unitz << 0,0,1; x = v2*d2 -v1; x/=x.norm(); if (abs(unity.dot(x)) < abs(unitz.dot(x))) { z = x.cross(unity);z/=z.norm(); y=z.cross(x); y/y.norm(); } else { y=unitz.cross(x); y/=y.norm(); z = x.cross(y); z/=z.norm(); } R.col(0)=x; R.col(1)=y; R.col(2)=z; //calculating c, s, tx, ty, tz Eigen::MatrixXd D(2 * n, 6); D.setZero(); R0 = R.transpose(); Eigen::VectorXd r(Eigen::Map<Eigen::VectorXd>(R0.data(), R0.cols()*R0.rows())); for (int j = 0; j<n; j++) { double ui = img_pts(j, 0), vi = img_pts(j, 1), xi = P(0, j), yi = P(1, j), zi = P(2, j); D.row(2 * j) << -r(1)*yi + ui*(r(7)*yi + r(8)*zi) - r(2)*zi, -r(2)*yi + ui*(r(8)*yi - r(7)*zi) + r(1)*zi, -1, 0, ui, ui*r(6)*xi - r(0)*xi; D.row(2 * j + 1) << -r(4)*yi + vi*(r(7)*yi + r(8)*zi) - r(5)*zi, -r(5)*yi + vi*(r(8)*yi - r(7)*zi) + r(4)*zi, 0, -1, vi, vi*r(6)*xi - r(3)*xi; } Eigen::MatrixXd DTD = D.transpose()*D; Eigen::EigenSolver<Eigen::MatrixXd> es(DTD); Eigen::VectorXd Diag = es.pseudoEigenvalueMatrix().diagonal(); Eigen::MatrixXd V_mat = es.pseudoEigenvectors(); Eigen::MatrixXd::Index min_index; Diag.minCoeff(&min_index); Eigen::VectorXd V = V_mat.col(min_index); V /= V(5); double c = V(0), s = V(1); t << V(2), V(3), V(4); //calculating the camera pose by 3d alignment Eigen::VectorXd xi, yi, zi; xi = P.row(0); yi = P.row(1); zi = P.row(2); Eigen::MatrixXd XXcs(3, n), XXc(3,n); XXc.setZero(); XXcs.row(0) = r(0)*xi + (r(1)*c + r(2)*s)*yi + (-r(1)*s + r(2)*c)*zi + t(0)*Eigen::VectorXd::Ones(n); XXcs.row(1) = r(3)*xi + (r(4)*c + r(5)*s)*yi + (-r(4)*s + r(5)*c)*zi + t(1)*Eigen::VectorXd::Ones(n); XXcs.row(2) = r(6)*xi + (r(7)*c + r(8)*s)*yi + (-r(7)*s + r(8)*c)*zi + t(2)*Eigen::VectorXd::Ones(n); for (int ii = 0; ii < n; ii++) XXc.col(ii) = Q.col(ii)*XXcs.col(ii).norm(); Eigen::Matrix3d R2; Eigen::Vector3d t2; Eigen::MatrixXd XXw = obj_pts.transpose(); calcampose(XXc, XXw, R2, t2); R_cum[i] = R2; t_cum[i] = t2; for (int k = 0; k < n; k++) XXc.col(k) = R2 * XXw.col(k) + t2; Eigen::MatrixXd xxc(2, n); xxc.row(0) = XXc.row(0).array() / XXc.row(2).array(); xxc.row(1) = XXc.row(1).array() / XXc.row(2).array(); double res = ((xxc.row(0) - img_pts.col(0).transpose()).norm() + (xxc.row(1) - img_pts.col(1).transpose()).norm()) / 2; err_cum[i] = res; } int pos_cum = std::min_element(err_cum.begin(), err_cum.end()) - err_cum.begin(); R_ = R_cum[pos_cum]; t_ = t_cum[pos_cum]; return true; }
static void ShowStats(void) { _GetStorageInfo(&statsP->stat); statsP->nextBase = NULL; statsP->totFree = 0; statsP->totUsed = 0; statsP->totHole = 0; statsP->totMaps = 0; statsP->holeBlocks = 0; statsP->freeBlocks = 0; statsP->usedBlocks = 0; statsP->mapsBlocks = 0; statsP->largestFreeBlock = 0; D0("Storage description. (All sizes in bytes)\n"); D0("Current storage analysis (by traversing heap):"); do { statsP->elementBase = statsP->nextBase; _NextHeapElement(&statsP->nextBase, &statsP->guard, &statsP->size, &statsP->freeBlk, &statsP->heapHole, &statsP->bitmap, &statsP->firstWord); if (statsP->heapHole) { statsP->holeBlocks++; statsP->totHole += statsP->size; } else if (statsP->freeBlk) { if (statsP->size > statsP->largestFreeBlock) statsP->largestFreeBlock = statsP->size; statsP->freeBlocks++; statsP->totFree += statsP->size; } else if (statsP->bitmap) {statsP->mapsBlocks++; statsP->totMaps += statsP->size;} else {statsP->usedBlocks++; statsP->totUsed += statsP->size;} } while (statsP->nextBase != NULL); D0("\n"); D4("Free memory of %d in %d blocks + overhead of %d = %d\n", statsP->totFree, statsP->freeBlocks, statsP->freeBlocks*OVERHEAD, statsP->totFree+statsP->freeBlocks*OVERHEAD); D1("Largest free block = %d\n", statsP->largestFreeBlock); D4("Used memory of %d in %d blocks + overhead of %d = %d\n", statsP->totUsed, statsP->usedBlocks, statsP->usedBlocks*OVERHEAD, statsP->totUsed+statsP->usedBlocks*OVERHEAD); D4("Memory taken by heap holes = %d in %d blocks + overhead of %d = %d\n", statsP->totHole, statsP->holeBlocks, statsP->holeBlocks*OVERHEAD, statsP->totHole + statsP->holeBlocks*OVERHEAD); D4("Memory taken by GC bitmaps = %d in %d blocks + overhead of %d = %d\n", statsP->totMaps, statsP->mapsBlocks, statsP->mapsBlocks*OVERHEAD, statsP->totMaps + statsP->mapsBlocks*OVERHEAD); D1("Current heap requirement (all except user free blocks) = %d\n", (statsP->totHole+statsP->holeBlocks*OVERHEAD) + (statsP->totUsed+statsP->usedBlocks*OVERHEAD) + (statsP->totMaps+statsP->mapsBlocks*OVERHEAD) + (statsP->freeBlocks*OVERHEAD) - OVERHEAD); D1("total heap usage = %d\n", (statsP->totHole+statsP->holeBlocks*OVERHEAD) + (statsP->totUsed+statsP->usedBlocks*OVERHEAD) + (statsP->totMaps+statsP->mapsBlocks*OVERHEAD) + (statsP->totFree+statsP->freeBlocks*OVERHEAD) - OVERHEAD); D0("\n"); D0("Current storage statistics:\n"); D3("%d coalesces, %d heap extensions, %d garbage collects\n", statsP->stat.coalesces, statsP->stat.heapExtensions, statsP->stat.garbageCollects); D3("Heap base = &%X, heap top = &%X, size of user heap = %d\n", (unsigned) statsP->stat.heapLow, (unsigned) statsP->stat.heapHigh, statsP->stat.userHeap); D2("Maximum storage requested = %d, current storage requested = %d\n", statsP->stat.maxHeapRequirement, statsP->stat.currentHeapRequirement); D4("total allocated = %d in %d blocks, deallocated = %d in %d\n", statsP->stat.bytesAllocated, statsP->stat.blocksAllocated, statsP->stat.bytesDeallocated, statsP->stat.blocksDeallocated); D0("\n"); statsP->eventNo = _GetLastEvent(); D0("Description of past events in storage (most recent first):\n"); while (_GetEventData(statsP->eventNo, &statsP->eventInfo)) { print_event(statsP->eventInfo.event); D3(" block size = %d, user heap size %d, %d usable\n", statsP->eventInfo.blockThatCausedEvent, statsP->eventInfo.userHeap, statsP->eventInfo.totalFree); D4(" allocated %d in %d, deallocated %d in %d since last event\n", statsP->eventInfo.bytesAllocated, statsP->eventInfo.allocates, statsP->eventInfo.bytesDeallocated, statsP->eventInfo.deallocates); statsP->eventNo--; } }
int main(void){ // hier komt de test /* int i = 0, j; char letter = 'a'; char buffer[3]; std::string* hulp; for(; i < h_nodes; i++){ for(j = 0; j < v_nodes; j++){ sprintf(buffer, "%c%d" , letter, j); hulp = new std::string(buffer); new Node(*hulp); delete hulp; } letter++; if(letter > 'z'){ letter = 'a'; } } printf("%p\n", Node::getParticularNode(std::string("a1"))); Node::getParticularNode(std::string("a1"))->print(); printf("%p\n", Node::getParticularNode(std::string("b1"))); Node::getParticularNode(std::string("b1"))->print(); Node::deleteAllNodes(); printf("%p\n", Node::getParticularNode(std::string("a1"))); Node::getParticularNode(std::string("a1"))->print(); */ Node A1("A1"); A1.addNeighbournode("B1", 4); A1.addNeighbournode("B2", 2); Node B1("B1"); B1.addNeighbournode("C1", 2); Node B2("B2"); B2.addNeighbournode("C3", 1); B2.setState(nodeUsed); Node C1("C1"); C1.addNeighbournode("D1", 1); Node C2("C2"); C2.addNeighbournode("D2", 3); C2.addNeighbournode("D3", 6); Node C3("C3"); C3.addNeighbournode("C2", 1); C3.addNeighbournode("D4", 3); Node D1("D1"); D1.addNeighbournode("D2", 9); Node D2("D2"); D2.addNeighbournode("D1", 9); D2.addNeighbournode("C2", 3); Node D3("D3"); D3.addNeighbournode("D4", 2); Node D4("D4"); D4.addNeighbournode("D3", 2); dijkstra planner; planner.calculateRoute(&A1, &D4); planner.printpath(); return 0; }