cv::Mat GraphComparator::_calculateCostDistance(shared_ptr<OsmMap> map, Coordinate c) { // make a copy of the map so we can manipulate it. map.reset(new OsmMap(map)); // find the nearest feature long wId = map->getIndex().findNearestWay(c); shared_ptr<Way> w = map->getWay(wId); // split way at c WayLocation wl = LocationOfPoint::locate(map, w, c); vector< shared_ptr<Way> > v = WaySplitter::split(map, w, wl); wl = LocationOfPoint::locate(map, v[0], c); if (wl.isNode() == false) { // I haven't been able to recreate the case when this happens. LOG_WARN("Internal Error: Expected wl to be on a node, but it was this: " << wl); } assert(wl.isNode() == true); // populate graph shared_ptr<DirectedGraph> graph(new DirectedGraph()); graph->deriveEdges(map); ShortestPath sp(graph); // set cost at c to zero. long sourceId = v[0]->getNodeId(wl.getSegmentIndex()); sp.setNodeCost(sourceId, 0.0); // calculate cost sp.calculateCost(); // paint graph onto raster //_exportGraphImage(map, *graph, sp, "/home/jason.surratt/tmp/graph.png"); cv::Mat mat = _paintGraph(map, *graph, sp); // calculate cost distance raster _calculateRasterCost(mat); return mat; }
void GraphComparator::drawCostDistance(shared_ptr<OsmMap> map, vector<Coordinate>& c, QString output) { _updateBounds(); // make a copy of the map so we can manipulate it. map.reset(new OsmMap(map)); for (size_t i = 0; i < c.size(); i++) { cout << c[i].x << " " << c[i].y << endl; c[i] = _findNearestPointOnFeature(map, c[i]); cout << c[i].x << " " << c[i].y << endl; // find the nearest feature long wId = map->getIndex().findNearestWay(c[i]); shared_ptr<Way> w = map->getWay(wId); // split way at c WayLocation wl = LocationOfPoint::locate(map, w, c[i]); vector< shared_ptr<Way> > v = WaySplitter::split(map, w, wl); wl = LocationOfPoint::locate(map, v[0], c[i]); assert(wl.isNode() == true); } // populate graph shared_ptr<DirectedGraph> graph(new DirectedGraph()); graph->deriveEdges(map); LOG_WARN("Running cost"); ShortestPath sp(graph); for (size_t i = 0; i < c.size(); i++) { long wId = map->getIndex().findNearestWay(c[i]); shared_ptr<Way> w = map->getWay(wId); WayLocation wl = LocationOfPoint::locate(map, w, c[i]); // set cost at c to zero. long sourceId = w->getNodeId(wl.getSegmentIndex()); sp.setNodeCost(sourceId, 0.0); } // calculate cost sp.calculateCost(); LOG_WARN("Cost done"); cv::Mat mat = _paintGraph(map, *graph, sp); _saveImage(mat, output, -1.0, false); _saveImage(mat, output.replace(".png", "2.png"), -1.0, true); shared_ptr<OGRSpatialReference> srs(new OGRSpatialReference()); srs->importFromEPSG(900913); Coordinate c1 = MapProjector::project(Coordinate(_projectedBounds.MinX, _projectedBounds.MinY), map->getProjection(), srs); cout << "coord " << c1.x << ", " << c1.y << endl; Coordinate c2 = MapProjector::project(Coordinate(_projectedBounds.MaxX, _projectedBounds.MaxY), map->getProjection(), srs); cout << "coord2 " << c2.x << ", " << c2.y << endl; printf("POSITION_Y=%f\n", (c1.y + c2.y) / 2.0); //cout << "POSITION_Y=" << (c1.y + c2.y) / 2.0 << endl; printf("POSITION_X=%f\n", (c1.x + c2.x) / 2.0); //cout << "POSITION_X=" << (c1.x + c2.x) / 2.0 << endl; cout << "M12=0.0" << endl; cout << "M11=1.0" << endl; cout << "M10=0.0" << endl; cout << "M02=0.0" << endl; cout << "INITIAL_SCALE=" << (c2.x - c1.x) / (double)_width * 100.0 << endl; cout << "M01=0.0" << endl; cout << "M00=1.0" << endl; // paint graph onto raster //_exportGraphImage(map, *graph, sp, output); }