// カメラの内部パラメータ、歪み係数の読み込み
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);
}
