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