Voronoi::Voronoi(QString fn) {
//Set external triangle
Point a,b,c;
a.setX(100);
a.setY(100);
b.setX(-100);
b.setY(100);
c.setX(0);
c.setY(-100);
points.append(a);
points.append(b);
points.append(c);
Triangle t(a,b,c);
triangles.append(t);
ext1 = t.ab;
ext2 = t.bc;
ext3 = t.ca;
extr1 = a;
extr2 = b;
extr3 = c;
fileName = fn;
readPoints();
}
bool Foam::fileFormats::STARCDsurfaceFormat<Face>::read
(
const fileName& filename
)
{
const bool mustTriangulate = this->isTri();
this->clear();
fileName baseName = filename.lessExt();
// STAR-CD index of points
List<label> pointId;
// read points from .vrt file
readPoints
(
IFstream(baseName + ".vrt")(),
this->storedPoints(),
pointId
);
// Build inverse mapping (STAR-CD pointId -> index)
Map<label> mapPointId(2*pointId.size());
forAll(pointId, i)
{
mapPointId.insert(pointId[i], i);
}
int main()
{
struct Point points[MAX_POINTS];
int numPoints = readPoints(points); //populate the array and get numPoints
if(numPoints == 0)
return 1; //exit failure
struct Point hullPoints[MAX_POINTS]; //sizing is just to be safe
struct Point pointOnHull = leftmostPoint(points, numPoints);
int i = 0;
struct Point endpoint;
do
{
hullPoints[i] = pointOnHull; //use as next pivot
endpoint = points[0]; //initial endpoint candidate
for(int j = 1; j < numPoints; j++)
{
if(equal(endpoint,pointOnHull)||
(ccw(hullPoints[i], endpoint, points[j]) > 0))
{
endpoint = points[j]; //found greater left turn, update endpoint
}
}
i++;
pointOnHull = endpoint;
}
while(!equal(endpoint,hullPoints[0])); //wrapped around to first hull point
printf("Set of points:\n");
displayPoints(points, numPoints);
printf("Convex hull:\n");
displayPoints(hullPoints,i);
return 0; //success
}
int CsvInterface::readPoints(std::string const& fname, char delim,
std::vector<GeoLib::Point*> &points,
std::string const& x_column_name,
std::string const& y_column_name,
std::string const& z_column_name)
{
std::ifstream in(fname.c_str());
std::array<std::string, 3> const column_names = {{x_column_name, y_column_name, z_column_name}};
if (!in.is_open()) {
ERR ("CsvInterface::readPoints(): Could not open file %s.", fname.c_str());
return -1;
}
std::string line;
getline(in, line);
std::array<std::size_t, 3> const column_idx =
{{ CsvInterface::findColumn(line, delim, x_column_name),
CsvInterface::findColumn(line, delim, y_column_name),
(z_column_name.empty()) ? CsvInterface::findColumn(line, delim, y_column_name) :
CsvInterface::findColumn(line, delim, z_column_name) }};
for (std::size_t i=0; i<3; ++i)
if (column_idx[i] == std::numeric_limits<std::size_t>::max())
{
ERR ("Column \"%s\" not found in file header.", column_names[i].c_str());
return -1;
}
return readPoints(in, delim, points, column_idx);
}
// Construct from components
starMesh::starMesh
(
const fileName& prefix,
const Time& rt,
const scalar scaleFactor
)
:
casePrefix_(prefix),
runTime_(rt),
points_(0),
cellShapes_(0),
boundary_(0),
patchTypes_(0),
defaultFacesName_("defaultFaces"),
defaultFacesType_(emptyPolyPatch::typeName),
patchNames_(0),
patchPhysicalTypes_(0),
starPointLabelLookup_(0),
starPointID_(0),
starCellID_(0),
starCellLabelLookup_(0),
starCellPermutation_(0),
cellFaces_(0),
boundaryCellIDs_(0),
boundaryCellFaceIDs_(0),
meshFaces_(0),
cellPolys_(0),
nInternalFaces_(0),
polyBoundaryPatchStartIndices_(0),
pointCellsPtr_(NULL),
couples_(0),
isShapeMesh_(true)
{
readPoints(scaleFactor);
readCells();
readBoundary();
fixCollapsedEdges();
readCouples();
if (couples_.size())
{
createCoupleMatches();
}
markBoundaryFaces();
mergeCoupleFacePoints();
purgeCellShapes();
collectBoundaryFaces();
}
/**
* Main function
*
* Takes user input and finds/prints the convex hull using
* jarvis algorithm.
*
* @param argc the number of arguments
* @param argv array of arguments
* @returns int the error code; 0 if no error
*/
int main( int argc, const char* argv[] ) {
struct Point points[MAX_POINTS];
int numPoints = readPoints( points );
struct Point results[numPoints];
int numResults = jarvis( points, numPoints, results );
printf("Convex hull:\n");
displayPoints( results, numResults );
}
Hull::Hull(QWidget *parent)
: QGLWidget(parent)
{
setFormat(QGLFormat(QGL::DoubleBuffer | QGL::DepthBuffer));
rotationX = 0.0;
rotationY = 0.0;
rotationZ = 0.0;
readPoints();
}
int main()
{
struct Point points[20];
int numPoints = readPoints(points);
struct Point lowPoint = lowestPoint(points,numPoints);
printf("lowest point: ");
displayPoint(lowPoint);
return 0;
}
void ConfigurationReader::readRooms(JsonArray& rooms, Actor* actor) {
char *name;
Room *room;
for(JsonArray::iterator it=rooms.begin(); it!=rooms.end(); ++it)
{
JsonObject &jsonRoom = it->asObject();
name = jsonRoom[JSON_ROOM_NAME];
room = new Room(name);
JsonArray& jsonPoints = jsonRoom[JSON_POINTS];
readPoints(jsonPoints, room);
actor->addRoom(room);
}
}
int main(int nargs, char **args){
if (nargs < 7) {
usage(args[0]);
exit(1);
}
nPoints = atoi(args[1]);
nQueries = atoi(args[2]);
dimension = atoi(args[3]);
p = atof(args[4]);
K = atoi(args[5]);
readPoints(args[6]); // read all points
FILE *queryFile = fopen(args[7], "rt");
//fscanf(queryFile, "%d\n", &nQueries);
query = (RealT*)malloc(dimension * sizeof(RealT));
printf("nPoints = %d\n", nPoints);
//printf("nQueries = %d\n", nQueries);
for(int i = 0; i < nQueries; i++){
// read in the query point.
for(int d = 0; d < dimension; d++){
FSCANF_REAL(queryFile, &(query[d]));
}
//printRealVector1("Query: ", dimension, query);
std::priority_queue<Node> myq;
TimeVarT time = 0;
RealT tempdis = 0;
TIMEV_START(time);
for(int j = 0; j < nPoints; j++){
tempdis = dist(query, points[j]);
updataQ(myq, tempdis, j);
//printf("Distance[dist] (%d): %lf\n", j, dist(query, points[j]));
//printRealVector1("X: ", dimension, points[j]);
}
TIMEV_END(time); // time only finding the near neighbors, and exclude printing from timing.
printf("Total time for K-NN query \t%0.6lf\n",time);
printf("Query point %d 's %d NNs are:\n", i, K);
display(myq);
}
}
int main(void)
{
struct Point jarvisPoints[MAX_POINTS];
int numberOfjarvisPoints=readPoints(jarvisPoints);
if(numberOfjarvisPoints>0)
{
displayPoints(jarvisPoints,numberOfjarvisPoints);
struct Point convexHullSet[numberOfjarvisPoints];
int numberOfConvexPoints=jarvis(jarvisPoints,convexHullSet,numberOfjarvisPoints);
displayConvexPoints(convexHullSet,numberOfConvexPoints);
}
}
int main()
{
// A list of points.
struct Point ptList[ PT_LIMIT ];
// Read all the points
int len = readPoints( ptList );
// find the closest pair.
struct Pair pair = findNearest( ptList, len );
// Report it.
printf( "%s and %s are closest\n", ptList[ pair.a ].name, ptList[ pair.b ].name );
return 0;
}
void PartitionIO<MeshType>::read (mesh_ptrtype& meshPart)
{
meshPart.reset();
M_meshPartIn.reset (new mesh_type);
M_HDF5IO.openFile (M_fileName, M_comm, true);
readStats();
readPoints();
readEdges();
readFaces();
readElements();
M_HDF5IO.closeFile();
meshPart = M_meshPartIn;
M_meshPartIn.reset();
}
// Construct from components
sammMesh::sammMesh
(
const fileName& prefix,
const Time& rt,
const scalar scaleFactor
)
:
casePrefix_(prefix),
runTime_(rt),
points_(0),
cellShapes_(0),
boundary_(0),
patchTypes_(0),
defaultFacesName_("defaultFaces"),
defaultFacesType_(emptyPolyPatch::typeName),
patchNames_(0),
patchPhysicalTypes_(0),
starPointLabelLookup_(0),
starCellLabelLookup_(0),
cellFaces_(0),
meshFaces_(0),
cellPolys_(0),
nInternalFaces_(0),
polyBoundaryPatchStartIndices_(0),
pointCellsPtr_(NULL),
isShapeMesh_(true)
{
// Fill in the lookup tables
fillSammCellShapeTable();
fillSammAddressingTable();
readPoints(scaleFactor);
readCells();
readBoundary();
fixCollapsedEdges();
readCouples();
// create boundary faces
createBoundaryFaces();
// after all this is done do couples
}
int main(void)
{
struct Point quickHullPoints[MAX_POINTS];
int numberOfquickHullPoints=readPoints(quickHullPoints);
if(numberOfquickHullPoints>0)
{
displayPoints(quickHullPoints,numberOfquickHullPoints);
struct Point convexHullSet[numberOfquickHullPoints];
int numberOfConvexPoints=quickHull(quickHullPoints,convexHullSet,numberOfquickHullPoints);
displayConvexPoints(convexHullSet,numberOfConvexPoints);
}
}
int CsvInterface::readPoints(std::string const& fname, char delim,
std::vector<GeoLib::Point*> &points,
std::size_t x_column_idx,
std::size_t y_column_idx,
std::size_t z_column_idx)
{
std::ifstream in(fname.c_str());
if (!in.is_open()) {
ERR ("CsvInterface::readPoints(): Could not open file %s.", fname.c_str());
return -1;
}
if (z_column_idx == std::numeric_limits<std::size_t>::max())
z_column_idx = y_column_idx;
std::array<std::size_t, 3> const column_idx = {{ x_column_idx, y_column_idx, z_column_idx }};
return readPoints(in, delim, points, column_idx);
}
MainWindow::MainWindow(QWidget *parent)
: QMainWindow(parent)
, m_update_pending(false), m_animating(false)
, m_fgColor(255, 255, 255), m_bgColor(0, 0, 0)
{
m_oglviewer = new OGLViewer;
ui.setupUi(this);
ui.ogl_layout->addWidget(m_oglviewer);
//setWindowTitle(tr("OpenGL Qt Template"));
m_oglviewer->setFocusPolicy(Qt::StrongFocus);
connect(ui.clear_button, SIGNAL(clicked()), m_oglviewer, SLOT(clearVertex()));
connect(ui.curve_type, SIGNAL(currentIndexChanged(int)), m_oglviewer, SLOT(changeCurveType(int)));
connect(ui.degree_val, SIGNAL(valueChanged(int)), m_oglviewer, SLOT(setDegree(int)));
connect(ui.seg_val, SIGNAL(valueChanged(int)), m_oglviewer, SLOT(setSegment(int)));
connect(ui.actionOpen, SIGNAL(triggered()), this, SLOT(readPoints()));
connect(ui.actionSave, SIGNAL(triggered()), this, SLOT(savePoints()));
connect(ui.actionExport, SIGNAL(triggered()), this, SLOT(exportSVG()));
signalMapper = new QSignalMapper(this);
connect(signalMapper, SIGNAL(mapped(int)), m_oglviewer, SLOT(changeOperation(int)));
signalMapper->setMapping(ui.actionInsert, 0);
signalMapper->setMapping(ui.actionMove, 1);
connect(ui.actionInsert, SIGNAL(triggered()), signalMapper, SLOT(map()));
connect(ui.actionMove, SIGNAL(triggered()), signalMapper, SLOT(map()));
connect(ui.intersection_button, SIGNAL(clicked()), m_oglviewer, SLOT(findIntersections()));
connect(ui.disp_ctrl_pts, SIGNAL(toggled(bool)), m_oglviewer, SLOT(setDispCtrlPts(bool)));
connect(ui.disp_curves, SIGNAL(toggled(bool)), m_oglviewer, SLOT(setDispCurves(bool)));
connect(ui.disp_intersections, SIGNAL(toggled(bool)), m_oglviewer, SLOT(setDispIntersections(bool)));
ui.foreground_color->setStyleSheet("QPushButton { background-color : #FFFFFF;}");
ui.background_color->setStyleSheet("QPushButton { background-color : #000000;}");
connect(ui.foreground_color, SIGNAL(clicked()), this, SLOT(pickColor()));
}
struct GridPointDataListIterator * getExtractGridDataReturnValues(FunctionCallInfo fcinfo)
{
struct PlaceSpecification ps;
Datum placeSpec = PG_GETARG_DATUM(0);
extractPlaceSpecification( & ps, & placeSpec );
GEOSGeom location = NULL;
if ( ! PG_ARGISNULL(1) )
{
bytea * locationRaw = PG_GETARG_BYTEA_P(1);
location = GEOSGeomFromWKB_buf((unsigned char *) VARDATA(locationRaw), VARSIZE(locationRaw) - VARHDRSZ);
}
enum InterpolationType interpolation = (enum InterpolationType) PG_GETARG_INT32(2);
FileId dataId = PG_GETARG_INT64(3);
TransactionId xid = GetTopTransactionId();
CommandId cid = GetCurrentCommandId(true); // Incremented for each function call in the same transaction
// function takes ownership of location parameter
struct GridPointDataListIterator * ret = readPoints(& ps, location, interpolation, dataId, xid, cid);
return ret;
}
/**
* main function
* divided to two brances for master & slave processors respectively
* @param argc commandline argument count
* @param argv array of commandline arguments
* @return 0 if success
*/
int main(int argc, char* argv[])
{
int rank;
int size;
int num_clusters;
int num_points;
int dex;
int job_size;
int job_done=0;
Point* centroids;
Point* points;
Point* received_points;
int * slave_clusters;
int * former_clusters;
int * latter_clusters;
MPI_Init(&argc, &argv);
MPI_Status status;
MPI_Comm_rank(MPI_COMM_WORLD, &rank);
MPI_Comm_size(MPI_COMM_WORLD, &size);
//creation of derived MPI structure
MPI_Datatype MPI_POINT;
MPI_Datatype type=MPI_DOUBLE;
int blocklen=2;
MPI_Aint disp=0;
MPI_Type_create_struct(1,&blocklen,&disp,&type,&MPI_POINT);
MPI_Type_commit(&MPI_POINT);
/******** MASTER PROCESSOR WORKS HERE******************************************************/
if(rank==MASTER)
{
//inputting from file
FILE *input;
input=fopen(argv[1],"r");
readHeaders(input,&num_clusters,&num_points);
points=(Point*)malloc(sizeof(Point)*num_points);
readPoints(input,points,num_points);
fclose(input);
//other needed memory locations
former_clusters=(int*)malloc(sizeof(int)*num_points);
latter_clusters=(int*)malloc(sizeof(int)*num_points);
job_size=num_points/(size-1);
centroids=malloc(sizeof(Point)*num_clusters);
//reseting and initializing to default behaviour
initialize(centroids,num_clusters);
resetData(former_clusters,num_points);
resetData(latter_clusters,num_points);
//Sending the essential data to slave processors
for(dex=1;dex<size;dex++)
{
printf("Sending to [%d]\n",dex);
MPI_Send(&job_size ,1 , MPI_INT ,dex,0,MPI_COMM_WORLD);
MPI_Send(&num_clusters ,1 , MPI_INT ,dex,0,MPI_COMM_WORLD);
MPI_Send(centroids ,num_clusters, MPI_POINT ,dex,0,MPI_COMM_WORLD);
MPI_Send(points+(dex-1)*job_size,job_size , MPI_POINT ,dex,0,MPI_COMM_WORLD);
}
printf("Sent!\n");
MPI_Barrier(MPI_COMM_WORLD);
//Main job of master processor is done here
while(1)
{
MPI_Barrier(MPI_COMM_WORLD);
printf("Master Receiving\n");
for(dex=1;dex<size;dex++)
MPI_Recv(latter_clusters+(job_size*(dex-1)),job_size,MPI_INT,dex,0,MPI_COMM_WORLD,&status);
printf("Master Received\n");
calculateNewCentroids(points,latter_clusters,centroids,num_clusters,num_points);
printf("New Centroids are done!\n");
if(checkConvergence(latter_clusters,former_clusters,num_points)==0)
{
printf("Converged!\n");
job_done=1;
}
else
{
printf("Not converged!\n");
for(dex=0;dex<num_points;dex++)
former_clusters[dex]=latter_clusters[dex];
}
//Informing slaves that no more job to be done
for(dex=1;dex<size;dex++)
MPI_Send(&job_done,1, MPI_INT,dex,0,MPI_COMM_WORLD);
MPI_Barrier(MPI_COMM_WORLD);
if(job_done==1)
break;
//Sending the recently created centroids
for(dex=1;dex<size;dex++)
MPI_Send(centroids,num_clusters, MPI_POINT,dex,0, MPI_COMM_WORLD);
MPI_Barrier(MPI_COMM_WORLD);
}
//Outputting to the output file
FILE* output=fopen(argv[2],"w");
fprintf(output,"%d\n",num_clusters);
fprintf(output,"%d\n",num_points);
for(dex=0;dex<num_clusters;dex++)
fprintf(output,"%lf,%lf\n",centroids[dex]._x,centroids[dex]._y);
for(dex=0;dex<num_points;dex++)
fprintf(output,"%lf,%lf,%d\n",points[dex]._x,points[dex]._y,latter_clusters[dex]+1);
fclose(output);
}
/*************END OF MASTER PROCESSOR'S BRANCH -- SLAVE PROCESSORS' JOB IS TO FOLLOW ************************/
else
{
//Receiving the essential data
printf("Receiving\n");
MPI_Recv(&job_size ,1 ,MPI_INT ,MASTER,0,MPI_COMM_WORLD,&status);
MPI_Recv(&num_clusters,1 ,MPI_INT ,MASTER,0,MPI_COMM_WORLD,&status);
centroids=malloc(sizeof(Point)*num_clusters);
MPI_Recv(centroids ,num_clusters,MPI_POINT,MASTER,0,MPI_COMM_WORLD,&status);
printf("part_size =%d\n",job_size);
received_points=(Point*)malloc(sizeof(Point)*job_size);
slave_clusters=(int*)malloc(sizeof(int)*job_size);
MPI_Recv(received_points,job_size,MPI_POINT ,MASTER,0,MPI_COMM_WORLD,&status);
printf("Received [%d]\n",rank);
MPI_Barrier(MPI_COMM_WORLD);
while(1)
{
printf("Calculation of new clusters [%d]\n",rank);
for(dex=0;dex<job_size;dex++)
{
slave_clusters[dex]=whoIsYourDaddy(received_points[dex],centroids,num_clusters);
}
printf("sending to master [%d]\n",rank);
MPI_Send(slave_clusters,job_size, MPI_INT,MASTER, 0, MPI_COMM_WORLD);
MPI_Barrier(MPI_COMM_WORLD);
MPI_Barrier(MPI_COMM_WORLD);
MPI_Recv(&job_done,1, MPI_INT,MASTER,0,MPI_COMM_WORLD,&status);
if(job_done==1) //No more work to be done
break;
//Receiving recently created centroids from master
MPI_Recv(centroids,num_clusters,MPI_POINT,MASTER,0, MPI_COMM_WORLD,&status);
MPI_Barrier(MPI_COMM_WORLD);
}
}
//End of all
MPI_Finalize();
return 0;
}
int main() {
// ## Make these inputs to the function ##
const char* camDataFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/PS3Eye_out_camera_data.yml";
const char* objPointsFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/LEDPos copy2.txt";
const char* imageFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/Test Images/Test3.jpg";
const char* imagePointsFilename = "/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/imagePts3RANDOM"
".txt";
// Open the correct image
std::cout << imageFilename << std::endl << imagePointsFilename << std::endl;
cv::Mat img = cv::imread(imageFilename);
cv::namedWindow("Window");
cv::imshow("Window", img);
// Get the camera matrix and distortion coefficients
cv::Mat cameraMatrix, distCoeffs;
getCalibrationData(camDataFilename, cameraMatrix, distCoeffs);
// Solve pose estimation problem
// get the object points (3d) and image points (2d)
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
std::cout << "Object Points:" << std::endl;
readPoints(objPointsFilename, objectPoints);
std::cout << "\nImage Points:" << std::endl;
readPoints(imagePointsFilename, imagePoints);
// sort the image points
ledFinder(imagePoints,imagePoints,1);
std::cout << "\nSorted Image Points:" << std::endl;
std::cout << imagePoints << std::endl;
// Provide an initial guess: (give an approximate attitude--assume following from behind)
// (OpenCV reference frame) NOTE: rvec has weird quaternion convention, but [0,0,0]
// corresponds to following direclty behind
cv::Mat rvec(3,1,CV_64F);
cv::Mat tvec(3,1,CV_64F);
rvec.at<double>(0,0) = 0*DEG2RAD;
rvec.at<double>(1,0) = 0*DEG2RAD;
rvec.at<double>(2,0) = 0*DEG2RAD;
tvec.at<double>(0,0) = 0*IN2MM;
tvec.at<double>(1,0) = 0*IN2MM;
tvec.at<double>(2,0) = 100*IN2MM;
//
// solve for the pose that minimizes reprojection error
// UNITS: (objectPoints = mm, imagePoints = pixels, ... , rvec = radians, tvec = mm)
clock_t t;
t = clock();
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, 1, CV_EPNP);
t = clock() - t;
std::cout << "\nTime To Estimate Pose: " << ((float)t/CLOCKS_PER_SEC*1000) << " ms" << std::endl;
// compute the theta and r parts of the Euler rotation
/*
double rvec_theta = cv::norm(rvec, cv::NORM_L2);
cv::Mat rvec_r = rvec/rvec_theta;
*/
// get rotation matrix
cv::Mat rotMat, CV2B(3,3,CV_64F,cv::Scalar(0)), B2CV;
CV2B.at<double>(0,2) = 1.0;
CV2B.at<double>(1,0) = 1.0;
CV2B.at<double>(2,1) = 1.0;
cv::transpose(CV2B,B2CV); // CV2B and B2CV convert between OpenCV
cv::Rodrigues(rvec,rotMat); // frame convention and typical body frame
// extract phi, theta, psi (NOTE: these are for typical body frame)
double phi, theta, psi;
rotMat = CV2B * rotMat * B2CV; // change the rotation matrix from CV convention to RHS body frame
theta = asin(-rotMat.at<double>(2,0)); // get the Euler angles
psi = acos(rotMat.at<double>(0,0) / cos(theta));
phi = asin(rotMat.at<double>(2,1) / cos(theta));
// add changes to the projection for troubleshooting
phi += 0*DEG2RAD; // phi, theta, psi in body frame
theta += 0*DEG2RAD;
psi += 0*DEG2RAD;
tvec.at<double>(0,0) += 0*IN2MM; //tvec in OpenCV frame
tvec.at<double>(1,0) += 0*IN2MM;
tvec.at<double>(2,0) += 0*IN2MM;
// reconstruct rotation matrix: from object frame to camera frame
rotMat.at<double>(0,0) = cos(theta)*cos(psi);
rotMat.at<double>(0,1) = sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi);
rotMat.at<double>(0,2) = cos(phi)*sin(theta)*cos(psi) + sin(phi)*sin(psi);
rotMat.at<double>(1,0) = cos(theta)*sin(psi);
rotMat.at<double>(1,1) = sin(phi)*sin(theta)*sin(psi) + cos(phi)*cos(psi);
rotMat.at<double>(1,2) = cos(phi)*sin(theta)*sin(psi) - sin(phi)*cos(psi);
rotMat.at<double>(2,0) = -sin(theta);
rotMat.at<double>(2,1) = sin(phi)*cos(theta);
rotMat.at<double>(2,2) = cos(phi)*cos(theta);
// rewrite the rvec from rotation matrix
rotMat = B2CV * rotMat * CV2B; // convert RHS body rotation matrix to OpenCV rotation matrix
cv::Rodrigues(rotMat,rvec);
// print to standard output
cv::Mat tvec_body;
tvec_body.push_back(tvec.at<double>(2,0)); // get tvec in body coordinates to display to
tvec_body.push_back(tvec.at<double>(0,0)); // standard output
tvec_body.push_back(tvec.at<double>(1,0));
std::cout << "\nPhi, Theta, Psi: " << (phi*RAD2DEG) << ", " << (theta*RAD2DEG) <<
", " << (psi*RAD2DEG) << std::endl;
std::cout << "\ntvec:\n" << (tvec_body*MM2IN) << std::endl;
std::cout << "\nTotal Distance: " << (cv::norm(tvec,cv::NORM_L2)*MM2IN) << std::endl;
// compute the (re)projected points
cv::vector<cv::Point3f> axesPoints; // create points for coordinate axes
axesPoints.push_back(cv::Point3f(0,0,0));
axesPoints.push_back(cv::Point3f(AXES_LN,0,0));
axesPoints.push_back(cv::Point3f(0,AXES_LN,0));
axesPoints.push_back(cv::Point3f(0,0,AXES_LN));
cv::vector<cv::Point2f> projImagePoints, projAxesPoints;
cv::projectPoints(objectPoints, rvec, tvec, cameraMatrix, distCoeffs, projImagePoints);
cv::projectPoints(axesPoints, rvec, tvec, cameraMatrix, distCoeffs, projAxesPoints);
std::cout << "\nProjected Image Points:\n" << projImagePoints << std::endl;
// ## Need to compute the projected error to determine feasibility Decide on a threshold
double reprojErr = scaledReprojError(imagePoints, projImagePoints);
std::cout << "\nReprojection Error " << reprojErr << std::endl << std::endl;
// Plot the LED's and reprojected positions on the image
for (int i=0; i<NO_LEDS; i++) {
cv::circle(img,imagePoints[i], 3, cv::Scalar(0,0,255), 2);
cv::circle(img,projImagePoints[i], 3, cv::Scalar(0,255,0), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[1], cv::Scalar(0,0,255), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[2], cv::Scalar(0,255,0), 2);
cv::line(img,projAxesPoints[0], projAxesPoints[3], cv::Scalar(255,0,0), 2);
}
cv::imshow("Window",img);
//cv::imwrite("/Users/michaeldarling/Dropbox/Thesis/OpenCV/Calibration/Zoomed In/VisionOut5.jpg",img);
cv::waitKey(0);
std::cout << "DONE!\n" << std::endl;
return 0;
}
void Application::_doRun
(
double& pbaTimeSum,
double& starTimeSum,
double& splayTimeSum,
double& outTimeSum,
double& gregTimeSum
)
{
Config& config = getConfig();
cout << "Run: " << config._run << endl;
// Get points and weights
if ( config._inFile ) readPoints();
else makePoints();
makeWeights();
// Initialize
gdelInit( config, _pointVec, _weightVec );
// Compute Delaunay
double timePba, timeInitialStar, timeConsistent, timeOutput;
HostTimer timerAll;
timerAll.start();
gdelCompute( timePba, timeInitialStar, timeConsistent, timeOutput );
timerAll.stop();
const double timeTotal = timerAll.value();
cout << "PBA: " << timePba << endl;
cout << "InitStar: " << timeInitialStar << endl;
cout << "Consistency: " << timeConsistent << endl;
cout << "StarOutput: " << timeOutput << endl;
cout << "TOTAL Time: " << timeTotal << endl;
pbaTimeSum += timePba;
starTimeSum += timeInitialStar;
splayTimeSum += timeConsistent;
outTimeSum += timeOutput;
gregTimeSum += timeTotal;
// Check
if ( config._doCheck )
{
TetraMesh tetMesh;
tetMesh.setPoints( _pointVec, _weightVec );
getTetraFromGpu( tetMesh );
tetMesh.check();
}
// Destroy
gdelDeInit();
_pointVec.clear();
_weightVec.clear();
return;
}
int main(int argc, char** argv)
{
if (argc != 4)
{
cerr << "Usage: " << argv[0] << " dim n area." << endl;
return -1;
}
int dim = atol(argv[1]);
int n = atol(argv[2]);
double area = atof(argv[3]);
if(dim <= 0)
{
cerr << "Dimension should be larger than 0." << endl;
return -1;
}
if(n <= 0)
{
cerr << "The number of query points should be larger than 0." << endl;
return -1;
}
if(area <= 0 || area > 1)
{
cerr << "the area of query points should be in (0, 1]." << endl;
return -1;
}
/*read static data set*/
vector <Point> P;
ifstream in("../data.ini");
if(!in)
{
cerr << "Cannot open file data.ini.\n";
return -1;
}
P = readPoints(in, dim);
uint32_t N = P.size();
try {
IStorageManager* memfile = StorageManager::createNewMemoryStorageManager();
StorageManager::IBuffer* file = StorageManager::createNewRandomEvictionsBuffer(*memfile, 10, false);
id_type indexIdentifier;
ISpatialIndex* tree = RTree::createNewRTree(*file, 0.7, CAPACITY, CAPACITY, dim, SpatialIndex::RTree::RV_RSTAR, indexIdentifier);
id_type id = 0;
for(uint32_t i = 0; i < N; ++i)
{
std::ostringstream os;
os << P[i];
std::string data = os.str();
tree->insertData(data.size() + 1, reinterpret_cast<const byte*>(data.c_str()), P[i], id);
id++;
}
/*std::cerr << "Operations: " << N << std::endl;
std::cerr << *tree;
std::cerr << "Buffer hits: " << file->getHits() << std::endl;
std::cerr << "Index ID: " << indexIdentifier << std::endl;
bool ret = tree->isIndexValid();
if (ret == false) std::cerr << "ERROR: Structure is invalid!" << std::endl;
else std::cerr << "The stucture seems O.K." << std::endl; */
for(uint32_t loop = 1; loop <= LOOPNUM; ++loop)
{
cout << "/**************** BEGIN " << loop << " ***************/" << endl;
/*generate query set*/
vector <Point> Q;
//Q = genPoints(dim, n, area, loop);
stringstream ss;
ss << "../query/n" << n << "M" << area << "/loop" << loop;
cout << ss.str().c_str() << endl;
ifstream qin(ss.str().c_str());
if(!qin)
{
cerr << "Cannot open query file";
return -1;
}
Q = readPoints(qin, dim);
/*************** BEGIN MQM method ******************/
MQM(tree, Q, n, FUN); // MQM method for finding ANN of Q
/*************** END MQM method *******************/
/*************** BEGIN ADM method ******************/
CATCH cost1;
cost1.catch_time();
vector <uint32_t> nnIDs = nearestNeighborSet(tree, Q, n); // find the NN for every qi in Q as qi'
vector <Point> newQ;
for(uint32_t i = 0; i < n; ++i)
{
newQ.push_back(P[nnIDs[i]]);
}
cost1.catch_time();
cout << "proposal method: cpu cost for finding NNs of Q as Q' is " << cost1.get_cost(2) << " millisecond(s)" << endl;
/***** read dist-matrix index for Q' ********/
uint32_t maxK = P.size() / RATIO; // the length of dist-matrix index
uint32_t * dmindex[n];
for(uint32_t i = 0; i < n; ++i)
{ // read the dist-matrix index of qi'
dmindex[i] = readDMIndex(nnIDs[i], maxK);
if (dmindex[i] == NULL)
{
cerr << "error for loading Dist-Matrix Index." << endl;
return -1;
}
}
double minadist = 0;
/* ADM method for finding approxiamte ANN */
Point adm_ANN = ADM(newQ, n, P, N, dmindex, maxK, FUN, minadist, ERROR_RATE);
cout << "ADM: best_dist is " << getAdist(adm_ANN, Q, n, FUN) << endl << endl;
/*************** END ADM method *******************/
/*************** BEGIN approxiamte vp-ANN method ******************/
/* approximate vp-ANN method for finding ANN of Q'*/
CATCH cost2;
cost2.catch_time();
Point centroid = findCentroid(Q, dim, n);
minadist = getAdist(centroid, Q, n, FUN);
uint32_t vpID = nearestNeighbor(tree, centroid);
uint32_t best_id_Q = epsilonANN(Q, n, P, N, vpID, dmindex, maxK, FUN);
cost2.catch_time();
cout << "approxiamte vp-ANN method: cpu cost is " << cost2.get_cost(2) << " millisecond(s)" << endl;
cout << "approximate vp-ANN method: best_dist is " << getAdist(P[best_id_Q], Q, n, FUN) << endl;
cout << "approxiamte vp-ANN method: best_NN is ";
displayCoordinates(P[best_id_Q]);
cout << endl;
/*************** END approxiamte vp-ANN method *******************/
/*************** BEGIN BF MBM method ******************/
/* MBM method for finding ANN of Q */
CATCH mbmcost;
mbmcost.catch_time();
Region M = getMBR(Q, dim, n);
MyQueryStrategy qs = MyQueryStrategy(M, Q, FUN);
tree->queryStrategy(qs);
mbmcost.catch_time();
cout << "MBM: cpu cost is " << mbmcost.get_cost(2) << " millisecond(s)" << endl;
cout << "MBM: best_dist is " << qs.best_dist << endl;
cout << "MBM: best_NN is ";
displayCoordinates(qs.best_NN);
cout << "MBM: leafIO = " << qs.mbm_leafIO << "; indexIO = " << qs.mbm_indexIO << endl << endl;
/*************** END BF MBM method *******************/
/*************** BEGIN brute method ******************/
/* brute method for finding ANN of Q*/
CATCH brute_cost;
brute_cost.catch_time();
uint32_t ANNid = brute_ANN(Q, n, P, N, FUN);
brute_cost.catch_time();
cout << "brute method: cpu cost is " << brute_cost.get_cost(2) << " millisecond(s)" << endl;
double adist = getAdist(P[ANNid], Q, n, FUN);
cout << "brute method: best_dist is " << adist << endl;
cout << "brute method: best_NN is ";
displayCoordinates(P[ANNid]);
/*************** END brute method *******************/
cout << "/**************** END " << loop << " ****************/" << endl << endl;
} // end loop
delete tree;
delete file;
delete memfile;
}
catch(Tools::Exception& e)
{
cerr << "*********ERROR**********" << endl;
std::string s = e.what();
cerr << s << endl;
return -1;
}
catch(...)
{
cerr << "**********ERROR********" << endl;
return -1;
}
return 1;
}
int main (int argc, char **argv)
{
Options mergetr = readParameters (argc, argv);
char filename_mesh_node[FILENAME_MAX];
char filename_mesh_ele[FILENAME_MAX];
char filename_otoczka[FILENAME_MAX];
char filename_output_node[FILENAME_MAX];
char filename_output_ele[FILENAME_MAX];
int no_of_meshes = argc-mergetr.args_start;
strcpy (filename_otoczka, mergetr.input);
if ( strstr (filename_otoczka, ".poly") == NULL)
strcat (filename_otoczka, ".poly");
strcpy (filename_output_node, mergetr.output);
strcat (filename_output_node, ".node");
strcpy (filename_output_ele, mergetr.output);
strcat (filename_output_ele, ".ele");
fprintf(stdout, "************************************\n");
fprintf(stdout, "***** M * E * R * G * E * T * R ****\n");
fprintf(stdout, "************************************\n");
fprintf(stdout, "* Otoczka filename: %s\n", filename_otoczka);
fprintf(stdout, "* Output filenames: %s & %s\n", filename_output_node, filename_output_ele);
fprintf(stdout, "* Triangle options: %s\n", mergetr.tr_opt);
fprintf(stdout, "************************************\n");
struct triangulateio *siatka;
struct triangulateio otoczka;
struct triangulateio out;
EdgeList **v;
PointList **p;
int i;
siatka = malloc ( no_of_meshes * sizeof *siatka);
v = malloc ( no_of_meshes * sizeof **v );
p = malloc ( no_of_meshes * sizeof **p );
if (siatka == NULL || v == NULL || p == NULL) {
fprintf (stderr, "** Error! Not enough memory!");
return -1;
}
initTriangulation (&otoczka);
/* OTOCZKA */
FILE *file_otoczka = fopen( filename_otoczka, "r");
if (file_otoczka == NULL) {
fprintf(stderr, "** Error while opening %s\n", filename_otoczka);
return -100;
}
readPoints (file_otoczka, &otoczka);
readSegments (file_otoczka, &otoczka);
readHoles (file_otoczka, &otoczka);
readRegions (file_otoczka, &otoczka);
fclose (file_otoczka);
/* MESHES */
for (i = 0; i < (argc - mergetr.args_start); i++) {
strcpy (filename_mesh_node, argv[mergetr.args_start+i]);
strcat (filename_mesh_node, ".node");
strcpy (filename_mesh_ele, argv[mergetr.args_start+i]);
strcat (filename_mesh_ele, ".ele");
fprintf(stdout, "************************************\n");
fprintf(stdout, "* Mesh filenames: %s & %s\n", filename_mesh_node, filename_mesh_ele);
fprintf(stdout, "************************************\n");
FILE *file_mesh_node = fopen( filename_mesh_node, "r");
FILE *file_mesh_ele = fopen( filename_mesh_ele, "r");
if (file_mesh_node == NULL) {
fprintf(stderr, "** Error while opening %s\n", filename_mesh_node);
return -101;
}
if (file_mesh_node == NULL) {
fprintf(stderr, "** Error while opening %s\n", filename_mesh_ele);
return -102;
}
initTriangulation (&siatka[i]);
readPoints (file_mesh_node, &siatka[i]);
readTriangles (file_mesh_ele, &siatka[i]);
fclose (file_mesh_node);
fclose (file_mesh_ele);
v[i] = createEdgeList(siatka[i]);
markBndEdges (siatka[i], v[i]);
p[i] = makePointList (otoczka, siatka[i], v[i]);
updatePoints (&otoczka, siatka[i], v[i], p[i]);
updateSegments (&otoczka, siatka[i], v[i], p[i]);
updateHoles (&otoczka, siatka[i]);
}
fprintf(stdout, "************************************\n");
/* TRIANGULAtE */
initTriangulation (&out);
strcat (mergetr.tr_opt, "pYYQ");
triangulate (mergetr.tr_opt, &otoczka, &out, (struct triangulateio *) NULL);
/* GLUE HOLES */
/* markNotBndEdges (siatka1, v); */
for (i = 0; i < no_of_meshes; i++) {
glueNotBndPoints (&out, siatka[i], p[i]); /* DOKLEJANIE DO OUT */
fixPointListNumbers (&out, &siatka[i], p[i]);
glueInteriorTriangles (&out, siatka[i], p[i]);
removeHole (&out);
}
FILE *file_output_node = fopen (filename_output_node, "w");
FILE *file_output_ele = fopen (filename_output_ele, "w");
writePoints (file_output_node, out);
writeTriangles (file_output_ele, out);
fclose (file_output_node);
fclose (file_output_ele);
fprintf(stdout, "************************************\n");
free (p);
free (v);
freeTriangulation (&otoczka);
freeTriangulation (&out);
for (i = 0; i < no_of_meshes; i++)
freeTriangulation (&siatka[i]);
return 0;
}
int main(int argc, char** argv)
{
if (argc != 4)
{
cerr << "Usage: " << argv[0] << " dim n area." << endl;
return -1;
}
int dim = atol(argv[1]);
int n = atol(argv[2]);
double area = atof(argv[3]);
if(dim <= 0)
{
cerr << "Dimension should be larger than 0." << endl;
return -1;
}
if(n <= 0)
{
cerr << "The number of query points should be larger than 0." << endl;
return -1;
}
if(area <= 0 || area > 1)
{
cerr << "the area of query points should be in (0, 1]." << endl;
return -1;
}
/*read static data set*/
vector <Point> P;
ifstream in("../data.ini");
if(!in)
{
cerr << "Cannot open file data.ini.\n";
return -1;
}
P = readPoints(in, dim);
uint32_t N = P.size();
try {
IStorageManager* memfile = StorageManager::createNewMemoryStorageManager();
StorageManager::IBuffer* file = StorageManager::createNewRandomEvictionsBuffer(*memfile, 10, false);
id_type indexIdentifier;
ISpatialIndex* tree = RTree::createNewRTree(*file, 0.7, CAPACITY, CAPACITY, dim, SpatialIndex::RTree::RV_RSTAR, indexIdentifier);
id_type id = 0;
for(uint32_t i = 0; i < N; ++i)
{
std::ostringstream os;
os << P[i];
std::string data = os.str();
tree->insertData(data.size() + 1, reinterpret_cast<const byte*>(data.c_str()), P[i], id);
id++;
}
//for(uint32_t loop = 1; loop <= LOOPNUM; ++loop)
for(uint32_t loop = 55; loop <= LOOPNUM; ++loop)
{
cout << "/**************** BEGIN " << loop << " ***************/" << endl;
/*generate query set*/
vector <Point> Q;
//Q = genPoints(dim, n, area, loop);
stringstream ss;
ss << "../query/n" << n << "M" << area << "/loop" << loop;
cout << ss.str().c_str() << endl;
ifstream qin(ss.str().c_str());
if(!qin)
{
cerr << "Cannot open query file";
return -1;
}
Q = readPoints(qin, dim);
/*************** BEGIN MQM method ******************/
//double err_arr[] = {0, 0.001, 0.005, 0.01, 0.05, 0.1};
double err_arr[] = {0, 0.01, 0.1};
for (uint32_t i = 0; i < sizeof(err_arr) / sizeof(double); ++i)
{
MQM(tree, Q, n, FUN, err_arr[i]); // MQM method for finding ANN of Q
}
/*************** END MQM method *******************/
/*************** BEGIN BF MBM method ******************/
/* MBM method for finding ANN of Q */
/*CATCH mbmcost;
mbmcost.catch_time();
Region M = getMBR(Q, dim, n);
MyQueryStrategy qs(M, Q, FUN);
tree->queryStrategy(qs);
mbmcost.catch_time();
cout << "MBM: cpu cost is " << mbmcost.get_cost(2) << " millisecond(s)" << endl;
cout << "MBM: best_dist is " << qs.best_dist << endl;
cout << "MBM: best_NN is ";
displayCoordinates(qs.best_NN);
cout << "MBM: leafIO = " << qs.mbm_leafIO << "; indexIO = " << qs.mbm_indexIO << endl << endl; */
/*************** END BF MBM method *******************/
/*************** BEGIN brute method ******************/
/* brute method for finding ANN of Q*/
/*CATCH brute_cost;
brute_cost.catch_time();
uint32_t ANNid = brute_ANN(Q, n, P, N, FUN);
brute_cost.catch_time();
cout << "brute method: cpu cost is " << brute_cost.get_cost(2) << " millisecond(s)" << endl;
double adist = getAdist(P[ANNid], Q, n, FUN);
cout << "brute method: best_dist is " << adist << endl;
cout << "brute method: best_NN is ";
displayCoordinates(P[ANNid]); */
/*************** END brute method *******************/
cout << "/**************** END " << loop << " ****************/" << endl << endl;
} // end loop
delete tree;
delete file;
delete memfile;
}
catch(Tools::Exception& e)
{
cerr << "*********ERROR**********" << endl;
std::string s = e.what();
cerr << s << endl;
return -1;
}
catch(...)
{
cerr << "**********ERROR********" << endl;
return -1;
}
return 1;
}
