static void completeFacet(rcContext* ctx, const float* pts, int npts, int* edges, int& nedges, const int maxEdges, int& nfaces, int e)
{
static const float EPS = 1e-5f;
int* edge = &edges[e*4];
// Cache s and t.
int s,t;
if (edge[2] == UNDEF)
{
s = edge[0];
t = edge[1];
}
else if (edge[3] == UNDEF)
{
s = edge[1];
t = edge[0];
}
else
{
// Edge already completed.
return;
}
// Find best point on left of edge.
int pt = npts;
float c[3] = {0,0,0};
float r = -1;
for (int u = 0; u < npts; ++u)
{
if (u == s || u == t) continue;
if (vcross2(&pts[s*3], &pts[t*3], &pts[u*3]) > EPS)
{
if (r < 0)
{
// The circle is not updated yet, do it now.
pt = u;
circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
continue;
}
const float d = vdist2(c, &pts[u*3]);
const float tol = 0.001f;
if (d > r*(1+tol))
{
// Outside current circumcircle, skip.
continue;
}
else if (d < r*(1-tol))
{
// Inside safe circumcircle, update circle.
pt = u;
circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
}
else
{
// Inside epsilon circum circle, do extra tests to make sure the edge is valid.
// s-u and t-u cannot overlap with s-pt nor t-pt if they exists.
if (overlapEdges(pts, edges, nedges, s,u))
continue;
if (overlapEdges(pts, edges, nedges, t,u))
continue;
// Edge is valid.
pt = u;
circumCircle(&pts[s*3], &pts[t*3], &pts[u*3], c, r);
}
}
}
// Add new triangle or update edge info if s-t is on hull.
if (pt < npts)
{
// Update face information of edge being completed.
updateLeftFace(&edges[e*4], s, t, nfaces);
// Add new edge or update face info of old edge.
e = findEdge(edges, nedges, pt, s);
if (e == UNDEF)
addEdge(ctx, edges, nedges, maxEdges, pt, s, nfaces, UNDEF);
else
updateLeftFace(&edges[e*4], pt, s, nfaces);
// Add new edge or update face info of old edge.
e = findEdge(edges, nedges, t, pt);
if (e == UNDEF)
addEdge(ctx, edges, nedges, maxEdges, t, pt, nfaces, UNDEF);
else
updateLeftFace(&edges[e*4], t, pt, nfaces);
nfaces++;
}
else
{
updateLeftFace(&edges[e*4], s, t, HULL);
}
}
void reduceCapacity(const Graph::edge_iterator& ii, const GNode& src, const GNode& dst, int amount) {
int& cap1 = app.graph.getEdgeData(ii);
int& cap2 = app.graph.getEdgeData(findEdge(app.graph, dst, src));
cap1 -= amount;
cap2 += amount;
}
int main( int argc, char** argv )
{
/*
* serial stuff
*/
int fd = 0;
char serialport[256];
int baudrate = B19200;
//FIXME
fd = serialport_init("/dev/ttyUSB0", baudrate);
if(fd==-1) return -1;
usleep(3000 * 1000);
///////////////////////////////////////////////////////
int c = 0, fps = 0;
//capture from camera
CvCapture *capture = cvCaptureFromCAM(1);
//quit if camera not found
if(!capture) {
printf("cannot init capture!\n");
return -1;
}
//display original video stream
cvNamedWindow("stream", CV_WINDOW_NORMAL);
cvResizeWindow("stream", 320, 240);
cvNamedWindow("hue", CV_WINDOW_NORMAL);
cvResizeWindow("hue", 320, 240);
cvMoveWindow("hue", 320, 0);
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 2, CV_AA);
//keep capturing frames until escape
while(c != 27) //27 is escape key
{
//quit if can't grab frame
if(!(frame = cvQueryFrame(capture))) break;
//show the default image
//cvShowImage("stream", frame);
//edge detection - todo: HSV color filtering
findLine(frame);
//edge detection
findEdge(frame);
if(flag == 2)
cvPutText(frame, "right edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
else if(flag == 1)
cvPutText(frame, "left edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
//display center of gravity in coordinates
COG(&cog_x, &cog_y);
char x_coord[10];
char y_coord[10];
sprintf(x_coord, "%1.2f", cog_x);
sprintf(y_coord, "%1.2f", cog_y);
//find center of y coordinate in left and right edge
COG_edges(&left_y, &right_y);
//printf("%1.2f\n", left_y);
//printf("%1.2f\n", right_y);
//find slant
findSlant(frame);
char write_slant[15];
if(slant && offset) sprintf(write_slant, "%s", "crooked slant!"); //eventually invokes s movement
else sprintf(write_slant, "%s", "\0");
//TODO: FIXME
//motor1 is closer to bottom, motor 2 is closer to top (in starting config)
uint8_t b = 0b00000000;
//motor logic
char motor1[10];
char motor2[10];
//handles any errors - move 0.5 second for now
//might have to flip these
if(flag == 1)
{
b = 0b00000111;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "backward");
write(fd,&b,1);
}
else if(flag == 2)
{
b = 0b00000100;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "forward");
write(fd,&b,1);
}
else if(ROR) //rotate right (eventually look at angle and change timing appropriately)
{
printf("%s\n", "ROTATE RIGHT");
sleep(2);
b = 0b00000101;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "backward");
write(fd,&b,1);
}
else if(ROL) //rotate left (eventually look at angle and change timing appropriately)
{
printf("%s\n", "ROTATE LEFT");
sleep(2);
b = 0b00000110;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "forward");
write(fd,&b,1);
}
//NO ERROR - THIS HANDLES LOGIC TO DRIVE ROBOT IN CORRECT SEQUENCE
//determines the amount of time and directions in which to move the motors.
//the arduino is responsible for sending stop command to wheels!
else
{
switch(count)
{
//first shingle - already in place
case 0:
b = 0b00000000; //do nothing
write(fd,&b,1);
//sleep(6); //however long it takes to place shingle, or wait until arduino says go
printf("%s\n", "case 0");
break;
//second/third shingle - drive forward one foot (for now 2 secs, figure this out!)
case 1:
case 2:
b = 0b01000000; //forward 1 foot
write(fd,&b,1);
sleep(3);
printf("%s\n", "case 1/2");
break;
//left (viewing from bottom) overhang
case 3:
b = 0b00100011; //backward 1/2 foot
write(fd,&b,1);
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
b = 0b00100011; //backward 1/2 foot
write(fd,&b,1);
//sleep(12);
printf("%s\n", "case 3");
break;
//right overhang
case 4:
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
b = 0b01000000; //forward 1 foot
write(fd,&b,1);
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
//sleep(14);
printf("%s\n", "case 4");
break;
//next to right overhang
case 5:
b = 0b01000011; //backward 1 foot
write(fd,&b,1);
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
//sleep(8);
break;
//next to left overhang
case 6:
b = 0b01000000; //forward 1 foot
write(fd,&b,1);
//sleep(6);
break;
//reposition at beginning position
case 7:
b = 0b00110001; //ROR 90 degrees
write(fd,&b,1);
b = 0b00100000; //forward 1/2 foot
write(fd,&b,1);
b = 0b00110010; //ROL 90 degrees
write(fd,&b,1);
b = 0b01100011; //backward 1+1/2 foot
write(fd,&b,1);
//sleep(14);
break;
//should not get here
default:
b = 0; //error
write(fd,&b,1);
//sleep(20);
break;
}
//count = (count+1)%8; //FIXME
}
//SERIAL - motor logic
//xxxx_xxyy -> y = motor1/motor2, 0 = forward, 1 = backward, x = time in ms
//edges
//write(fd,&b,1);
//cvPutText(frame, x_coord, cvPoint(30,300), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "y:", cvPoint(0,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, y_coord, cvPoint(30,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor1:", cvPoint(0,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor1, cvPoint(150,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor2:", cvPoint(0,200), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor2, cvPoint(150,200), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, write_slant, cvPoint(0,50), &font, cvScalar(255, 0,
0, 0));
cvShowImage("stream", frame);
c = cvWaitKey(10);
//release images
//cvReleaseImage(&image);
//cvReleaseImage(&edge);
//cvReleaseImage(&final);
//cvReleaseImage(&frame);
//cvReleaseImage(&bw);
//cvReleaseImage(&gray);
}
//release created images
cvReleaseCapture(&capture);
return 0;
}
int Steiner::steiner(const set<ListGraph::Node> terminals)
{
if (this->s != 0) delete this->s;
this->s = new ListGraph();
if (this->sweight != 0) delete this->sweight;
this->sweight = new ListGraph::EdgeMap<int>(*this->s);
// perform dijkstra for every terminal
ListGraph::NodeMap<Dijkstra*> dijk(this->g);
for (set<ListGraph::Node>::iterator it = terminals.begin(); it != terminals.end(); ++it)
{
dijk[*it] = new Dijkstra(this->g, this->weight);
dijk[*it]->dijkstra(*it);
}
// build intermediate graph
ListGraph intermediate;
ListGraph::EdgeMap<int> iweight(intermediate);
map<ListGraph::Node, ListGraph::Node> imapper;
for (set<ListGraph::Node>::iterator it = terminals.begin(); it != terminals.end(); ++it)
{
ListGraph::Node n = intermediate.addNode();
imapper[n] = *it;
}
for (ListGraph::NodeIt it1(intermediate); it1 != INVALID; ++it1)
{
ListGraph::NodeIt it2 = it1;
for (++it2; it2 != INVALID; ++it2)
{
ListGraph::Edge e = intermediate.addEdge(it1, it2);
iweight[e] = (*dijk[imapper[it1]]->dist)[imapper[it2]];
}
}
// compute mst
MST mst(intermediate, iweight);
mst.prim();
// Kruskal mst(intermediate, iweight);
// mst.kruskal();
// build final graph
map<ListGraph::Node, ListGraph::Node> smapper;
for (set<ListGraph::Edge>::iterator it = mst.mst->begin(); it != mst.mst->end(); ++it)
{ // for each edge in the mst
// add end nodes to graph
ListGraph::Node u = imapper[intermediate.u(*it)];
if (smapper.count(u) == 0) smapper[u] = this->s->addNode();
ListGraph::Node v = imapper[intermediate.v(*it)];
if (smapper.count(v) == 0) smapper[v] = this->s->addNode();
ListGraph::Node last = v;
ListGraph::Node cur = v;
do
{ // walk through path
cur = (*dijk[u]->pred)[cur];
if (smapper.count(cur) == 0) smapper[cur] = this->s->addNode();
// add edge to graph, if not already existing
if (findEdge(*this->s, smapper[last], smapper[cur]) == INVALID)
{
ListGraph::Edge e = this->s->addEdge(smapper[last], smapper[cur]);
(*this->sweight)[e] = (*dijk[u]->dist)[last] - (*dijk[u]->dist)[cur];
}
last = cur;
}
while (cur != u);
}
// compute overall weight
int overallw = 0;
for (ListGraph::EdgeIt e(*this->s); e != INVALID; ++e)
{
overallw += (*this->sweight)[e];
}
// clean up dijkstras
for (set<ListGraph::Node>::iterator it = terminals.begin(); it != terminals.end(); ++it)
{
delete dijk[*it];
}
return overallw;
}
//OpenMP Implementation of Dijkstra's Algorithm
void DijkstraOMP(Vertex *vertices, Edge *edges, int *weights, Vertex *root)
{
double start, end;
root->visited = TRUE;
int len[V];
len[(int)root->title] = 0;
int i, j;
for(i = 0; i < V;i++)
{
if(vertices[i].title != root->title)
{
len[(int)vertices[i].title] = findEdge(*root, vertices[i], edges, weights);
}
else{
vertices[i].visited = TRUE;
}
}
start = omp_get_wtime();
for(j = 0; j < V; j++){
Vertex u;
int h = minPath(vertices, len);
u = vertices[h];
//OpenMP Parallelization Starts here!!!
#pragma omp parallel for schedule(runtime) private(i)
for(i = 0; i < V; i++)
{
if(vertices[i].visited == FALSE)
{
int c = findEdge( u, vertices[i], edges, weights);
len[vertices[i].title] = minimum(len[vertices[i].title], len[u.title] + c);
}
}
}
end = omp_get_wtime();
printArray(len);
printf("Running time: %f ms\n", (end - start)*1000);
}
void operator()(GNode node, Galois::UserContext<GNode>& ctx) {
// Find self-edge for this node, update it
bool hasEdge = false;
edgedata& factor = graph.getEdgeData(findEdge(graph, node, node, &hasEdge),
Galois::MethodFlag::NONE);
assert(hasEdge);
assert(factor > 0);
factor = sqrt(factor);
assert(factor != 0 && !isnan(factor));
// Update seen flag on node
Node &noded = graph.getData(node);
assert(!noded.seen);
noded.seen = true;
//std::cout << "STARTING " << noded.id << " " << factor << "\n";
//printf("STARTING %4d %10.5f\n", noded.id, factor);
// Update all edges (except self-edge)
for (Graph::edge_iterator
ii = graph.edge_begin(node, Galois::MethodFlag::ALL),
ei = graph.edge_end(node, Galois::MethodFlag::ALL);
ii != ei; ++ii) {
GNode dst = graph.getEdgeDst(ii);
Node &dstd = graph.getData(dst);
if ( !dstd.seen ) {
edgedata &ed = graph.getEdgeData(ii, Galois::MethodFlag::NONE);
ed /= factor;
//printf("N-EDGE %4d %4d %10.5f\n", noded.id, graph.getData(dst).id, ed);
//std::cout << noded.id << " " << dstd.id << " " << ed << "\n";
}
}
// Update all edges between neighbors (we're operating on the filled graph,
// so we they form a (directed) clique)
for (Graph::edge_iterator
iis = graph.edge_begin(node, Galois::MethodFlag::ALL),
eis = graph.edge_end(node, Galois::MethodFlag::ALL);
iis != eis; ++iis) {
GNode src = graph.getEdgeDst(iis);
Node &srcd = graph.getData(src);
if ( srcd.seen ) continue;
edgedata& eds = graph.getEdgeData(iis, Galois::MethodFlag::NONE);
// Enumerate all other neighbors
for (Graph::edge_iterator
iid = graph.edge_begin(node, Galois::MethodFlag::ALL),
eid = graph.edge_end(node, Galois::MethodFlag::ALL);
iid != eid; ++iid) {
GNode dst = graph.getEdgeDst(iid);
Node &dstd = graph.getData(dst);
if ( dstd.seen ) continue;
// Find the edge that bridges these two neighbors
hasEdge = false;
Graph::edge_iterator bridge = findEdge(graph, src, dst, &hasEdge);
if ( !hasEdge ) continue;
// Update the weight of the bridge edge
edgedata &edd = graph.getEdgeData(iid, Galois::MethodFlag::NONE),
&edb = graph.getEdgeData(bridge, Galois::MethodFlag::NONE);
edb -= eds*edd;
//printf("I-EDGE %4d %4d %10.5f\n", srcd.id, dstd.id, edb);
//std::cout << srcd.id << " " << dstd.id << " " << edb << "\n";
}
}
//std::cout << "OPERATED ON " << noded.id << "\n";
//sleep(1); // Maybe use this to help debug parallelism
}
void QuadricErrorSimplification::contractEdge(STTriangleMesh* m, Edge* e){
//find the indexes of the vertices
int i1 = findVertex(m, e->vertex1);
int i2 = findLastVertex(m, e->vertex2);
if (i1==-1){
printf("Error: QuadricErrorSimplification::contractEdges could not find vertex at %f,%f,%f\n",
e->vertex1->pt.x,e->vertex1->pt.y,e->vertex1->pt.z);
}
if (i2==-1){
printf("Error: QuadricErrorSimplification::contractEdges could not find vertex at %f,%f,%f\n",
e->vertex2->pt.x,e->vertex2->pt.y,e->vertex2->pt.z);
}
STMatrix4* Q1 = qMatrixes[i1];
STMatrix4* Q2 = qMatrixes[i2];
STVertex *a = m->mVertices[i1];
STVertex *b = m->mVertices[i2];
STVertex *w = new STVertex(0,0,0,0,0);
if (i1 == i2){
w->pt.x = a->pt.x;
w->pt.y = a->pt.y;
w->pt.z = a->pt.z;
}else{
computeCost(m, a, b, w, Q1, Q2);
}
printf("ContractingVertex %f,%f,%f\n",w->pt.x,w->pt.y,w->pt.z);
printf("From %f,%f,%f\n",a->pt.x,a->pt.y,a->pt.z);
printf("And %f,%f,%f\n\n",b->pt.x,b->pt.y,b->pt.z);
//remove unnecessary faces:
for(int itr=0; itr != m->mFaces.size(); ++itr)
{
int vertexesReplaced = 0;
if(vertexEquals(m->mFaces[itr]->v[0],a) || vertexEquals(m->mFaces[itr]->v[0],b))
{
m->mFaces[itr]->v[0] = w;
vertexesReplaced++;
}
if(vertexEquals(m->mFaces[itr]->v[1],a) || vertexEquals(m->mFaces[itr]->v[1],b))
{
m->mFaces[itr]->v[1] = w;
vertexesReplaced++;
}
if(vertexEquals(m->mFaces[itr]->v[2],a) || vertexEquals(m->mFaces[itr]->v[2],b))
{
m->mFaces[itr]->v[2] = w;
vertexesReplaced++;
}
//if a face needed to chance 2 points, that face has been eliminated
if (vertexesReplaced>1){
STFace* removed = m->mFaces[itr];
m->mFaces.erase(m->mFaces.begin()+itr);
delete removed;
itr--;
}
}
//remove v1, v2, Q1, and Q2, and add w and sum_Q
STMatrix4* sum_Q = new STMatrix4();
sum_Q->table[0][0] = Q1->table[0][0] + Q2->table[0][0];
sum_Q->table[0][1] = Q1->table[0][1] + Q2->table[0][1];
sum_Q->table[0][2] = Q1->table[0][2] + Q2->table[0][2];
sum_Q->table[0][3] = Q1->table[0][3] + Q2->table[0][3];
sum_Q->table[1][0] = Q1->table[1][0] + Q2->table[1][0];
sum_Q->table[1][1] = Q1->table[1][1] + Q2->table[1][1];
sum_Q->table[1][2] = Q1->table[1][2] + Q2->table[1][2];
sum_Q->table[1][3] = Q1->table[1][3] + Q2->table[1][3];
sum_Q->table[2][0] = Q1->table[2][0] + Q2->table[2][0];
sum_Q->table[2][1] = Q1->table[2][1] + Q2->table[2][1];
sum_Q->table[2][2] = Q1->table[2][2] + Q2->table[2][2];
sum_Q->table[2][3] = Q1->table[2][3] + Q2->table[2][3];
int i;
for (i = 0; i<edges.size();i++){
bool recalculated = i1==i2;
if (vertexEquals(edges[i]->vertex1,a)||vertexEquals(edges[i]->vertex1,b)){
edges[i]->vertex1 = w;
if (!recalculated){
STVertex tmp = STVertex(0,0,0,0,0);
edges[i]->cost=computeCost(m, edges[i]->vertex1,edges[i]->vertex2,&tmp,
sum_Q,
qMatrixes[findLastVertex(m,edges[i]->vertex2)]);
//printf("recomputedCost %f\n",double(edges[i]->cost));
recalculated = true;
}
}
if (vertexEquals(edges[i]->vertex2,a)||vertexEquals(edges[i]->vertex2,b)){
edges[i]->vertex2 = w;
if (!recalculated){
STVertex tmp = STVertex(0,0,0,0,0);
edges[i]->cost=computeCost(m, edges[i]->vertex1,edges[i]->vertex2,&tmp,
qMatrixes[findVertex(m,edges[i]->vertex1)],
sum_Q);
//printf("recomputedCost %f\n",double(edges[i]->cost));
recalculated = true;
}
}
}
//The order the vertexes and Qs are removed is important:
//the first removal may affect the second
int max = std::max(i1,i2);
int min = std::min(i1,i2);
m->mVertices.erase(m->mVertices.begin()+max);
qMatrixes.erase(qMatrixes.begin()+max);
if (i1!=i2){
m->mVertices.erase(m->mVertices.begin()+min);
qMatrixes.erase(qMatrixes.begin()+min);
}
//delete Q1;
//delete Q2;
//We must also delete this edge
int ie = findEdge(this, e);
e = edges[ie];
edges.erase(edges.begin()+ie);
//delete e;
m->mVertices.push_back(w);
qMatrixes.push_back(sum_Q);
}
PLANE_GRAPH *decodePlanarCode(unsigned short* code, PG_INPUT_OPTIONS *options) {
int i, j, codePosition, nv, maxn;
int edgeCounter = 0;
PG_EDGE *inverse;
nv = code[0];
codePosition = 1;
if(options->maxn <= 0){
maxn = nv*options->maxnFactor;
} else {
maxn = options->maxn;
}
PLANE_GRAPH *pg = newPlaneGraph(maxn, options->maxe);
pg->nv = nv;
for (i = 0; i < nv; i++) {
pg->degree[i] = 0;
pg->firstedge[i] = pg->edges + edgeCounter;
pg->edges[edgeCounter].start = i;
pg->edges[edgeCounter].end = code[codePosition] - 1;
pg->edges[edgeCounter].next = pg->edges + edgeCounter + 1;
if (code[codePosition] - 1 < i) {
inverse = findEdge(pg, code[codePosition] - 1, i);
pg->edges[edgeCounter].inverse = inverse;
inverse->inverse = pg->edges + edgeCounter;
} else {
pg->edges[edgeCounter].inverse = NULL;
}
edgeCounter++;
codePosition++;
for (j = 1; code[codePosition]; j++, codePosition++) {
pg->edges[edgeCounter].start = i;
pg->edges[edgeCounter].end = code[codePosition] - 1;
pg->edges[edgeCounter].prev = pg->edges + edgeCounter - 1;
pg->edges[edgeCounter].next = pg->edges + edgeCounter + 1;
if (code[codePosition] - 1 < i) {
inverse = findEdge(pg, code[codePosition] - 1, i);
pg->edges[edgeCounter].inverse = inverse;
inverse->inverse = pg->edges + edgeCounter;
} else {
pg->edges[edgeCounter].inverse = NULL;
}
edgeCounter++;
}
pg->firstedge[i]->prev = pg->edges + edgeCounter - 1;
pg->edges[edgeCounter - 1].next = pg->firstedge[i];
pg->degree[i] = j;
codePosition++; /* read the closing 0 */
}
pg->ne = edgeCounter;
if(options->computeDual){
makeDual(pg);
}
return pg;
}
//finds similiar edge with the same vertices (source and destination can be swicthed)
const EdgePtr Graph::findEdge(const EdgePtr &edge) const{
VertexPtr src = edge->getDestination();
VertexPtr dest = edge->getDestination();
return findEdge(src,dest);
}
// Return the edge matching the descriptions
Edge* Vertex::getEdge(const EdgeDesc& ed)
{
EdgePtrVecIter i = findEdge(ed);
assert(i != m_edges.end());
return *i;
}
bool Vertex::hasEdge(const EdgeDesc& ed) const
{
return findEdge(ed) != m_edges.end();
}
int main( int argc, char** argv )
{
///////////
//serial stuff
int fd = 0;
char serialport[256];
int baudrate = B19200;
//int baudrate = B115200; // default
fd = serialport_init("/dev/ttyUSB0", baudrate);
if(fd==-1) return -1;
usleep(3000 * 1000 );
///////////
int c = 0, fps = 0;
//capture from camera
CvCapture *capture = cvCaptureFromCAM(1);
//quit if camera not found
if(!capture) {
printf("cannot init capture!\n");
return -1;
}
//display original video stream
cvNamedWindow("stream", CV_WINDOW_AUTOSIZE);
cvNamedWindow("hue", CV_WINDOW_NORMAL);
cvResizeWindow("hue", 320, 240);
cvMoveWindow("hue", 640, 0);
CvFont font;
cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 2, CV_AA);
//keep capturing frames until escape
while(c != 27) //27 is escape key
{
//quit if can't grab frame
if(!(frame = cvQueryFrame(capture))) break;
//show the default image
//cvShowImage("stream", frame);
//edge detection - todo: HSV color filtering
findLine(frame);
//edge detection
findEdge(frame);
if(flag == 2)
cvPutText(frame, "right edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
else if(flag == 1)
cvPutText(frame, "left edge", cvPoint(30, 400), &font, cvScalar(255, 0, 0, 0));
//display center of gravity in coordinates
COG(&cog_x, &cog_y);
char x_coord[10];
char y_coord[10];
sprintf(x_coord, "%1.2f", cog_x);
sprintf(y_coord, "%1.2f", cog_y);
//find center of y coordinate in left and right edge
COG_edges(&left_y, &right_y);
printf("%1.2f\n", left_y);
printf("%1.2f\n", right_y);
uint8_t b = 0b00000000;
//motor logic
char motor1[10];
char motor2[10];
if(flag == 1)
{
b = 0b00010001;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "backward");
}
else if(flag == 2)
{
b = 0b10011001;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "forward");
}
else if((int)(left_y/10.0) - (int)(right_y/10.0) < -4) //rotate right
{
b = 0b10010001;
sprintf(motor1, "%s", "forward");
sprintf(motor2, "%s", "backward");
}
else if((int)(right_y/10.0) - (int)(left_y/10.0) < -4) //rotate left
{
b = 0b00011001;
sprintf(motor1, "%s", "backward");
sprintf(motor2, "%s", "forward");
}
else
{
b = 0;
sprintf(motor1, "%s", "STOP");
sprintf(motor2, "%s", "STOP");
}
//SERIAL - motor logic
//xxxx_xxxx = motor1_motor2, 1-15 -> -7,7 -> 8 = 0 = 1000
//edges
write(fd,&b,1);
//cvPutText(frame, x_coord, cvPoint(30,300), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "y:", cvPoint(0,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, y_coord, cvPoint(30,350), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor1:", cvPoint(0,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor1, cvPoint(150,150), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, "motor2:", cvPoint(0,200), &font, cvScalar(255, 0, 0, 0));
cvPutText(frame, motor2, cvPoint(150,200), &font, cvScalar(255, 0, 0, 0));
cvShowImage("stream", frame);
c = cvWaitKey(10);
}
cvReleaseCapture(&capture);
//avoid memory leaks
cvReleaseImage(&image);
cvReleaseImage(&red);
cvReleaseImage(&green);
cvReleaseImage(&red_edge);
cvReleaseImage(&green_edge);
cvReleaseImage(&edge);
cvReleaseImage(&final);
cvReleaseImage(&frame);
cvReleaseImage(&bw);
cvReleaseImage(&gray);
return 0;
}
// 在 Region 中 找边缘像素点
int arrFindEdgesInRegion(ARREdgeDetector *detecotr,
const int left, const int top, const int width, const int height,
ARREdge **edges, int *num) // output edges, change num
{
edges_num = 0;
int prev1, prev2; // 像素差，维护两个历史 ???
int x, y;
#if DEBUG_ENABLE
// debug functions: 画 此区块(Region) 的 SectorGrids
if (detecotr->drawSectorGrids) {
//画横线
for (y = top; y < top + height; y += ARR_RASTER_SIZE) {
//drawLine
}
//画纵线
for (x = left; x < left + width; x += ARR_RASTER_SIZE) {
//drawLine
}
}
// debug functions: 画 此区块(Region) 的 Sectors（4条边界线）
if (detecotr->drawSectors) {
//drawLine
}
#endif
for (y = 0; y < height; y += ARR_RASTER_SIZE) { // 遍历横向扫描线
ARRByte *offset = detecotr->image->data + (left + (y+top) * detecotr->image->width) * 3; //指向扫描线左端点byte
prev1 = prev2 = 0;
const int pitch = 3; //大概就是间隔的意思，每个横向扫描线，3个channel为一个pixel
for (x = 0; x < width; x++, offset += pitch) {
findEdge(detecotr, x, y, left, top, &prev1, &prev2, offset, pitch, TRUE);
}
}
#if DEBUG_ENABLE
// debug function: 画横边缘像素点
if( detecotr->drawEdges ) {
for( i=0; i<edges_num; i++ ) {
//drawPoint
}
}
int debugNumOfHorizontalEdges = edges_num;
#endif
for( x=0; x<width; x+=ARR_RASTER_SIZE ) {
ARRByte *offset = detecotr->image->data + (left + x + top * detecotr->image->width ) * 3;
const int pitch = 3 * detecotr->image->width;
prev1 = prev2 = 0;
for( int y=0; y<height; y++, offset += pitch) {
findEdge(detecotr, x, y, left, top, &prev1, &prev2, offset, pitch, FALSE);
}
}
#if DEBUG_ENABLE
// debug function: 画纵边缘像素点
if( detecotr->drawEdges ) {
for( i=debugNumOfHorizontalEdges; i<edges_num; i++ ) {
//drawPoint
}
}
#endif
*edges = edges_static;
*num = edges_num;
return 0;
}
