Node *readNewickNode(FILE *infile, Tree *tree, Node *parent, int &depth) { char chr, char1; Node *node; string token; // read first character if (!(char1 = readChar(infile, depth))) { printError("unexpected end of file"); return NULL; } if (char1 == '(') { // read internal node int depth2 = depth; node = tree->addNode(new Node()); if (parent) parent->addChild(node); // read all child nodes at this depth while (depth == depth2) { Node *child = readNewickNode(infile, tree, node, depth); if (!child) return NULL; } // read distance for this node char chr = readUntil(infile, token, "):,;", depth); if (chr == ':') { node->dist = readDist(infile, depth); if (!(chr = readUntil(infile, token, "):,", depth))) return NULL; } else { // node name, if it does not start with a number or ".","-" if (!isdigit(chr) && chr != '.' && chr != '-') { node->longname = char1 + trim(token.c_str()); } } return node; } else { // read leaf node = tree->addNode(new Node()); if (parent) parent->addChild(node); // read name if (!(chr = readUntil(infile, token, ":),", depth))) return NULL; node->longname = char1 + trim(token.c_str()); // read distance for this node if (chr == ':') { node->dist = readDist(infile, depth); if (!(chr = readUntil(infile, token, ":),", depth))) return NULL; } return node; } }
void startSlam(int v) { FileStorage fs("config.yml", FileStorage::READ); Mat cam, dist, rpos, q, s; fs["cameraMatrix"] >> cam; fs["distMatrix"] >> dist; fs["sensorPos"] >> rpos; fs["q"] >> q; fs["s"] >> s; fs.release(); ml = new MarkerLocator(cam, dist, rpos, 100.); // Mat q = (Mat_<double>(2,1) << .01, .1*M_PI/180); // Mat s = (Mat_<double>(2,1) << 10, .5*M_PI/180); slam = new EkfSlam(&scan, q, s); #ifdef GUI namedWindow("EKFSlam"); disp = new MapDisplay("Map"); waitKey(); #endif Mat u = (Mat_<double>(2,1) << 0, 0); int fd = openPort(); oiStart(); setSafeMode(); readDist(); readAngle(); VideoCapture cap(0); if(!cap.isOpened()) { printf("Failed capture\n"); return; } VideoWriter record("/tmp/demo.avi", CV_FOURCC('D','I','V','X'), 30, Size(1340,600), true); if(!record.isOpened()) { printf("Failed writer\n"); return; } clock_t t; time_t stime; while(1) { t = clock(); setDrive(v,v); double mindist = 99999; while(msElap(t) < 100 && !hitWall()) { u.at<double>(0) = (double)readDist(); u.at<double>(1) = M_PI*(double)readAngle()/180; mindist = slamStep(u, cap, record); } // backup before turn when running into something if(hitWall() || mindist < 700) { if(hitWall()) { // backup setDrive(-15,-15); t = clock(); while(msElap(t) < 500) { u.at<double>(0) = (double)readDist(); u.at<double>(1) = M_PI*(double)readAngle()/180; slamStep(u, cap, record); } } // turn if(time(&stime)%60 < 30) { setDrive(-5, 5); } else setDrive(5,-5); t = clock(); while(msElap(t) < 250) { u.at<double>(0) = 0; u.at<double>(1) = M_PI*(double)readAngle()/180; slamStep(u, cap, record); } } if(waitKey(10) == 27) break; } setDrive(0,0); imwrite("lastFrame.png", g_outFrame); close(fd); delete ml; delete slam; #ifdef GUI delete disp; #endif }