void destroy_points(ANNkd_tree& kdtree)
{
ANNpointArray dataPts = kdtree.thePoints();
annDeallocPts(dataPts);
}
mitk::ContourElement::VertexType* mitk::ContourElement::OptimizedGetVertexAt(const mitk::Point3D &point, float eps)
{
if( (eps > 0) && (this->m_Vertices->size()>0) )
{
int k = 1;
int dim = 3;
int nPoints = this->m_Vertices->size();
ANNpointArray pointsArray;
ANNpoint queryPoint;
ANNidxArray indexArray;
ANNdistArray distanceArray;
ANNkd_tree* kdTree;
queryPoint = annAllocPt(dim);
pointsArray = annAllocPts(nPoints, dim);
indexArray = new ANNidx[k];
distanceArray = new ANNdist[k];
int i = 0;
//fill points array with our control points
for(VertexIterator it = this->m_Vertices->begin(); it != this->m_Vertices->end(); it++, i++)
{
mitk::Point3D cur = (*it)->Coordinates;
pointsArray[i][0]= cur[0];
pointsArray[i][1]= cur[1];
pointsArray[i][2]= cur[2];
}
//create the kd tree
kdTree = new ANNkd_tree(pointsArray,nPoints, dim);
//fill mitk::Point3D into ANN query point
queryPoint[0] = point[0];
queryPoint[1] = point[1];
queryPoint[2] = point[2];
//k nearest neighbour search
kdTree->annkSearch(queryPoint, k, indexArray, distanceArray, eps);
VertexType* ret = NULL;
try
{
ret = this->m_Vertices->at(indexArray[0]);
}
catch(std::out_of_range ex)
{
//ret stays NULL
return ret;
}
//clean up ANN
delete [] indexArray;
delete [] distanceArray;
delete kdTree;
annClose();
return ret;
}
return NULL;
}
// get_neighbors_at_dist
string get_neighbor_at_dist(tree_struct stree, int dist,
ANNpoint queryPt,
int k_, int group)
{
ANNkd_tree *kdTree = stree.kdTree;
int dim = stree.dim;
ANNpointArray dataPts = stree.dataPts;
int k=k_;
int done = 1;
int nPts; // actual number of data points
ANNidxArray nnIdx; // near neighbor indices
ANNdistArray dists; // near neighbor distances
nnIdx = new ANNidx[k]; // allocate near neigh indices
dists = new ANNdist[k];
string return_string ="";
print_point(cout, queryPt, dim);
printf("looking for %d neighbors within the radius of %d \n", k, dist);
// first make sure it is a valid point
kdTree->annkSearch( // search
queryPt, // query point
k, // number of near neighbors
nnIdx, // nearest neighbors (returned)
dists, // distance (returned)
eps); // error bound
char buf [8];
sprintf(buf, "%d", group);
string group_str(buf);
// return_string = "global init__" + group_str +"\n set init__" +
// group_str + " [list ";
return_string += " [list ";
for (int i=0; i < k; i++) {
if (dists[i] == dist) {
return_string += " [list ";
for(int j=0; j < dim; j++) {
stringstream ss;
double d = (dataPts[nnIdx[i]])[j];
ss << d;
string buf =ss.str();
return_string += buf;
if(j!=dim-1)
return_string += " ";
}
return_string += "]";
if(i!=k-1)
return_string += " ";
}
}
return_string += "]\0";
/*
FILE * pFile;
pFile = fopen (filename.c_str(),"a");
if (pFile!=NULL)
{
fputs (return_string.c_str(),pFile);
fclose (pFile);
}
else {
done = 0;
}
return done;
*/
return return_string;
}
/******************************
* I am a file that sets up models/features for creating Viewpoint Invariant Patches
* If you don't compile me with the ann library I will be sad :( (See ann user guide)
* Also needs to be compiled with OpenCV now
Compile command: (Probably don't need all the OpenCV libraries listed here)
g++ warpSIFT.cpp -I/path/to/ann_1.1.2/include -L/path/to/ann_1.1.2/lib -lANN -g -I/usr/local/include/opencv -I/usr/local/include -L/usr/local/lib -lopencv_shape -lopencv_stitching -lopencv_objdetect -lopencv_superres -lopencv_videostab -lopencv_calib3d -lopencv_features2d -lopencv_highgui -lopencv_videoio -lopencv_imgcodecs -lopencv_video -lopencv_photo -lopencv_ml -lopencv_imgproc -lopencv_flann -lopencv_core
Lio: g++ -std=c++11 warpSIFT.cpp -I/home/lionelt/ann_1.1.2/include -L/home/lionelt/ann_1.1.2/lib -lANN -g -I/usr/local/include/opencv -I/usr/local/include -L/usr/local/lib -lopencv_shape -lopencv_stitching -lopencv_objdetect -lopencv_superres -lopencv_videostab -lopencv_calib3d -lopencv_features2d -lopencv_highgui -lopencv_videoio -lopencv_imgcodecs -lopencv_video -lopencv_photo -lopencv_ml -lopencv_imgproc -lopencv_flann -lopencv_core
g++ -std=c++11 warpSIFT.cpp -I/home/lionelt/ann_1.1.2/include -L/home/lionelt/ann_1.1.2/lib -lANN -g -I/usr/local/include/opencv -I/usr/local/include -L/usr/local/lib-lopencv_core
To run, something like that:
./a.out -df ../ETH_example_3D_model/dense3.nvm.cmvs/00/models/option-0000.ply -sf ../ETH_example_3D_model/dense3.nvm -sift ../ETH_example_3D_model/eth_photos_night
* Must be given 3 flags:
* -df: path to dense file
* -sf: path to sparse file
* -sift: path to folder containing images and SIFT files
* that generated the model. (Plz no '/' at end of path)
*******************************/
int main(int argc, char **argv)
{
ifstream denseStream;
ifstream sparseStream;
string siftFolder;
int maxPoints = 1024;
char line[1024];
// Read in command line arguments and open the file streams
int i = 1;
while (i < argc) {
if (!strcmp(argv[i], "-df")) {
denseStream.open(argv[++i], ios::in);
if (!denseStream) {
cerr << "Cannot open dense file\n";
exit(1);
}
}
else if (!strcmp(argv[i], "-sf")) {
sparseStream.open(argv[++i], ios::in);
if (!sparseStream) {
cerr << "Cannot open sparse file\n";
exit(1);
}
}
else if (!strcmp(argv[i],"-sift")){
siftFolder = argv[++i];
}
else if (!strcmp(argv[i],"-n")){
maxPoints = stoi(argv[++i]);
}
else {
cerr << "Unrecognized option.\n";
exit(1);
}
i++;
}
// Store cameras and sift features
// Creates a vector of Camera objects. Each Camera object
// has image name, focal length, quaternion, center, distortion
// and a vector of all SIFT features of that camera/image
sparseStream.getline(line, 1028);
sparseStream.getline(line, 1028);
sparseStream.getline(line, 1028);
int numCameras = stoi(line);
vector<Camera> cameras(numCameras);
// Read a camera line and store all camera parameters
for (int i = 0; i < numCameras; i++){
sparseStream.getline(line, 1028);
istringstream ss(line);
Camera c;
ss >> c.name;
ss >> c.focalLength;
for (int j = 0; j < 4; j++){
ss >> c.quaternion[j];
}
for (int j = 0; j < 3; j++){
ss >> c.center[j];
}
ss >> c.radialDistortion;
// Iterate through the correpsonding SIFT file and parse all SIFT features
// readSIFT returns a vector of SIFT features for the corresponding image/camera
c.SiftVector = readSIFT(siftFolder + "/" + c.name.substr(0,c.name.find(".")) + ".sift",i);
if (c.SiftVector.size() == 0){
cerr << "Error importing SIFT features for " << c.name << "\n";
}
cameras[i] = c;
}
// Read in dense model for use in nearest neighbour calculations later
ANNpointArray densePts = annAllocPts(maxPoints, 3);
double eps = 0.01;
int k = 5; //was 5 before ???
int dim = 3;
ANNidxArray nnIdx = new ANNidx[k];
ANNdistArray dists = new ANNdist[k];
double normals[maxPoints][dim];
int numPoints = 0;
for (int i = 0; i < 13; i++){
denseStream.getline(line, 1028);
}
int check = 1;
if(!denseStream.getline(line, 1028)){
check=0;
}
while(numPoints < maxPoints && check) {
istringstream ss(line);
for (int i = 0; i < dim; i++){
ss >> densePts[numPoints][i];
}
for (int i = 0; i < dim; i++){
ss >> normals[numPoints][i];
}
numPoints++;
if(!denseStream.getline(line, 1028)){
check=0;
}
}
ANNkd_tree* kdTree = new ANNkd_tree(densePts, numPoints, dim); // ??? + where is nearest neighbor search???
denseStream.close();
// Create sparse point with corresponding feature indices and normal plane
// For now, normal plane is just taken directly from the nearest neighbour
// Aggregate SIFT features for sparse point and calculate homography using
// rotation and translation. For each camera.
sparseStream.getline(line, 1028);
sparseStream.getline(line, 1028);
int numSparsePoints = stoi(line);
vector<sparseModelPoint> sparsePoints(numSparsePoints);
ANNpoint queryPt = annAllocPt(dim);
for (int i = 0; i < 1; i++){
sparseStream.getline(line, 1028);
istringstream ss(line);
sparseModelPoint smp;
for (int j = 0; j < 3; j++){
ss >> smp.point[j];
queryPt[j] = smp.point[j];
}
kdTree->annkSearch(queryPt, k, nnIdx, dists, eps);
// for (int k2 = 0; k2 < k; k2++){
// for (int j = 0; j < 3; j++){
// smp.normal[j] += normals[nnIdx[k2]][j];
// }
// }
// for (int j = 0; j < 3; j++){
// smp.normal[j] = smp.normal[j]/k;
// }
for (int j = 0; j < 3; j++){
smp.normal[j] = normals[nnIdx[0]][j];
// smp.normal[i] = no
}
// for (int j = 0; j < 3; j++){
// smp.normal[j] = normals[nnIdx[0]][j];
// }
// for (int j = 0; j < 3; j++){
// smp.normal[j] = 0.001;
// }
// smp.normal[2] = 1;
int temp;
for (int j = 0; j < 3; j++){
ss >> temp;
}
string ns;
ss >> ns;
int numSift = stoi(ns);
vector<sparseSiftFeature> sparseSifts(numSift);
cout << "Point" << i << ": (" << smp.point[0] << "," << smp.point[1] << "," << smp.point[2] << "): \n";
// CREATION OF VIP
for (int j = 0; j < 1; j++){
sparseSiftFeature ssf;
ss >> ns;
int imageIndex = stoi(ns);
ss >> ns;
int featIndex = stoi(ns);
Camera cam = cameras[imageIndex];
ssf.Sift = cam.SiftVector[featIndex];
/** Side Note: ssf.modelXY and ssf.Sfit.point both refer to
* same point in the image, just with different coordinate
* systems.
* The modelXY has the origin at the centre of the image.
* The pointXY obtained from SIFT has the origin in the bottom left corner
**/
ss >> ssf.modelXY[0];
ss >> ssf.modelXY[1];
// cam: list feature index and camera index for each point. It doesn't tell you which camera
// belongs to which sift descriptor. Tian created cam to place it into camera. c is the same.
// cam and smp are both COPIES, while &ssf is a POINTER => working on ssf edits the real cam.
// ssf: sparse sift feature
// smp: sparse model point
cout << "\t" << ssf.Sift.point[0] << "," << ssf.Sift.point[1] << " in image " << cam.name << "\n";
computeRotation(cam, &ssf, smp);
computeTranslation(cam, &ssf, smp);
computeHomography(cam, &ssf, smp.normal);
// a P becomes a VIP
// Lionel:createVIP(cam, "/home/lionelt/3D_vision_pollefeys/circle_warped.jpeg", &ssf, "Point"+to_string(i)+"Sift"+to_string(j));
// PLEASE DON'T EDIT BEFORE HAVING PULLED THE LATEST CHANGES, I am getting maaad restoring my stuff everytime.
// Also beware of the stuff you push XD
createVIP(cam, "../circle_warped.jpeg", &ssf, "Point"+to_string(i)+"Sift"+to_string(j));
sparseSifts[j] = ssf;
}
smp.features = sparseSifts;
sparsePoints[i] = smp;
} // END OF VIP
sparseStream.close();
// Every point in sparsePoints should now have a list of VIPs
// Get rid of extra information
delete [] nnIdx;
delete [] dists;
delete kdTree;
annClose();
}
void createParcels
(
const fvMesh& mesh,
cfdemCloud& sm,
const int& parcelSize_,
int**& parcelCloud_,
double**& parcelPositions_,
double**& parcelVelocities_,
int*& parcelNparts_,
double** & parcelKinStress_,
scalar& aveSubQparcel2_,
vector& meanParcelVel_,
const bool verbose_
)
{
if ( parcelSize_ * parcelSize_ * parcelSize_ > sm.numberOfParticles() )
{
FatalError << " Number of particles in a parcel > number of particles" << abort(FatalError);
}
if ( parcelSize_ < 1 )
{
FatalError << " Number of particles < 0 in a parcel " << abort(FatalError);
}
// Number of particles in a parcel
int k = parcelSize_ * parcelSize_ * parcelSize_;
// Dimensions, exact OR approximate
int dim =3; double eps = 0;
// Number of points
int nPts;
nPts = sm.numberOfParticles();
// Data points
ANNpointArray dataPts;
// Query points
ANNpoint queryPt;
ANNidxArray nnIdx; // near neighbour indices
ANNdistArray dists; // near neighbour distances
ANNkd_tree* kdTree; // search structure
// Allocate
queryPt = annAllocPt(dim);
dataPts = annAllocPts(nPts, dim);
nnIdx = new ANNidx[k];
dists = new ANNdist[k];
for(int index = 0; index < sm.numberOfParticles(); index++)
{
dataPts[index][0] = sm.position(index).x();
dataPts[index][1] = sm.position(index).y();
dataPts[index][2] = sm.position(index).z();
}
kdTree = new ANNkd_tree(dataPts, nPts, dim);
// Initialize sub-parcel agitation
aveSubQparcel2_ = 0.;
// Initialize parcel velocity
meanParcelVel_ = vector(0,0,0);
for(int index = 0; index < sm.numberOfParticles(); index++)
{
// Particle neighbouring search distance
scalar sqRad = parcelSize_ * sm.radius(index);
queryPt[0] = sm.position(index).x();
queryPt[1] = sm.position(index).y();
queryPt[2] = sm.position(index).z();
kdTree->annkFRSearch(
queryPt, // query point
sqRad, // squared radius
k, // number of the near neighbours to return
nnIdx, // nearest neighbor array
dists, // dist to near neighbours
eps );
int nParts = 0;
scalar dist = 0;
// Initialize parcel velocities & positions & kinetic stresses
for(int j=0;j<3;j++)
{
parcelVelocities_[index][j] = 0.;
parcelPositions_[index][j] = 0.;
parcelKinStress_[index][j] = 0.;
parcelKinStress_[index][2*j] = 0.;
}
for (int i = 0; i < k; i++)
{
parcelCloud_[index][i] = nnIdx[i];
dist = mag( sm.position(nnIdx[i]) - sm.position(index) ) - sqRad;
if ( dist < SMALL )
{
for(int j=0;j<3;j++)
{
// Parcel velocity
parcelVelocities_[index][j] += sm.velocity(nnIdx[i])[j];
// Parcel center of mass
parcelPositions_[index][j] += sm.position(nnIdx[i])[j];
}
nParts++;
}
}
for(int j=0;j<3;j++) parcelPositions_[index][j] /= nParts;
parcelNparts_[index] = nParts;
// Parcel kinetic stresses
for(int i = 0; i < parcelNparts_[index]; i++)
{
int particleID = parcelCloud_[index][i];
// U'xU'x
parcelKinStress_[index][0] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[0] - parcelVelocities_[index][0] );
// U'yU'y
parcelKinStress_[index][1] += ( sm.velocity(particleID)[1] - parcelVelocities_[index][1] )
*( sm.velocity(particleID)[1] - parcelVelocities_[index][1] );
// U'zU'z
parcelKinStress_[index][2] += ( sm.velocity(particleID)[2] - parcelVelocities_[index][2] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
// U'xU'y
parcelKinStress_[index][3] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[1] - parcelVelocities_[index][1] );
// U'xU'z
parcelKinStress_[index][4] += ( sm.velocity(particleID)[0] - parcelVelocities_[index][0] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
// U'yU'z
parcelKinStress_[index][5] += ( sm.velocity(particleID)[1] - parcelVelocities_[index][1] )
*( sm.velocity(particleID)[2] - parcelVelocities_[index][2] );
}
// Mean parcel velocity
for(int j=0;j<3;j++) meanParcelVel_[j] += parcelVelocities_[index][j];
// Domain-averaged parcel agitation
aveSubQparcel2_ += 1./2. * ( parcelKinStress_[index][0] + parcelKinStress_[index][1] + parcelKinStress_[index][2] );
}
for(int j=0;j<3;j++) meanParcelVel_[j] /= sm.numberOfParticles();
if ( verbose_ )
{
int index = 0;
Info << " Parcel particle list ";
for (int i = 0; i < parcelNparts_[index]; i++)
{
Info << parcelCloud_[index][i] << " " ;
}
Info << endl;
Info << " Parcel center " << parcelPositions_[index][0] << "," << parcelPositions_[index][1] << "," << parcelPositions_[index][2] << endl;
Info << " Parcel velocity " << parcelVelocities_[index][0] << "," << parcelVelocities_[index][1] << "," << parcelVelocities_[index][2] << endl;
for (int i = 0; i < parcelNparts_[index]; i++)
{
Info << " Particle " << parcelCloud_[index][i] << endl;
Info << " Particle center " << sm.position(parcelCloud_[index][i]) << endl;
Info << " Particle velocity " << sm.velocity(parcelCloud_[index][i]) << endl;
}
}
}
