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; }