// カメラの内部パラメータ、歪み係数の読み込み DLLExport void loadCameraParam(double projectionMatrix[], double dist[]) { cv::Mat cameraMat(3, 3, CV_64F); cv::Mat distMat(1, 5, CV_64F); cv::FileStorage cvfs("Calibration/calibration.xml", CV_STORAGE_READ); cvfs["cam_K"] >> cameraMat; cvfs["cam_dist"] >> distMat; for(int i = 0; i < 9; ++i) { projectionMatrix[i] = cameraMat.at<double>(i); } for(int i = 0; i < 5; ++i) { dist[i] = distMat.at<double>(i); } }
void bfs(Graph* _g, std::vector<std::vector<int> >& _dist) { //BFS for (int i = 1; i <= _g->nVertices; ++i) { int root = _g->vertices[i].getCode(); std::list<int> f; std::vector<bool> visited(_g->nVertices + 1, false); #ifdef DEBUG std::vector<int> distMat(_g->nVertices + 1, 0); #endif f.push_back(root); while (f.size() != 0) { int v1 = *f.begin(); visited[v1] = true; for (int j = 0; j < _g->adjList[v1].size(); ++j) { int v2 = _g->adjList[v1][j].getTail().getCode(); if (!visited[v2]) { visited[v2] = true; _dist[root][v2] = _dist[root][v1] + 1; f.push_back(v2); #ifdef DEBUG distMat[v2] = distMat[v1] + 1; #endif } } f.pop_front(); } } return; }
void Hypergraph::layoutFast(int boundary [], int dim, float c, int Vmax, int Smax, int nIteration) { _layoutDim = dim; float temperature = 10; srand(time(NULL)); int area = 1; for (int i=0; i<dim; ++i) { area *= boundary[2*i+1]-boundary[2*i]; } float k = c*sqrt(area*1.0f/_nVertices); /*initialize vertex layout*/ int nEdges = _edges.size(); int nTotal = _nVertices+nEdges; _layout.resize(nTotal); vector<fvec> dispBuf (nTotal); for (int i=0; i<nTotal; ++i) { _layout[i] = fvec(dim); for (int j=0; j<dim; ++j) { _layout[i](j) = boundary[2*j]+rand()%(boundary[2*j+1]-boundary[2*j]); } dispBuf[i] = fvec(dim); } int i,j; vector<list<int> > sampleBufs (nTotal); vector<int> Vsizes (nTotal); vector<float> maxDists (nTotal); maxDists.assign(nTotal, 40); /*initialize distance matrix*/ vector<float> distMat (nTotal*(nTotal-1)/2); distMat.assign(nTotal*(nTotal-1)/2, 50); for (int i=0; i<nEdges; ++i) { int edgeIdx = i+_nVertices; for (int j=0; j<_edges[i].size(); ++j) { int n1 = _edges[i][j]; for (int k=0; k<j; ++k) { int n2 = _edges[i][k]; distMat[n1*(n1-1)/2+n2] = 20; } distMat[edgeIdx*(edgeIdx-1)/2+n1] = 10; } } QProgressDialog prog ("Performing graph layout ...", "", 0, nIteration-1); prog.setCancelButton(NULL); for (int it=0; it<nIteration; ++it) { DWORD t1 = GetTickCount(); bool bChanged = false; for (i=0; i<nTotal; ++i) { dispBuf[i].fill(0); } DWORD t2 = GetTickCount(); DWORD tFindSample = 0; for (i=0; i<nTotal; ++i) { DWORD t3 = GetTickCount(); findSampleSets(i, maxDists[i], distMat, sampleBufs[i], Vsizes[i], Vmax, Smax); list<int> &sampleBuf = sampleBufs[i]; tFindSample += GetTickCount()-t3; //for (j=0; j<sampleBuf.size(); ++j) for (list<int>::iterator sampleIt = sampleBuf.begin(); sampleIt!=sampleBuf.end(); ++sampleIt) { int sampleIdx = *sampleIt; ++sampleIt; float dist = *sampleIt; if (sampleIdx==i) { continue; } fvec d = _layout[sampleIdx]-_layout[i]; float normD = norm(d,2); /*calculate repulsive force*/ fvec dispRepulsive; if (normD<1.0e-5) { dispRepulsive = fvec(dim); for (int iDim=0; iDim<dim; ++iDim) { dispRepulsive(iDim) = rand()%100/100.0f; } dispRepulsive *= temperature; } else { dispRepulsive = d*k*k/pow(normD,2); } dispBuf[i] -= dispRepulsive; dispBuf[sampleIdx] += dispRepulsive; /*calculate attractive force*/ //float dist = i>sampleIdx?distMat[i*(i-1)/2+sampleIdx]:distMat[sampleIdx*(sampleIdx-1)/2+i]; /*if (dist<50) { float c3 = dist<15?3.0:1.0; fvec dispAttractive = c3*d*normD/k; dispBuf[i] += dispAttractive; dispBuf[sampleIdx] -= dispAttractive; }*/ } } DWORD tRepAttrForces = GetTickCount()-t2; /*restrict the points from being pushed towards the boundary*/ t2 = GetTickCount(); for (i=0; i<nTotal; ++i) { for (j=0; j<dim; ++j) { float c1=1.0e6; float x = _layout[i](j); if (x==boundary[j*2] || x==boundary[j*2+1]) { int stop = 1; } float d = c1*(1/(x-boundary[j*2]+1.0e-3)-1/(boundary[j*2+1]-x+1.0e-3)); dispBuf[i](j) += d; } } DWORD tBoundaries = GetTickCount()-t2; /*limit the maximum displacement, boundary check*/ t2 = GetTickCount(); for (i=0; i<nTotal; ++i) { float n = norm(dispBuf[i],2); fvec newLayout = _layout[i]+dispBuf[i]*min(n, temperature)/n; for (j=0; j<dim; ++j) { int minBoundary = boundary[2*j]; int maxBoundary = boundary[2*j+1]; newLayout(j) = newLayout(j) < minBoundary?minBoundary:(newLayout(j)>maxBoundary?maxBoundary:newLayout(j)); } } DWORD tLimits = GetTickCount()-t2; DWORD perItTime = GetTickCount() - t1; t1 = GetTickCount(); prog.setValue(it); } }
PyObject *embedBoundsMatrix(python::object boundsMatArg,int maxIters=10, bool randomizeOnFailure=false,int numZeroFail=2, python::list weights=python::list(), int randomSeed=-1){ PyObject *boundsMatObj = boundsMatArg.ptr(); if(!PyArray_Check(boundsMatObj)) throw_value_error("Argument isn't an array"); PyArrayObject *boundsMat=reinterpret_cast<PyArrayObject *>(boundsMatObj); // get the dimensions of the array unsigned int nrows = boundsMat->dimensions[0]; unsigned int ncols = boundsMat->dimensions[1]; if(nrows!=ncols) throw_value_error("The array has to be square"); if(nrows<=0) throw_value_error("The array has to have a nonzero size"); if (boundsMat->descr->type_num != PyArray_DOUBLE) throw_value_error("Only double arrays are currently supported"); unsigned int dSize = nrows*nrows; double *cData = new double[dSize]; double *inData = reinterpret_cast<double *>(boundsMat->data); memcpy(static_cast<void *>(cData), static_cast<const void *>(inData), dSize*sizeof(double)); DistGeom::BoundsMatrix::DATA_SPTR sdata(cData); DistGeom::BoundsMatrix bm(nrows,sdata); RDGeom::Point3D *positions=new RDGeom::Point3D[nrows]; std::vector<RDGeom::Point *> posPtrs; for (unsigned int i = 0; i < nrows; i++) { posPtrs.push_back(&positions[i]); } RDNumeric::DoubleSymmMatrix distMat(nrows, 0.0); // ---- ---- ---- ---- ---- ---- ---- ---- ---- // start the embedding: bool gotCoords=false; for(int iter=0;iter<maxIters && !gotCoords;iter++){ // pick a random distance matrix DistGeom::pickRandomDistMat(bm,distMat,randomSeed); // and embed it: gotCoords=DistGeom::computeInitialCoords(distMat,posPtrs,randomizeOnFailure, numZeroFail,randomSeed); // update the seed: if(randomSeed>=0) randomSeed+=iter*999; } if(gotCoords){ std::map<std::pair<int,int>,double> weightMap; unsigned int nElems=PySequence_Size(weights.ptr()); for(unsigned int entryIdx=0;entryIdx<nElems;entryIdx++){ PyObject *entry=PySequence_GetItem(weights.ptr(),entryIdx); if(!PySequence_Check(entry) || PySequence_Size(entry)!=3){ throw_value_error("weights argument must be a sequence of 3-sequences"); } int idx1=PyInt_AsLong(PySequence_GetItem(entry,0)); int idx2=PyInt_AsLong(PySequence_GetItem(entry,1)); double w=PyFloat_AsDouble(PySequence_GetItem(entry,2)); weightMap[std::make_pair(idx1,idx2)]=w; } DistGeom::VECT_CHIRALSET csets; ForceFields::ForceField *field = DistGeom::constructForceField(bm,posPtrs,csets,0.0, 0.0, &weightMap); CHECK_INVARIANT(field,"could not build dgeom force field"); field->initialize(); if(field->calcEnergy()>1e-5){ int needMore=1; while(needMore){ needMore=field->minimize(); } } delete field; } else { throw_value_error("could not embed matrix"); } // ---- ---- ---- ---- ---- ---- ---- ---- ---- // construct the results matrix: npy_intp dims[2]; dims[0] = nrows; dims[1] = 3; PyArrayObject *res = (PyArrayObject *)PyArray_SimpleNew(2,dims,NPY_DOUBLE); double *resData=reinterpret_cast<double *>(res->data); for(unsigned int i=0;i<nrows;i++){ unsigned int iTab=i*3; for (unsigned int j = 0; j < 3; ++j) { resData[iTab + j]=positions[i][j]; //.x; } } delete [] positions; return PyArray_Return(res); }