├── .gitignore ├── CSG.py ├── CSG_ego.py ├── GenerateCurRoad.py ├── LICENSE ├── Lib_OpenSCENARIO ├── ScenarioGeneration.py ├── ScenarioGeneration_actions.py ├── ScenarioGeneration_basic.py ├── ScenarioGeneration_multi_routing.py ├── pyoscx │ ├── __init__.py │ ├── actions.py │ ├── entities.py │ ├── enumerations.py │ ├── helpers.py │ ├── position.py │ ├── scenario.py │ ├── storyboard.py │ ├── triggers.py │ └── utils.py └── route_in_crossing.py ├── README.md ├── ScaleSolver.py ├── config.py ├── demo.mp4 ├── environment.yml ├── icon ├── clipper.jpg ├── icon.gif ├── intersection.jpg ├── pass.png └── warning.png ├── requirements.txt └── src ├── SLAM └── mono_kitti ├── __init__.py ├── calibration ├── CamCal_config.py ├── CamParam.py ├── CameraCal.py ├── ParamRng.py ├── VPLine.py ├── __init__.py ├── calib_main.py └── cameracalib.py ├── convert_util.py ├── depth ├── __init__.py ├── monodepth.py └── net.py ├── detect ├── VehicleLampDet.py ├── __init__.py ├── detect_factory.py ├── mask_rcnn │ ├── __init__.py │ ├── coco.py │ ├── config.py │ ├── config_v2.py │ ├── model.py │ ├── model_v2.py │ ├── utils.py │ ├── utils_v2.py │ └── visualize.py └── yolo4 │ ├── __init__.py │ ├── model.py │ ├── utils.py │ ├── yolo.py │ └── yolo_anchors.txt ├── error_check.py ├── file_parser.py ├── road ├── GenerateCurRoad.py ├── GenerateIntersection_v03.py ├── PINet │ ├── __init__.py │ ├── agent.py │ ├── data_loader.py │ ├── evaluation.py │ ├── fix_dataset.py │ ├── hard_sampling.py │ ├── hourglass_network.py │ ├── parameters.py │ ├── train.py │ ├── util.py │ └── util_hourglass.py ├── __init__.py ├── lane.py ├── retinex.py └── road_main.py ├── scale ├── CamCal_config.py ├── CamParam.py ├── CameraCal.py ├── ParamRng.py ├── PoseData.py ├── auto_scale_solver.py ├── cameracalib.py └── utils.py ├── track ├── JRETrack │ ├── __init__.py │ ├── basetrack.py │ ├── feature_extractor.py │ ├── kalman_filter.py │ ├── log.py │ ├── matching.py │ └── multitracker.py ├── ObjectTracking.py ├── __init__.py ├── kalman_filter.py ├── kcf │ ├── Makefile │ ├── __init__.py │ ├── fhog.py │ ├── fhog_util.pyx │ ├── kcftracker.py │ ├── run.py │ └── setup.py ├── simple_tracker.py ├── track_factory.py └── trajectory.py ├── ui ├── __init__.py ├── canvas.py ├── color.py ├── dialog.py ├── drag_line.py ├── drag_rect.py ├── image_utils.py ├── lane_edit_frame.py ├── lane_frame.py └── video_clipper.py └── xml_generation.py /.gitignore: -------------------------------------------------------------------------------- 1 | data/ 2 | *.c 3 | # Byte-compiled / optimized / DLL files 4 | __pycache__/ 5 | *.py[cod] 6 | *$py.class 7 | 8 | # C extensions 9 | *.so 10 | 11 | # Distribution / packaging 12 | .Python 13 | build/ 14 | develop-eggs/ 15 | dist/ 16 | downloads/ 17 | eggs/ 18 | .eggs/ 19 | lib/ 20 | lib64/ 21 | parts/ 22 | sdist/ 23 | var/ 24 | wheels/ 25 | pip-wheel-metadata/ 26 | share/python-wheels/ 27 | *.egg-info/ 28 | .installed.cfg 29 | *.egg 30 | MANIFEST 31 | 32 | # PyInstaller 33 | # Usually these files are written by a python script from a template 34 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 35 | *.manifest 36 | *.spec 37 | 38 | # Installer logs 39 | pip-log.txt 40 | pip-delete-this-directory.txt 41 | 42 | # Unit test / coverage reports 43 | htmlcov/ 44 | .tox/ 45 | .nox/ 46 | .coverage 47 | .coverage.* 48 | .cache 49 | nosetests.xml 50 | coverage.xml 51 | *.cover 52 | *.py,cover 53 | .hypothesis/ 54 | .pytest_cache/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | local_settings.py 63 | db.sqlite3 64 | db.sqlite3-journal 65 | 66 | # Flask stuff: 67 | instance/ 68 | .webassets-cache 69 | 70 | # Scrapy stuff: 71 | .scrapy 72 | 73 | # Sphinx documentation 74 | docs/_build/ 75 | 76 | # PyBuilder 77 | target/ 78 | 79 | # Jupyter Notebook 80 | .ipynb_checkpoints 81 | 82 | # IPython 83 | profile_default/ 84 | ipython_config.py 85 | 86 | # pyenv 87 | .python-version 88 | 89 | # pipenv 90 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 91 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 92 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 93 | # install all needed dependencies. 94 | #Pipfile.lock 95 | 96 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 97 | __pypackages__/ 98 | 99 | # Celery stuff 100 | celerybeat-schedule 101 | celerybeat.pid 102 | 103 | # SageMath parsed files 104 | *.sage.py 105 | 106 | # Environments 107 | .env 108 | .venv 109 | env/ 110 | venv/ 111 | ENV/ 112 | env.bak/ 113 | venv.bak/ 114 | 115 | # Spyder project settings 116 | .spyderproject 117 | .spyproject 118 | 119 | # Rope project settings 120 | .ropeproject 121 | 122 | # mkdocs documentation 123 | /site 124 | 125 | # mypy 126 | .mypy_cache/ 127 | .dmypy.json 128 | dmypy.json 129 | 130 | # Pyre type checker 131 | .pyre/ 132 | -------------------------------------------------------------------------------- /Lib_OpenSCENARIO/pyoscx/__init__.py: -------------------------------------------------------------------------------- 1 | # __init__.py 2 | 3 | from .utils import * 4 | from .actions import * 5 | from .helpers import * 6 | from .position import * 7 | from .triggers import * 8 | from .scenario import * 9 | from .storyboard import * 10 | from .entities import * 11 | from .enumerations import * -------------------------------------------------------------------------------- /Lib_OpenSCENARIO/pyoscx/enumerations.py: -------------------------------------------------------------------------------- 1 | XMLNS = 'http://www.w3.org/2001/XMLSchema-instance' 2 | XSI = 'OpenSccenario.xsd' 3 | 4 | from enum import Enum, auto 5 | 6 | class CloudState(Enum): 7 | """ Enum for CloudState 8 | """ 9 | skyOff = auto() 10 | free = auto() 11 | cloudy = auto() 12 | overcast = auto() 13 | rainy = auto() 14 | 15 | class ConditionEdge(Enum): 16 | """ Enum for ConditionEdge 17 | """ 18 | rising = auto() 19 | falling = auto() 20 | risingOrFalling = auto() 21 | none = auto() 22 | 23 | class DynamicsDimension(Enum): 24 | """ Enum for DynamicsDimension 25 | """ 26 | rate = auto() 27 | time = auto() 28 | distance = auto() 29 | 30 | class DynamicsShapes(Enum): 31 | """ Enum for DynamicsShapes 32 | """ 33 | linear = auto() 34 | cubic = auto() 35 | sinusoidal = auto() 36 | step = auto() 37 | 38 | class FollowMode(Enum): 39 | """ Enum for FollowMode 40 | """ 41 | position = auto() 42 | follow = auto() 43 | 44 | class MiscObjectCategory(Enum): 45 | """ Enum for MiscObjectCategory 46 | """ 47 | 48 | none = auto() 49 | obstacle = auto() 50 | pole = auto() 51 | tree = auto() 52 | vegetation = auto() 53 | barrier = auto() 54 | building = auto() 55 | parkingSpace = auto() 56 | patch = auto() 57 | railing = auto() 58 | grafficIsland = auto() 59 | crosswalk = auto() 60 | streetLamp = auto() 61 | gantry = auto() 62 | soundBarrier = auto() 63 | wind = auto() 64 | roadMark = auto() 65 | 66 | class ObjectType(Enum): 67 | """ Enum for ObjectType 68 | """ 69 | pedestrian = auto() 70 | vehicle = auto() 71 | miscellaneous = auto() 72 | 73 | class ParameterType(Enum): 74 | """ Enum for ParameterType 75 | """ 76 | integer = auto() 77 | double = auto() 78 | string = auto() 79 | unsighedInt = auto() 80 | unsighedShort = auto() 81 | boolean = auto() 82 | dateTime = auto() 83 | 84 | class PedestrianCategory(Enum): 85 | """ Enum for PedestrianCategory 86 | """ 87 | pedestrian = auto() 88 | wheelchair = auto() 89 | animal = auto() 90 | 91 | class PrecipitationType(Enum): 92 | """ Enum for PercipitationType 93 | """ 94 | dry = auto() 95 | rain = auto() 96 | snow = auto() 97 | 98 | class Priority(Enum): 99 | """ Enum for Priority 100 | """ 101 | overwrite = auto() 102 | skip = auto() 103 | parallel = auto() 104 | 105 | class ReferenceContext(Enum): 106 | """ Enum for ReferenceContext 107 | """ 108 | relative = auto() 109 | absolute = auto() 110 | 111 | class RelativeDistanceType(Enum): 112 | """ Enum for RelativeDistanceType 113 | """ 114 | longitudinal = auto() 115 | lateral = auto() 116 | cartesianDistance = auto() 117 | 118 | class RouteStrategy(Enum): 119 | """ Enum for RouteStrategy 120 | """ 121 | fastest = auto() 122 | shortest = auto() 123 | leastIntersections = auto() 124 | random = auto() 125 | 126 | class Rule(Enum): 127 | """ Enum for Rule 128 | """ 129 | greaterThan = auto() 130 | lessThan = auto() 131 | equalTo = auto() 132 | 133 | class SpeedTargetValueType(Enum): 134 | """ Enum for SpeedTargetValueType 135 | """ 136 | delta = auto() 137 | factor = auto() 138 | 139 | class StoryboardElementState(Enum): 140 | """ Enum for StoryboardElementState 141 | """ 142 | startTransition = auto() 143 | endTransition = auto() 144 | stopTransition = auto() 145 | skipTransition = auto() 146 | completeState = auto() 147 | runningState = auto() 148 | standbyState = auto() 149 | 150 | class StoryboardElementType(Enum): 151 | """ Enum for StoryboardElementType 152 | """ 153 | story = auto() 154 | act = auto() 155 | maneuver = auto() 156 | event = auto() 157 | action = auto() 158 | maneuverGroup = auto() 159 | 160 | class TriggeringEntitiesRule(Enum): 161 | """ Enum for TriggeringEntitiesRule 162 | """ 163 | any = auto() 164 | all = auto() 165 | 166 | class VehicleCategory(Enum): 167 | """ Enum for VehicleCategory 168 | """ 169 | car = auto() 170 | van = auto() 171 | truck = auto() 172 | trailer = auto() 173 | semitrailer = auto() 174 | bus = auto() 175 | motorbike = auto() 176 | bicycle = auto() 177 | train = auto() 178 | tram = auto() 179 | 180 | -------------------------------------------------------------------------------- /Lib_OpenSCENARIO/pyoscx/helpers.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as ET 2 | import xml.dom.minidom as mini 3 | import os 4 | 5 | 6 | def esminiRunner(scenario, esminipath='esmini', args='--window 50 50 800 400'): 7 | """ write a scenario and runs it in esmini 8 | Parameters 9 | ---------- 10 | scenario (Scenario): the pypscx scenario to run 11 | 12 | esminipath (str): the path to esmini 13 | Default: pyoscx 14 | 15 | args: pass through esmini launch commands 16 | 17 | """ 18 | _scenariopath = os.path.join(esminipath, 'resources', 'xosc') 19 | scenario.write_xml(os.path.join(_scenariopath, 'pythonscenario2.xosc'), True) 20 | 21 | if os.name == 'posix': 22 | os.system(os.path.join('.', 23 | esminipath, 24 | 'bin', 25 | 'esmini') + ' --osc ' + esminipath + '/resources/xosc/pythonscenario2.xosc ' + args) 26 | elif os.name == 'nt': 27 | os.system(os.path.join(esminipath, 28 | 'bin', 29 | 'esmini.exe') + ' --osc ' + esminipath + '/resources/xosc/pythonscenario2.xosc ' + args) 30 | 31 | def esminiRunViewer(scenario,esminipath='esmini',args=' --window 100 100 1000 800'): 32 | """ writes and runs esmini in headless mode then launch the replayer for analysis of the scenario 33 | Parameters 34 | ---------- 35 | scenario (Scenario): the pypscx scenario to run 36 | 37 | esminipath (str): the path to esmini 38 | Default: esmini 39 | 40 | args: pass through replayer commands 41 | """ 42 | _scenariopath = os.path.join(esminipath, 'resources', 'xosc') 43 | scenario.write_xml(os.path.join(_scenariopath, 'pythonscenario2.xosc'), True) 44 | 45 | if os.name == 'posix': 46 | os.system(os.path.join('.', 47 | esminipath, 48 | 'bin', 49 | 'esmini') + ' --osc ' + esminipath + '/resources/xosc/pythonscenario2.xosc --record python_replay --headless') 50 | 51 | os.system(os.path.join('.', 52 | esminipath, 53 | 'bin', 54 | 'replayer') + ' --file python_replay --res_path ' + os.path.join(esminipath,'resources') + args) 55 | 56 | 57 | elif os.name == 'nt': 58 | os.system(os.path.join(esminipath, 59 | 'bin', 60 | 'esmini.exe') + ' --osc ' + esminipath + '/resources/xosc/pythonscenario2.xosc --record python_replay --headless' + args) 61 | 62 | 63 | 64 | def prettyprint(element): 65 | """ prints the element to the commandline 66 | 67 | Parameters 68 | ---------- 69 | element (Element): element to print 70 | 71 | """ 72 | rough = ET.tostring(element, 'utf-8') 73 | reparsed = mini.parseString(rough) 74 | print(reparsed.toprettyxml(indent="\t")) 75 | 76 | 77 | def printToFile(element, filename, prettyprint=True): 78 | """ prints the element to a xml file 79 | 80 | Parameters 81 | ---------- 82 | element (Element): element to print 83 | 84 | filename (str): file to save to 85 | 86 | prettyprint (bool): pretty or "ugly" print 87 | 88 | """ 89 | if prettyprint: 90 | rough = ET.tostring(element, 'utf-8').replace(b'\n', b'').replace(b'\t', b'') 91 | reparsed = mini.parseString(rough) 92 | towrite = reparsed.toprettyxml(indent="\t") 93 | with open(filename, "w") as file_handle: 94 | file_handle.write(towrite) 95 | else: 96 | tree = ET.ElementTree(element) 97 | with open(filename, "wb") as file_handle: 98 | tree.write(file_handle) 99 | -------------------------------------------------------------------------------- /Lib_OpenSCENARIO/pyoscx/scenario.py: -------------------------------------------------------------------------------- 1 | import xml.etree.ElementTree as ET 2 | import xml.dom.minidom as mini 3 | 4 | 5 | from .helpers import printToFile 6 | from .utils import FileHeader, ParameterDeclarations, Catalog, TrafficSignalController 7 | from .enumerations import XMLNS, XSI 8 | from .entities import Entities 9 | from .storyboard import StoryBoard 10 | 11 | class Scenario(): 12 | """ The Scenario class collects all parts of OpenScenario and creates a .xml file 13 | 14 | Attributes 15 | ---------- 16 | header (FileHeader): the header of the scenario file 17 | 18 | parameters (ParameterDeclarations): the parameters to be used in the scenario 19 | 20 | entities (Entities): the entities in the scenario 21 | 22 | storyboard (StoryBoard): the storyboard of the scenario 23 | 24 | roadnetwork (RoadNetwork): the roadnetwork of the scenario 25 | 26 | catalog (Catalog): the catalogs used in the scenario 27 | Methods 28 | ------- 29 | get_element() 30 | Returns the full ElementTree of the class 31 | 32 | write_xml(filename) 33 | write a open scenario xml 34 | 35 | """ 36 | _XMLNS = XMLNS 37 | _XSI = XSI 38 | def __init__(self,name,author,parameters,entities,storyboard,roadnetwork,catalog): 39 | """ Initalizes the Scenario class, and creates the header. 40 | 41 | Parameters 42 | ---------- 43 | name (str): name of the scenario 44 | 45 | author (str): the author fo the scenario 46 | 47 | parameters (ParameterDeclarations): the parameters to be used in the scenario 48 | 49 | entities (Entities): the entities in the scenario 50 | 51 | storyboard (StoryBoard): the storyboard of the scenario 52 | 53 | roadnetwork (RoadNetwork): the roadnetwork of the scenario 54 | 55 | catalog (Catalog): the catalogs used in the scenario 56 | """ 57 | if not isinstance(entities,Entities): 58 | raise TypeError('entities input is not of type Entities') 59 | if not isinstance(storyboard,StoryBoard): 60 | raise TypeError('storyboard input is not of type StoryBoard') 61 | if not isinstance(roadnetwork,RoadNetwork): 62 | raise TypeError('roadnetwork input is not of type RoadNetwork') 63 | if not isinstance(catalog,Catalog): 64 | raise TypeError('catalog input is not of type StorCatalogyBoard') 65 | if not isinstance(parameters,ParameterDeclarations): 66 | raise TypeError('parameters input is not of type ParameterDeclarations') 67 | 68 | self.entities = entities 69 | self.storyboard = storyboard 70 | self.roadnetwork = roadnetwork 71 | self.catalog = catalog 72 | self.parameters = parameters 73 | self.header = FileHeader(name,author) 74 | 75 | def get_element(self): 76 | """ returns the elementTree of the Scenario 77 | 78 | """ 79 | #xinxin element = ET.Element('OpenSCENARIO',attrib={'xmlns:xsi':self._XMLNS,'xsi:noNamespaceSchemaLocation':self._XSI}) 80 | element = ET.Element('OpenSCENARIO') 81 | element.append(self.header.get_element()) 82 | element.append(self.parameters.get_element()) 83 | element.append(self.catalog.get_element()) 84 | element.append(self.roadnetwork.get_element()) 85 | element.append(self.entities.get_element()) 86 | element.append(self.storyboard.get_element()) 87 | 88 | return element 89 | 90 | def write_xml(self,filename,prettyprint = True): 91 | """ writeXml writes the open scenario xml file 92 | 93 | Parameters 94 | ---------- 95 | filename (str): path and filename of the wanted xml file 96 | 97 | prettyprint (bool): pretty print or ugly print? 98 | Default: True 99 | 100 | """ 101 | printToFile(self.get_element(),filename,prettyprint) 102 | 103 | 104 | 105 | 106 | class RoadNetwork(): 107 | """ The RoadNetwork class creates the RoadNetwork of the openScenario 108 | 109 | Parameters 110 | ---------- 111 | roadfile (str): path to the opendrive file 112 | 113 | scenegraph (str): path to the opensceengraph file (optional) 114 | 115 | Attributes 116 | ---------- 117 | road_file (str): path to the opendrive file 118 | 119 | scene (str): path to the opensceengraph file 120 | 121 | traffic_signals (list of TrafficSignalController): all traffic signals in the roadnetwork 122 | 123 | Methods 124 | ------- 125 | get_element() 126 | Returns the full ElementTree of the class 127 | 128 | 129 | 130 | """ 131 | def __init__(self,roadfile,scenegraph=None): 132 | """ Initalizes the RoadNetwork 133 | 134 | Parameters 135 | ---------- 136 | roadfile (str): path to the opendrive file 137 | 138 | scenegraph (str): path to the opensceengraph file (optional) 139 | 140 | """ 141 | self.road_file = roadfile 142 | self.scene = scenegraph 143 | self.traffic_signals = [] 144 | 145 | def add_traffic_signal_controller(self,traffic_signal_controller): 146 | """ adds a TrafficSignalController to the RoadNetwork 147 | 148 | Parameters 149 | ---------- 150 | traffic_signal_controller (TrafficSignalController): the traffic signal controller to add 151 | 152 | """ 153 | if not isinstance(traffic_signal_controller,TrafficSignalController): 154 | raise TypeError('traffic_signal_controller input is not of type TrafficSignalController') 155 | self.traffic_signals.append(traffic_signal_controller) 156 | 157 | def get_element(self): 158 | """ returns the elementTree of the RoadNetwork 159 | 160 | """ 161 | roadnetwork = ET.Element('RoadNetwork') 162 | ET.SubElement(roadnetwork,'LogicFile',{'filepath': self.road_file}) 163 | if self.scene: 164 | ET.SubElement(roadnetwork,'SceneGraphFile',{'filepath':self.scene}) 165 | if self.traffic_signals: 166 | trafsign_element = ET.SubElement(roadnetwork,'TrafficSignals') 167 | for ts in self.traffic_signals: 168 | trafsign_element.append(ts.get_element()) 169 | return roadnetwork 170 | 171 | -------------------------------------------------------------------------------- /Lib_OpenSCENARIO/route_in_crossing.py: -------------------------------------------------------------------------------- 1 | """ Notes: 2 | Simple example showing how to pick a route in a junction 3 | 4 | Some features used: 5 | Route 6 | AssingRouteAction 7 | LanePosition 8 | """ 9 | 10 | import pyoscx 11 | 12 | ### create catalogs 13 | catalog = pyoscx.Catalog() 14 | catalog.add_catalog('VehicleCatalog','../xosc/Catalogs/Vehicles') 15 | 16 | 17 | ### create road 18 | road = pyoscx.RoadNetwork(roadfile="../xodr/fabriksgatan.xodr",scenegraph="../models/fabriksgatan.osgb") 19 | 20 | ### create parameters 21 | paramdec = pyoscx.ParameterDeclarations() 22 | 23 | ## create entities 24 | 25 | egoname = 'Ego' 26 | targetname = 'Target' 27 | 28 | entities = pyoscx.Entities() 29 | entities.add_scenario_object(egoname,pyoscx.CatalogReference('VehicleCatalog','car_red')) 30 | 31 | ### create init 32 | 33 | init = pyoscx.Init() 34 | 35 | init.add_init_action(egoname,pyoscx.TeleportAction(pyoscx.LanePosition(50,0,1,0))) 36 | init.add_init_action(egoname,pyoscx.AbsoluteSpeedAction(10,pyoscx.TransitionDynamics(pyoscx.DynamicsShapes.step,pyoscx.DynamicsDimension.time,1))) 37 | 38 | # create a router 39 | 40 | ego_route = pyoscx.Route('ego_route') 41 | ego_route.add_waypoint(pyoscx.LanePosition(30,0,1,0),pyoscx.RouteStrategy.fastest) 42 | ego_route.add_waypoint(pyoscx.LanePosition(10,0,-1,1),pyoscx.RouteStrategy.fastest) 43 | 44 | 45 | # create action 46 | ego_action = pyoscx.AssingRouteAction(ego_route) 47 | 48 | 49 | ego_event = pyoscx.Event('ego_event',pyoscx.Priority.overwrite) 50 | ego_event.add_action('ego_route',ego_action) 51 | ego_event.add_trigger(pyoscx.ValueTrigger('target_start',0,pyoscx.ConditionEdge.none,pyoscx.SimulationTimeCondition(1,pyoscx.Rule.greaterThan))) 52 | 53 | 54 | ## create the storyboard 55 | ego_man = pyoscx.Maneuver('ego_man') 56 | ego_man.add_event(ego_event) 57 | 58 | sb = pyoscx.StoryBoard(init,pyoscx.ValueTrigger('stop_simulation',0,pyoscx.ConditionEdge.rising,pyoscx.SimulationTimeCondition(10,pyoscx.Rule.greaterThan),'stop')) 59 | sb.add_maneuver(ego_man,egoname) 60 | 61 | ## create the scenario 62 | sce = pyoscx.Scenario('adaptspeed_example','User',paramdec,entities=entities,storyboard = sb,roadnetwork=road,catalog=catalog) 63 | 64 | # display the scenario 65 | pyoscx.prettyprint(sce.get_element()) 66 | 67 | # pyoscx.esminiRunner(sce,esminipath='/home/mander76/local/scenario_creation/esmini') -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Critical Scenario Generation Toolkit 2 | 3 | ## Introduction 4 | This repo offers the source code for Critical Scnenario Generation (CSG) toolkit. The toolkit aims to extract dynamic (trajectories) and static (road) elements from a given surveillance or dash camera video, and fromulated as OpenDrive and OpenScenario files as outputs. 5 | ## How to build 6 | 7 | 1. Install system dependencies: 8 | ```Shell 9 | sudo apt-get install python3-tk 10 | ``` 11 | 2. Install python3 package: 12 | ```Shell 13 | pip3 install Cython numpy 14 | pip3 install -r requirements.txt 15 | ``` 16 | 3. compile cython package 17 | ```Shell 18 | cd src/track/kcf && make & cd ../../../ 19 | ``` 20 | 21 | ## How to run: 22 | 1. Edit variables in config.py (Optional) 23 | 2. Download machine learning models from [Google Drive](https://drive.google.com/file/d/1QAyzSvLjLBETgSk5w7hOp-1WGwD9aH9I/view?usp=sharing ) and unzip to "data" folder. 24 | 3. Run 25 | 26 | - For survelliance video: 27 | ```Shell 28 | python3 CSG.py 29 | ``` 30 | 31 | - For dash camera video: 32 | 33 | It uses [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2 "ORB-SLAM2") to get extrinsic parameters of moving camera. Please install ORB-SLAM2 first and put the executable file under the "/src/SLAM" folder. 34 | 35 | Then run: 36 | ```Shell 37 | python3 CSG_ego.py 38 | ``` 39 | 40 | ## Reqruiments 41 | 42 | At least 1 GPU is needed. By default, the models are deployed on gpu:0. You can change your settings in "config" file. 43 | ## Folder structure 44 | ```bash 45 | +-- critical-scenario-generation 46 | ¦ +-- data 47 | ¦ ¦ +-- yolo 48 | ¦ ¦ +-- vehicle_reid 49 | ¦ ¦ +-- mask_rcnn_ego 50 | ¦ ¦ +-- mask_rcnn 51 | ¦ ¦ +-- lane 52 | ¦ ¦ +-- depth 53 | ¦ +-- src 54 | ¦ ¦ +-- SLAM 55 | ¦ ¦ ¦ +-- mono_kitti 56 | ¦ ¦ +-- ... 57 | ¦ +-- Lib_OpenSCENARIO 58 | ¦ +-- icon 59 | ¦ +-- CSG.py 60 | ¦ +-- CSG_ego.py 61 | ¦ +-- ScaleSolver.py 62 | ¦ +-- config.py 63 | ¦ +-- requirements.txt 64 | ``` 65 | 66 | ## Licence 67 | check [LICENSE](LICENSE) 68 | 69 | ## Citation 70 | If you use our source code, please consider citing the following: 71 | ```bibtex 72 | @InProceedings{csg2020, 73 | title={CSG: critical scenario generation from real traffic accidents}, 74 | author={Zhang, Xinxin and Li, Fei and Wu, Xiangbin}, 75 | booktitle = {IV}, 76 | year={2020} 77 | } 78 | ``` 79 | -------------------------------------------------------------------------------- /config.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | ROS_PATH = '/opt/ros/kinetic/lib/python2.7/dist-packages' 4 | CURR_PATH = os.path.dirname(__file__) 5 | 6 | SOURCE = {'Auto':1, 'Track':2, 'Manual':3} 7 | 8 | DETECT_MODELS = ('Mask RCNN', "YOLO") 9 | 10 | DET_CONFIG = dict() 11 | DET_CONFIG['DO_LAMP_DETECTION'] = False 12 | 13 | DET_CONFIG['MASK_RCNN_LOG_DIR'] = os.path.join(CURR_PATH, 'data', 'mask_rcnn', 'logs') 14 | DET_CONFIG['CLASSES_PATH'] = os.path.join(CURR_PATH, 'data', 'coco_classes.txt') 15 | DET_CONFIG['MASK_RCNN_MODEL_PATH'] = os.path.join(CURR_PATH, 'data', 'mask_rcnn', 'mask_rcnn_coco.h5') 16 | DET_CONFIG['MASK_RCNN_EGO_MODEL_PATH'] = os.path.join(CURR_PATH, 'data', 'mask_rcnn_ego', 'mask_rcnn_apollo.h5') 17 | DET_CONFIG['CLASSES_PATH_EGO'] = os.path.join(CURR_PATH, 'data', 'apollo_classes.txt') 18 | 19 | DET_CONFIG['MASK_RCNN_SCORE_THRESHOLD'] = 0.7 20 | 21 | DET_CONFIG['YOLO_MODEL_PATH'] = os.path.join(CURR_PATH, 'data','yolo', 'yolo4_weights.h5') 22 | DET_CONFIG['YOLO_ANCHORS_PATH'] = os.path.join(CURR_PATH, 'data','yolo', 'yolo4_anchors.txt') 23 | DET_CONFIG['YOLO_CLASSES_PATH'] = os.path.join(CURR_PATH, 'data', 'coco_classes.txt') 24 | DET_CONFIG['YOLO_SCORE_THRESHOLD'] = 0.6 25 | 26 | DET_CONFIG['DEVICE'] = '/GPU:0'#/CPU:0' 27 | DET_CONFIG['NMS_RATIO'] = 0.8 28 | 29 | #Track 30 | TRACK_CONFIG = dict() 31 | TRACK_CONFIG['TRACK_MODE'] = 'DEEP_SORT' 32 | TRACK_CONFIG['REID_MODEL'] = os.path.join(CURR_PATH, 'data', 'vehicle_reid', 'vehicle_reid_model.pb')#used by deep_sort 33 | TRACK_CONFIG['DEVICE'] = '/CPU:0' 34 | 35 | #Lane 36 | LANE_CONFIG = dict() 37 | LANE_CONFIG['MODEL'] = os.path.join(CURR_PATH, 'data', 'lane', 'model_culane.pkl') 38 | LANE_CONFIG['DEVICE'] = 'cuda:0' 39 | 40 | #depth 41 | DEPTH_CONFIG = dict() 42 | DEPTH_CONFIG['MODEL'] = os.path.join(CURR_PATH, 'data', 'depth') 43 | DEPTH_CONFIG['DEVICE'] = 'cuda:0' 44 | -------------------------------------------------------------------------------- /demo.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/demo.mp4 -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: csg 2 | channels: 3 | - defaults 4 | dependencies: 5 | - _libgcc_mutex=0.1=main 6 | - _openmp_mutex=5.1=1_gnu 7 | - blas=1.0=mkl 8 | - ca-certificates=2023.12.12=h06a4308_0 9 | - cython=3.0.6=py38h5eee18b_0 10 | - intel-openmp=2023.1.0=hdb19cb5_46306 11 | - ld_impl_linux-64=2.38=h1181459_1 12 | - libffi=3.4.4=h6a678d5_0 13 | - libgcc-ng=11.2.0=h1234567_1 14 | - libgomp=11.2.0=h1234567_1 15 | - libstdcxx-ng=11.2.0=h1234567_1 16 | - mkl=2023.1.0=h213fc3f_46344 17 | - mkl-service=2.4.0=py38h5eee18b_1 18 | - mkl_fft=1.3.8=py38h5eee18b_0 19 | - mkl_random=1.2.4=py38hdb19cb5_0 20 | - ncurses=6.4=h6a678d5_0 21 | - openssl=3.0.12=h7f8727e_0 22 | - pip=23.3.1=py38h06a4308_0 23 | - python=3.8.18=h955ad1f_0 24 | - readline=8.2=h5eee18b_0 25 | - setuptools=68.2.2=py38h06a4308_0 26 | - sqlite=3.41.2=h5eee18b_0 27 | - tbb=2021.8.0=hdb19cb5_0 28 | - tk=8.6.12=h1ccaba5_0 29 | - wheel=0.41.2=py38h06a4308_0 30 | - xz=5.4.5=h5eee18b_0 31 | - zlib=1.2.13=h5eee18b_0 32 | - pip: 33 | - absl-py==2.0.0 34 | - aiohttp==3.9.1 35 | - aiosignal==1.3.1 36 | - antlr4-python3-runtime==4.9.3 37 | - asttokens==2.4.1 38 | - astunparse==1.6.3 39 | - async-timeout==4.0.3 40 | - attrs==23.2.0 41 | - backcall==0.2.0 42 | - cachetools==5.3.2 43 | - certifi==2023.11.17 44 | - charset-normalizer==3.3.2 45 | - commonroad-io==2022.1 46 | - commonroad-vehicle-models==3.0.2 47 | - contourpy==1.1.1 48 | - cycler==0.12.1 49 | - cython-bbox==0.1.3 50 | - decorator==5.1.1 51 | - easydict==1.10 52 | - executing==2.0.1 53 | - flatbuffers==23.5.26 54 | - fonttools==4.47.2 55 | - frozenlist==1.4.1 56 | - gast==0.4.0 57 | - google-auth==2.26.2 58 | - google-auth-oauthlib==1.0.0 59 | - google-pasta==0.2.0 60 | - grpcio==1.60.0 61 | - h5py==3.7.0 62 | - idna==3.6 63 | - imageio==2.33.1 64 | - importlib-metadata==7.0.1 65 | - importlib-resources==6.1.1 66 | - ipython==8.10.0 67 | - iso3166==2.1.1 68 | - jax==0.4.13 69 | - jedi==0.19.1 70 | - joblib==1.3.2 71 | - keras==2.12.0 72 | - kiwisolver==1.4.5 73 | - lap==0.4.0 74 | - libclang==16.0.6 75 | - llvmlite==0.39.1 76 | - lxml==5.1.0 77 | - markdown==3.5.2 78 | - markupsafe==2.1.3 79 | - matplotlib==3.7.4 80 | - matplotlib-inline==0.1.6 81 | - ml-dtypes==0.2.0 82 | - multidict==6.0.4 83 | - networkx==3.1 84 | - numba==0.56.4 85 | - numpy==1.23.5 86 | - nvidia-cublas-cu11==11.10.3.66 87 | - nvidia-cuda-nvrtc-cu11==11.7.99 88 | - nvidia-cuda-runtime-cu11==11.7.99 89 | - nvidia-cudnn-cu11==8.5.0.96 90 | - oauthlib==3.2.2 91 | - omegaconf==2.3.0 92 | - opencv-python==4.6.0.66 93 | - opt-einsum==3.3.0 94 | - packaging==23.2 95 | - parso==0.8.3 96 | - pexpect==4.9.0 97 | - pickleshare==0.7.5 98 | - pillow==10.0.1 99 | - prompt-toolkit==3.0.43 100 | - protobuf==4.25.2 101 | - ptyprocess==0.7.0 102 | - pure-eval==0.2.2 103 | - pyasn1==0.5.1 104 | - pyasn1-modules==0.3.0 105 | - pycocotools==2.0.6 106 | - pygments==2.17.2 107 | - pykalman==0.9.5 108 | - pyparsing==3.1.1 109 | - python-dateutil==2.8.2 110 | - python-magic==0.4.27 111 | - pywavelets==1.4.1 112 | - pyyaml==6.0 113 | - requests==2.31.0 114 | - requests-oauthlib==1.3.1 115 | - rsa==4.9 116 | - rtree==1.1.0 117 | - scikit-image==0.19.3 118 | - scikit-learn==1.2.0 119 | - scipy==1.10.0 120 | - shapely==2.0.2 121 | - six==1.16.0 122 | - stack-data==0.6.3 123 | - tensorboard==2.12.3 124 | - tensorboard-data-server==0.7.2 125 | - tensorflow==2.12.0 126 | - tensorflow-estimator==2.12.0 127 | - tensorflow-io-gcs-filesystem==0.34.0 128 | - termcolor==2.4.0 129 | - threadpoolctl==3.2.0 130 | - tifffile==2023.7.10 131 | - torch==1.13.1 132 | - traitlets==5.14.1 133 | - typing-extensions==4.9.0 134 | - urllib3==2.1.0 135 | - vtk==9.2.2 136 | - wcwidth==0.2.13 137 | - werkzeug==3.0.1 138 | - wrapt==1.14.1 139 | - wslink==1.12.4 140 | - yarl==1.9.4 141 | - zipp==3.17.0 142 | prefix: /home/olek/anaconda3/envs/csg 143 | -------------------------------------------------------------------------------- /icon/clipper.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/icon/clipper.jpg -------------------------------------------------------------------------------- /icon/icon.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/icon/icon.gif -------------------------------------------------------------------------------- /icon/intersection.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/icon/intersection.jpg -------------------------------------------------------------------------------- /icon/pass.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/icon/pass.png -------------------------------------------------------------------------------- /icon/warning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/icon/warning.png -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | commonroad-io==2022.1 2 | cython-bbox==0.1.3 3 | easydict==1.10 4 | h5py==3.7.0 5 | ipython==8.10.0 6 | keras==3.9.0 7 | lap==0.4.0 8 | numba==0.56.4 9 | opencv-python==4.8.1.78 10 | Pillow==10.3.0 11 | pycocotools==2.0.6 12 | pykalman==0.9.5 13 | python-magic==0.4.27 14 | PyYAML==6.0 15 | scikit-image==0.19.3 16 | scikit-learn==1.5.0 17 | scipy==1.10.0 18 | tensorflow==2.12.1 19 | torch==2.2.0 20 | vtk==9.2.2 21 | -------------------------------------------------------------------------------- /src/SLAM/mono_kitti: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/SLAM/mono_kitti -------------------------------------------------------------------------------- /src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/__init__.py -------------------------------------------------------------------------------- /src/calibration/CamCal_config.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | class Config(object): 5 | COORD_SYS_TYP = 1 6 | # define the type of coordinate system for camera calibration: 0: X-Z ground plane; 1: X-Y ground plane (default: 0) 7 | VY_EST_TYP = 1 8 | # define the type of Vy estimation: 0: computing center of mass; 1: RANSAC (default: 1) 9 | LINF_EST_TYP = 1 10 | # define the type of Linf estimation: 0: linear regression; 1: RANSAC (default: 1) 11 | PRIN_PT_EST_TYP = 0 12 | # define the type of principal point estimation: 13 | # 0: assuming as the image center; 14 | # 1: the point with minimum distance to the perpendicular line to Linf (default: 0) 15 | VY_RS_ITER_NUM = 100 16 | # define the number of iterations in RANSAC for Vy estimation, necessary when VY_EST_TYP = 1 (default: 100) 17 | VY_RS_DIST_THLD = 2 18 | # define the threshold for the distance (divided by the frame width) of RANSAC inliers to Vy, necessary when VY_EST_TYP = 1 (default: 2.0) 19 | LINF_RS_ITER_NUM = 100 20 | # define the number of iterations in RANSAC for Linf estimation, necessary when LINF_EST_TYP = 1 (default: 100) 21 | LINF_RS_DIST_THLD = 0.2 22 | # define the threshold for the distance (divided by the frame height) of RANSAC inliers to Linf, necessary when LINF_EST_TYP = 1 (default: 0.15) 23 | EDA_RNG_F = 0.2 24 | # define the range for focal length in ratio in EDA optimization (default: 0.2f) 25 | EDA_RNG_PRIN_PT = 100 26 | # define the range for principal point coordinates in pixels in EDA optimization (default: 100) 27 | EDA_RNG_ROT_ANG = 45 #define the range for rotation angles in degrees in EDA optimization (default: 45.0) 28 | EDA_INIT_POP = 2000 #define the initial population of EDA (default: 2000) 29 | EDA_SEL_POP = 20 #define the selected population of EDA (default: 20) 30 | EDA_ITER_NUM = 200 #define the number of iterations of EDA (default: 100) 31 | EDA_REPROJ_ERR_THLD = 0.01 # define the threshold of ratio of reprojection errors between iterations (default: 0.10) 32 | IMG_EXPN_RAT = 2 #define image expansion ratio for plotting vanishing points and horizon line (default: 2.0f) 33 | CalCamHeiMin=4000 34 | CalCamHeiMax=6500 35 | LenUnit=1 36 | # points selected from image for distance calib 37 | MeasLnSegNdPt=np.array(([1,1],[2,3])) 38 | # corresponding distance 39 | MeasLnSegDist=np.array([1]) 40 | ### set range of parameter opmization 41 | fFxMax = 5000 42 | fFxMin = 0 43 | fFyMax = 5000 44 | fFyMin = 0 45 | fCxMax = 5000 46 | fCxMin = 0 47 | fCyMax = 5000 48 | fCyMin = 0 49 | fRollMax = math.pi/2 50 | fRollMin = -math.pi/2 51 | fPitchMax = math.pi/2 52 | fPitchMin = -math.pi/2 53 | fYawMax = math.pi/2 54 | fYawMin = -math.pi/2 55 | fTxMax = 10 56 | fTxMin = -10 57 | fTyMax = 10 58 | fTyMin = -10 59 | fTzMax = 10 60 | fTzMin = -10 61 | CalEdaOptFlg=1 62 | CalSelVanLnFlg = 1 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/calibration/CamParam.py: -------------------------------------------------------------------------------- 1 | 2 | import random 3 | from .CamCal_config import Config 4 | import numpy as np 5 | import math 6 | from .ParamRng import ParamRng 7 | import secrets 8 | 9 | class CamParam: 10 | def __init__(self): 11 | self.R_Mat = [] 12 | self.Fx = 0 13 | self.Fy = 0 14 | self.Cx = 0 15 | self.Cy = 0 16 | self.Pitch = 0 17 | self.Yaw = 0 18 | self.Roll = 0 19 | self.Tx = 0 20 | self.Ty = 0 21 | self.Tz = 0 22 | self.config = Config() 23 | self.img_width =1920 24 | self.img_height =1080 25 | self.Mat_K = np.zeros((3, 3)) 26 | self.Mat_R = np.zeros((3, 3)) 27 | self.Mat_T = np.zeros((3, 1)) 28 | self.Mat_P = np.zeros((3, 4)) 29 | self.fReprojErr=0 30 | 31 | def setMatrix_K(self,f_x, f_y, c_x, c_y): 32 | self.Mat_K[0, 0] = f_x 33 | self.Mat_K[1, 1] = f_y 34 | self.Mat_K[2, 2] = 1.0 35 | self.Mat_K[0, 2] = c_x 36 | self.Mat_K[1, 2] = c_y 37 | 38 | def setMatrix_R(self, fRoll, fPitch, fYaw): 39 | 40 | if Config.COORD_SYS_TYP == 0: 41 | self.Mat_R[0,0]= math.cos(fRoll) * math.cos(fYaw) - math.sin(fRoll) * math.sin(fPitch) * math.sin(fYaw) 42 | self.Mat_R[0, 1] = -math.sin(fRoll) * math.cos(fPitch) 43 | self.Mat_R[0, 2] = (math.cos(fRoll) * math.sin(fYaw)) + (math.sin(fRoll) * math.sin(fPitch) * math.cos(fYaw)) 44 | self.Mat_R[1, 0] = (math.sin(fRoll) * math.cos(fYaw)) + (math.cos(fRoll) * math.sin(fPitch) * math.sin(fYaw)) 45 | self.Mat_R[1, 1] = math.cos(fRoll) * math.cos(fPitch) 46 | self.Mat_R[1, 2] = (math.sin(fRoll) * math.sin(fYaw)) - (math.cos(fRoll) * math.sin(fPitch) * math.cos(fYaw)) 47 | self.Mat_R[2, 0] = -math.cos(fPitch) * math.sin(fYaw) 48 | self.Mat_R[2, 1] = math.sin(fPitch) 49 | self.Mat_R[2, 2] = math.cos(fPitch) * math.cos(fYaw) 50 | elif Config.COORD_SYS_TYP == 1: 51 | self.Mat_R[0, 0] = (-math.cos(fRoll) * math.sin(fYaw)) - (math.sin(fRoll) * math.sin(fPitch) * math.cos(fYaw)) 52 | self.Mat_R[0, 1] = (-math.cos(fRoll) * math.cos(fYaw)) - (math.sin(fRoll) * math.sin(fPitch) * math.cos(fYaw)) 53 | self.Mat_R[0, 2] = math.sin(fRoll) * math.cos(fPitch) 54 | self.Mat_R[1, 0] = (-math.sin(fRoll) * math.sin(fYaw)) + (math.cos(fRoll) * math.sin(fPitch) * math.cos(fYaw)) 55 | self.Mat_R[1, 1] = (-math.sin(fRoll) * math.cos(fYaw)) - (math.cos(fRoll) * math.sin(fPitch) * math.sin(fYaw)) 56 | self.Mat_R[1, 2] = -math.cos(fRoll) * math.cos(fPitch) 57 | self.Mat_R[2, 0] = math.cos(fPitch) * math.cos(fYaw) 58 | self.Mat_R[2, 1] = -math.cos(fPitch) * math.sin(fYaw) 59 | self.Mat_R[2, 2] = math.sin(fPitch) 60 | 61 | def setMatrix_T(self,Tx, Ty, Tz): 62 | 63 | self.Mat_T[0] = Tx 64 | self.Mat_T[1] = Ty 65 | self.Mat_T[2] = Tz 66 | 67 | 68 | def calMatrix_P(self): 69 | temp_T=-np.dot(self.Mat_R,self.Mat_T) 70 | self.Mat_P=np.dot(self.Mat_K,np.concatenate((self.Mat_R,temp_T),axis=1)) 71 | 72 | def getFrmSize(self): 73 | return self.img_width 74 | 75 | def initCamMdl(self, sParamRng = ParamRng()): 76 | secretsGenerator = secrets.SystemRandom() 77 | 78 | self.Fx = secretsGenerator.uniform(sParamRng.fFxMin, sParamRng.fFxMax) 79 | self.Fy = secretsGenerator.uniform(sParamRng.fFyMin, sParamRng.fFyMax) 80 | self.Cx = secretsGenerator.uniform(sParamRng.fCxMin, sParamRng.fCxMax) 81 | self.Cy = secretsGenerator.uniform(sParamRng.fCyMin, sParamRng.fCyMax) 82 | self.Roll = secretsGenerator.uniform(sParamRng.fRollMin, sParamRng.fRollMax) 83 | self.Pitch = secretsGenerator.uniform(sParamRng.fPitchMin, sParamRng.fPitchMax) 84 | self.Yaw = secretsGenerator.uniform(sParamRng.fYawMin, sParamRng.fYawMax) 85 | self.Tx = secretsGenerator.uniform(sParamRng.fTxMin, sParamRng.fTxMax) 86 | self.Ty = secretsGenerator.uniform(sParamRng.fTyMin, sParamRng.fTyMax) 87 | self.Tz = secretsGenerator.uniform(sParamRng.fTzMin, sParamRng.fTzMax) 88 | 89 | #self.Fx = random.uniform(sParamRng.fFxMin, sParamRng.fFxMax) 90 | #self.Fy = random.uniform(sParamRng.fFyMin, sParamRng.fFyMax) 91 | #self.Cx = random.uniform(sParamRng.fCxMin, sParamRng.fCxMax) 92 | #self.Cy = random.uniform(sParamRng.fCyMin, sParamRng.fCyMax) 93 | #self.Roll = random.uniform(sParamRng.fRollMin, sParamRng.fRollMax) 94 | #self.Pitch = random.uniform(sParamRng.fPitchMin, sParamRng.fPitchMax) 95 | #self.Yaw = random.uniform(sParamRng.fYawMin, sParamRng.fYawMax) 96 | #self.Tx = random.uniform(sParamRng.fTxMin, sParamRng.fTxMax) 97 | #self.Ty = random.uniform(sParamRng.fTyMin, sParamRng.fTyMax) 98 | #self.Tz = random.uniform(sParamRng.fTzMin, sParamRng.fTzMax) 99 | 100 | def setReprojErr(self,fReprojErr): 101 | self.fReprojErr = fReprojErr 102 | -------------------------------------------------------------------------------- /src/calibration/ParamRng.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | class ParamRng: 4 | def __init__(self): 5 | self.fFxMax = 0 6 | self.fFxMin = 0 7 | self.fFyMax = 0 8 | self.fFyMin = 0 9 | self.fCxMax = 0 10 | self.fCxMin = 0 11 | self.fCyMax = 0 12 | self.fCyMin = 0 13 | self.fRollMax = 0 14 | self.fRollMin = 0 15 | self.fPitchMax = 0 16 | self.fPitchMin = 0 17 | self.fYawMax = 0 18 | self.fYawMin = 0 19 | self.fTxMax = 0 20 | self.fTxMin = 0 21 | -------------------------------------------------------------------------------- /src/calibration/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/calibration/__init__.py -------------------------------------------------------------------------------- /src/calibration/calib_main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from src.calibration.VPLine import CVanLnSel 3 | from src.calibration.CameraCal import CamCal 4 | from src.ui.image_utils import get_pixel_coordinate 5 | import copy 6 | 7 | 8 | def camera_clibration_canvas(para_lines, ref_points_distance, raw_image_shape, 9 | image_scale, image_paddings): 10 | # convert to image coordinates 11 | CALIBRATION_SHAPE=(1920,1080) 12 | FPts = [] 13 | SegNodes = [] 14 | NodeDist = [] 15 | camcal_obj=CamCal() 16 | vpline = CVanLnSel() 17 | width_scale = CALIBRATION_SHAPE[0]/raw_image_shape[0] 18 | height_scale = CALIBRATION_SHAPE[1]/raw_image_shape[1] 19 | para_lines_copy = copy.deepcopy(para_lines) 20 | ref_points_distance_copy = copy.deepcopy(ref_points_distance) 21 | for paraline in para_lines_copy: 22 | for i in range(2):#2 lines 23 | canvas_x, canvas_y = paraline[i][1], paraline[i][2] 24 | pixel_x, pixel_y = get_pixel_coordinate(canvas_x, canvas_y, image_scale, image_paddings) 25 | FPts.append(np.array([pixel_x*width_scale, pixel_y*height_scale]))#fit to calibration image 26 | canvas_x, canvas_y = paraline[i][3], paraline[i][4] 27 | pixel_x, pixel_y = get_pixel_coordinate(canvas_x, canvas_y, image_scale, image_paddings) 28 | FPts.append(np.array([pixel_x*width_scale, pixel_y*height_scale]))#fit to calibration image 29 | for ref_line in ref_points_distance_copy: 30 | for i in range(2): 31 | canvas_x, canvas_y = ref_line[1+2*i], ref_line[1+2*i+1] 32 | pixel_x, pixel_y = get_pixel_coordinate(canvas_x, canvas_y, image_scale, image_paddings) 33 | SegNodes.append(np.array([pixel_x*width_scale, pixel_y*height_scale])) 34 | NodeDist.append([float(ref_line[-1])]) 35 | 36 | VPs = vpline.process(FPts) 37 | camcal_obj.process(VPs, SegNodes, NodeDist) 38 | 39 | return camcal_obj.CamParam 40 | 41 | -------------------------------------------------------------------------------- /src/calibration/cameracalib.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import sys 4 | import random 5 | import math 6 | import numpy as np 7 | import skimage.io 8 | import matplotlib 9 | import matplotlib.pyplot as plt 10 | import pickle 11 | import scipy.io as sio 12 | from PIL import Image, ImageDraw 13 | import pylab as plt 14 | import numpy.linalg as nplg 15 | 16 | # Root directory of the project 17 | #ROOT_DIR = os.getcwd() 18 | 19 | # Directory of images to run detection on 20 | #IMAGE_DIR = os.path.join(ROOT_DIR, "images") 21 | # Pmat=sio.loadmat('MatForTestPara_1900_190313.mat') 22 | #Pmat = sio.loadmat('PMat_1900_190513_case2.mat') 23 | #Pinst = Pmat['P_Test'] 24 | #Hinst = Pmat['H_Test'] 25 | 26 | 27 | def I2WCal(x, y, h_real, isI2W, Pinst): 28 | P = np.asarray(Pinst) 29 | H = P[:, (0, 1, 3)] 30 | if isI2W == 1: 31 | x1 = x 32 | y1 = y 33 | if h_real == 0: 34 | # print(H) 35 | # print(Tran) 36 | # print(Tran*(np.linalg.inv(H)*np.array([[x1,y1,1]]).T)) 37 | Coor = normalization(np.dot(np.linalg.inv(H), np.array([x1, y1, 1]).reshape((3, 1)))) 38 | # VA=np.dot(np.linalg.inv(H),np.array([x1,y1,1]).reshape((3,1))) 39 | # Coor=normalization(np.dot(Tran,np.dot(np.linalg.inv(H),np.array([x1,y1,1]).reshape((3,1))))) 40 | # print(VA) 41 | else: 42 | h = h_real 43 | Hi = [x1, y1] 44 | a1 = Hi[0] * P[2, 0] - P[0, 0] 45 | b1 = Hi[0] * P[2, 1] - P[0, 1] 46 | a2 = Hi[1] * P[2, 0] - P[1, 0] 47 | b2 = Hi[1] * P[2, 1] - P[1, 1] 48 | c1 = (P[0, 2] - Hi[0] * P[2, 2]) * h + P[0, 3] - Hi[0] * P[2, 3] 49 | c2 = (P[1, 2] - Hi[1] * P[2, 2]) * h + P[1, 3] - Hi[1] * P[2, 3] 50 | Hl = np.array([[a1, b1], [a2, b2]]) 51 | C = np.array([[c1], [c2]]) 52 | temp1 = np.dot(np.linalg.inv(Hl), C) 53 | # print((P[1,2]-Hi[1]*P[2,2])*h) 54 | # print(Hi[1]*P[2,3]) 55 | # print(P[1,3]) 56 | # print(Hl) 57 | # print(np.linalg.inv(Hl)) 58 | # print(C) 59 | # Coor=np.array([temp1[0],temp1[1]]) 60 | # Coor=normalization(np.dot(Tran,np.array([temp1[0],temp1[1],1]).reshape((3,1)))) 61 | Coor = normalization(np.array([temp1[0], temp1[1], 1]).reshape((3, 1))) 62 | # Coor=normalization(temp1.reshape((3,1))) 63 | 64 | elif isI2W == 0: 65 | 66 | Coor = normalization(np.dot(P, np.array([x, y, h_real, 1]).reshape((4, 1)))) 67 | Coor[0] = Coor[0] * 1 68 | Coor[1] = Coor[1] * 1 69 | # X=Coor[0] 70 | # Y=Coor[1] 71 | return Coor 72 | 73 | 74 | # In[272]: 75 | 76 | def normalization(x): 77 | num = x.shape[1] 78 | y = np.zeros((3, num)); 79 | for i in range(0, num): 80 | y[0, i] = x[0, i] / x[2, i]; 81 | y[1, i] = x[1, i] / x[2, i]; 82 | y[2, i] = 1; 83 | return y 84 | 85 | 86 | def BBoxRegression(CData, Outline, Class_type, ry): 87 | H = Hinst 88 | P = Pinst 89 | CData1 = np.zeros((1080, 1902)) 90 | # CData1[Outline(0):Outline(2),Outline(1):Outline(3)]=CData; 91 | CData1 = CData 92 | # P=Pmat['P_'+CamNum] 93 | # Tran=TranMat['tran_'+CamNum] 94 | # HeightHeadCurve=HeightHeadCurveMat['HeightHeadCurve_'+CamNum] 95 | # HeightFootCurve=HeightFootCurveMat['HeightCurve_'+CamNum] 96 | # H=np.array(P[:,[0,1,3]]) 97 | CenterInitial1 = np.array([(Outline[1] + Outline[3]) / 2, (Outline[0] + Outline[2]) / 2]) 98 | # print(CenterInitial1) 99 | BIN = 3 100 | BinInter = 5 101 | XRange = np.arange(-(Outline[3] - Outline[1]) / BIN, (Outline[3] - Outline[1]) / BIN, BinInter) 102 | YRange = np.arange(-(Outline[2] - Outline[0]) / BIN, (Outline[2] - Outline[0]) / BIN, BinInter) 103 | CenterCandX = XRange + CenterInitial1[0] 104 | CenterCandY = YRange + CenterInitial1[1] + 10 105 | UniCenterCandX = CenterCandX * 1 106 | UniCenterCandY = CenterCandY * 1 107 | LengthX = len(XRange) 108 | LengthY = len(YRange) 109 | face_idx = np.array([[1, 2, 6, 5], [2, 3, 7, 6], [3, 4, 8, 7], [4, 1, 5, 8]]) 110 | Dimension = np.zeros((9, 3)) 111 | Dimension_Car = np.array([[4200, 1700, 1400]]) 112 | Dimension_Truck = np.array([[6500, 3500, 2500]]) 113 | Dimension[3, :] = Dimension_Car 114 | Dimension[6, :] = Dimension_Truck 115 | R = np.array([[math.cos(ry), -math.sin(ry), 0], [math.sin(ry), math.cos(ry), 0], [0, 0, 1]]) 116 | l = Dimension[Class_type, 0] 117 | w = Dimension[Class_type, 1] 118 | h = Dimension[Class_type, 2] 119 | y_corners = np.array([[l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]]) 120 | x_corners = np.array([[w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]]) 121 | z_corners = np.array([[0, 0, 0, 0, h, h, h, h]]) 122 | Dimension_3D = np.concatenate((x_corners, y_corners, z_corners), axis=0) 123 | Dimension_3D = np.dot(R, Dimension_3D) 124 | MountFinal = 0 125 | worldLocation = 0 126 | for i in range(0, LengthX): 127 | for j in range(0, LengthY): 128 | Coor = normalization( 129 | np.dot(np.linalg.inv(H), np.array([UniCenterCandX[i], UniCenterCandY[j], 1]).reshape((3, 1)))) 130 | # temp=[Coor(1)/Coor(3),Coor(2)/Coor(3),0]; 131 | corners_3D = np.zeros((3, 8)) 132 | corners_3D[0, :] = Dimension_3D[0, :] + Coor[0] 133 | corners_3D[1, :] = Dimension_3D[1, :] + Coor[1] 134 | corners_3D[2, :] = Dimension_3D[2, :] 135 | # corners_3D(3,:)=corners_3D(3,:)*HeightFootCurve(floor(CenterCandY(j))); 136 | corners_2D = np.dot(P, np.concatenate((corners_3D, np.ones((1, corners_3D.shape[1]))), axis=0)) 137 | corners_2D = normalization(corners_2D) 138 | corners_2D[0, :] = corners_2D[0, :] * 1 139 | corners_2D[1, :] = corners_2D[1, :] * 1 140 | if corners_2D[0, 0] > corners_2D[0, 7]: 141 | draw_order = np.array([1, 5, 6, 7, 3, 4, 1]) - 1 142 | else: 143 | draw_order = np.array([2, 3, 4, 8, 5, 6, 2]) - 1 144 | # print(corners_2D) 145 | NewCornersX = corners_2D[0, draw_order] - round(min(corners_2D[0, draw_order])) 146 | NewCornersY = corners_2D[1, draw_order] - round(min(corners_2D[1, draw_order])) 147 | NewRangeX = int(3 + round(max(corners_2D[0, draw_order]) - min(corners_2D[0, draw_order]))) 148 | NewRangeY = int(3 + round(max(corners_2D[1, draw_order]) - min(corners_2D[1, draw_order]))) 149 | Xs = int(max(round(min(corners_2D[0, draw_order])) - 10, 1)) 150 | Ys = int(max(round(min(corners_2D[1, draw_order])) - 10, 1)) 151 | # print(Xs,Ys) 152 | # print(NewRangeX,NewRangeY) 153 | CData1_Resize = CData1[Ys:min(Ys + NewRangeY, 1080), Xs:min(Xs + NewRangeX, 1902)] 154 | # mask = poly2mask(NewCornersX+10,NewCornersY+10,(min(Ys+NewRangeY,1080)-Ys,min(Xs+NewRangeX,1902)-Xs)) 155 | mask = poly2mask(np.maximum(corners_2D[0, draw_order], 1), np.maximum(corners_2D[1, draw_order], 1), 156 | [1080, 1902]) 157 | # mask=poly2mask(NewCornersX+10,NewCornersY+10,min(Ys+NewRangeY,1200)-Ys,min(Xs+NewRangeX,1600)-Xs); 158 | # print('mask',mask.shape) 159 | Mount = mask * CData1 160 | 161 | Mount = np.sum(Mount) 162 | if MountFinal < Mount: 163 | # Location=[CenterCandX(i),CenterCandY(j)] 164 | worldLocation = Coor 165 | MountFinal = Mount 166 | # plt.imshow(Mount) 167 | # Corners_2D_Final=corners_2D 168 | # DrawOrder=draw_order 169 | MaskOut = mask 170 | 171 | else: 172 | MountFinal = MountFinal 173 | 174 | return worldLocation 175 | 176 | 177 | # In[234]: 178 | 179 | 180 | from skimage import draw 181 | import numpy as np 182 | 183 | 184 | def poly2mask(vertex_row_coords, vertex_col_coords, shape): 185 | fill_row_coords, fill_col_coords = draw.polygon(vertex_row_coords, vertex_col_coords, shape) 186 | mask = np.zeros(shape, dtype=np.bool) 187 | mask[fill_row_coords, fill_col_coords] = 1 188 | return mask 189 | 190 | # In[ ]: 191 | -------------------------------------------------------------------------------- /src/convert_util.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import math 4 | import copy 5 | 6 | 7 | def inverse_rigid_trans(Tr): 8 | """ Inverse a rigid body transform matrix (3x4 as [R|t]) 9 | [R'|-R't; 0|1] 10 | """ 11 | inv_Tr = np.zeros_like(Tr) # 3x4 12 | inv_Tr[0:3, 0:3] = np.transpose(Tr[0:3, 0:3]) 13 | inv_Tr[0:3, 3] = np.dot(-np.transpose(Tr[0:3, 0:3]), Tr[0:3, 3]) 14 | return inv_Tr 15 | 16 | 17 | def project_2d_to_3d_depth(image_pt,intrisicMat, extrisicMat, depthValue, reorder=True): 18 | #ref: https://github.com/raulmur/ORB_SLAM2/issues/226 19 | u,v = image_pt[0].item(), image_pt[1].item() 20 | d = depthValue 21 | 22 | #fx = intrisicMat[0][0] 23 | #fy = intrisicMat[1][1] 24 | #cx = intrisicMat[0][2] 25 | #cy = intrisicMat[1][2] 26 | #x = ((u - cx)*d)/fx 27 | #y = ((v - cy)*d)/fy 28 | #pts_3d_ref = np.asarray([x,y,d]).reshape(3,1) 29 | twc = inverse_rigid_trans(extrisicMat) #T_wc 30 | rwc = twc[:3,:3]#extrisicMat[:3,:3]#inv_T[:3,:3] 31 | twc_vec = twc[:3,3].reshape(3,1)#extrisicMat[:3,3].reshape(3,1) 32 | p_c = np.dot(np.linalg.inv(intrisicMat), (np.asarray([u*d,v*d,d]).reshape(3,1))) 33 | p_world = np.dot(np.transpose(rwc), (p_c-twc_vec)) 34 | 35 | #debug 36 | if 0: 37 | homo_pt3d = np.array([p_world[0].item(), p_world[1].item(), p_world[2].item(), 1]) 38 | uvw = np.dot(intrisicMat, np.dot(extrisicMat[:3,:], homo_pt3d)) 39 | norm_uvw =[uvw[0]/uvw[2], uvw[1]/uvw[2], 1] 40 | #check diff between norm_uvw and image_pt 41 | 42 | #reshape to xyz-order 43 | if reorder: 44 | xyz_index = [0,2,1] 45 | p_world = p_world[xyz_index].squeeze() 46 | else: 47 | p_world = p_world.squeeze() 48 | 49 | return p_world 50 | 51 | def project_2d_to_3d_depth_arr(pt2d_arr, 52 | intrisicMat, extrisicMat, 53 | depth_arr, reorder=True): 54 | #pt2d_arr: (2xN), each column indicates a point 55 | #return value: pt3d: (2xN) 56 | N = pt2d_arr.shape[1] 57 | homo_pt2d_arr = np.zeros((3,N)) 58 | for i in range(N): 59 | homo_pt2d_arr[0,i] = pt2d_arr[0,i]* depth_arr[i] 60 | homo_pt2d_arr[1,i] = pt2d_arr[1,i]* depth_arr[i] 61 | homo_pt2d_arr[2,i] = depth_arr[i] 62 | twc = inverse_rigid_trans(extrisicMat) #T_wc 63 | rotMat = twc[:3,:3] 64 | transVec = twc[:3,3].reshape(3,1) 65 | transMat = np.repeat(transVec, N, axis=1) 66 | 67 | p_cam = np.dot(np.linalg.inv(intrisicMat), homo_pt2d_arr) 68 | p_cam_trans = p_cam - transMat 69 | p_world = np.dot(np.linalg.inv(rotMat), (p_cam - transMat)) 70 | 71 | #debug 72 | if 0: 73 | homo_pt3d = np.array([p_world[0].item(), p_world[1].item(), p_world[2].item(), 1]) 74 | uvw = np.dot(intrisicMat, np.dot(extrisicMat[:3,:], homo_pt3d)) 75 | norm_uvw =[uvw[0]/uvw[2], uvw[1]/uvw[2], 1] 76 | #check diff between norm_uvw and image_pt 77 | 78 | #reshape to xyz-order 79 | if reorder: 80 | xyz_index = [0,2,1] 81 | p_world = p_world[xyz_index, :].squeeze() 82 | else: 83 | p_world = p_world.squeeze() 84 | 85 | return p_world 86 | 87 | def project_3d_to_2d(pt3d, intrisicMat, extrisicMat, reorder=True): 88 | #intrinsicMat: 3x3 89 | #extrisicMat: 3x4[R|T] 90 | #pt3d: 3xn (each column represent a coordinate) 91 | 92 | #reorder 93 | if reorder: 94 | xyz_index = [0,2,1] 95 | pt3d = pt3d[xyz_index,:] 96 | n = pt3d.shape[1] 97 | twc = inverse_rigid_trans(extrisicMat) #T_wc 98 | homo_pt3d = np.append(pt3d, np.ones((1, n)), axis=0) 99 | #uvw = np.dot(intrisicMat, np.dot(extrisicMat[:3,:], homo_pt3d)) 100 | uvw = np.dot(intrisicMat, np.dot(twc[:3,:], homo_pt3d)) 101 | uvw[0,:] /=uvw[2,:] 102 | uvw[1,:] /=uvw[2,:] 103 | uvw[2,:] = 1 104 | return uvw[:2,:] 105 | 106 | 107 | def cart2hom( pts_3d): 108 | """ Input: nx3 points in Cartesian 109 | Oupput: nx4 points in Homogeneous by pending 1 110 | """ 111 | n = pts_3d.shape[0] 112 | pts_3d_hom = np.hstack((pts_3d, np.ones((n, 1)))) 113 | return pts_3d_hom 114 | def calculate_speed(p3d1, p3d2, frame_interval=1, fps=10): 115 | # Input 3D coordinates 116 | # return speed in km/h 117 | 118 | dist = math.sqrt((p3d1[0]-p3d2[0])*(p3d1[0]-p3d2[0])+ 119 | (p3d1[2]-p3d2[2])*(p3d1[2]-p3d2[2])) 120 | v_meter_sec = dist * fps/frame_interval 121 | v_km_hour = 3.6 * v_meter_sec 122 | return v_km_hour 123 | 124 | def get_abs_pose(rela_pose, ref_pose): 125 | #rela_pose: convert to current pose to referent_pose 126 | #all pose is 4x4[R|T] 127 | 128 | prod = np.dot(rela_pose, ref_pose) 129 | return prod[:3,:] 130 | 131 | def get_abs_slam_pose(slam_mat, sc): 132 | abs_slam_mat = copy.deepcopy(slam_mat) 133 | sx,sy,sz = sc[0], sc[1],sc[2] 134 | for i in range(len(slam_mat)): 135 | abs_slam_mat[i][0,-1] *= sx 136 | abs_slam_mat[i][1,-1] *= sy 137 | abs_slam_mat[i][2,-1] *= sz 138 | 139 | return abs_slam_mat 140 | 141 | -------------------------------------------------------------------------------- /src/depth/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/depth/__init__.py -------------------------------------------------------------------------------- /src/depth/monodepth.py: -------------------------------------------------------------------------------- 1 | from __future__ import division 2 | import os 3 | import time 4 | import math 5 | import numpy as np 6 | import tensorflow as tf 7 | import tensorflow.contrib.slim as slim 8 | from .net import Net 9 | 10 | #import matplotlib as mpl 11 | #import matplotlib.cm as cm 12 | import cv2 13 | 14 | from tensorflow.python.ops import control_flow_ops 15 | from numba import cuda 16 | 17 | def disp_to_depth(disp, min_depth, max_depth): 18 | min_disp = 1. / max_depth 19 | max_disp = 1. / min_depth 20 | scaled_disp = tf.to_float(min_disp) + tf.to_float(max_disp - min_disp) * disp 21 | depth = tf.to_float(1.) / scaled_disp 22 | return depth 23 | 24 | class DepthEstimator(object): 25 | def __init__(self): 26 | config = dict() 27 | config['model'] = dict() 28 | config['dataset'] = dict() 29 | config['model']['batch_norm_decay'] =0.95 30 | config['model']['batch_norm_epsilon'] = 1e-5 31 | config['model']['batch_size'] = 1 32 | config['model']['pose_scale'] = 1e-2 33 | config['model']['num_source']=3 34 | config['model']['num_scales']=4 35 | config['dataset']['image_width'] = 640 36 | config['dataset']['image_height']=192 37 | config['dataset']['min_depth'] = 0.1 38 | config['dataset']['max_depth'] = 100. 39 | 40 | self.config = config 41 | self.preprocess = True 42 | self.min_depth = np.float(config['dataset']['min_depth']) 43 | self.max_depth = np.float(config['dataset']['max_depth']) 44 | 45 | def preprocess_image(self, image): 46 | image = (image - 0.45) / 0.225 47 | return image 48 | 49 | def build_model(self, ckpt): 50 | self.num_scales = self.config['model']['num_scales'] 51 | self.num_source = self.config['model']['num_source'] 52 | 53 | with tf.name_scope('data_loading'): 54 | self.tgt_image_uint8 = tf.compat.v1.placeholder(tf.uint8, [1, 55 | self.config['dataset']['image_height'], self.config['dataset']['image_width'], 3]) 56 | self.tgt_image = tf.image.convert_image_dtype(self.tgt_image_uint8, dtype=tf.float32) 57 | tgt_image_net = self.preprocess_image(self.tgt_image) 58 | 59 | with tf.variable_scope('monodepth2_model', reuse=tf.AUTO_REUSE) as scope: 60 | net_builder = Net(False, **self.config) 61 | res18_tc, skips_tc = net_builder.build_resnet18(tgt_image_net) 62 | pred_disp = net_builder.build_disp_net(res18_tc, skips_tc) 63 | pred_disp_rawscale = [tf.image.resize_bilinear(pred_disp[i], 64 | [self.config['dataset']['image_height'], self.config['dataset']['image_width']]) for i in 65 | range(self.num_scales)] 66 | pred_depth_rawscale = disp_to_depth(pred_disp_rawscale, self.min_depth, self.max_depth) 67 | 68 | self.pred_depth = pred_depth_rawscale[0] 69 | self.pred_disp = pred_disp_rawscale[0] 70 | 71 | var_list = [var for var in tf.compat.v1.global_variables() if "moving" in var.name] 72 | var_list += tf.compat.v1.trainable_variables() 73 | saver = tf.compat.v1.train.Saver(var_list, max_to_keep=10) 74 | config = tf.compat.v1.ConfigProto() 75 | config.gpu_options.allow_growth = True 76 | self.sess = tf.compat.v1.Session(config=config) 77 | latest_ckpt = tf.train.latest_checkpoint('{}'.format(ckpt)) 78 | saver.restore(self.sess,latest_ckpt) 79 | 80 | 81 | def estimate_image(self, bgr): 82 | 83 | image = cv2.cvtColor(bgr,cv2.COLOR_BGR2RGB) 84 | image = cv2.resize(image,(self.config['dataset']['image_width'],self.config['dataset']['image_height']),cv2.INTER_AREA) 85 | image =np.expand_dims(image,axis=0) 86 | tgt_image_np_batch = image 87 | fetches = {'depth': self.pred_depth,'disp': self.pred_disp} 88 | results = self.sess.run(fetches, feed_dict={self.tgt_image_uint8: tgt_image_np_batch}) 89 | src_depth =np.squeeze(results['depth']) 90 | depth_resized = cv2.resize(src_depth,(bgr.shape[1], bgr.shape[0]), interpolation=cv2.INTER_CUBIC) 91 | 92 | #import matplotlib.pyplot as plt 93 | #plt.imshow(depth_resized) 94 | 95 | return depth_resized 96 | 97 | def release(self): 98 | 99 | self.sess.close() 100 | device = cuda.get_current_device() 101 | device.reset() 102 | return 103 | 104 | -------------------------------------------------------------------------------- /src/detect/VehicleLampDet.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | from sklearn.cluster import MeanShift,KMeans,estimate_bandwidth 4 | 5 | 6 | 7 | def Detect(roi): 8 | center=[0,0] 9 | #hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV) 10 | hsv=roi 11 | red_lower1 = np.array([150, 43, 46]) 12 | red_upper1 = np.array([185, 255, 255]) 13 | red_lower2 = np.array([0, 43, 46]) 14 | red_upper2 = np.array([10, 255, 255]) 15 | 16 | mask1 = cv2.inRange(hsv, red_lower1, red_upper1) 17 | mask2 = cv2.inRange(hsv, red_lower2, red_upper2) 18 | mask = mask1 + mask2 19 | Xlist = [] 20 | for i in range(mask.shape[0]): 21 | for j in range(mask.shape[1]): 22 | if mask[i, j] == 255: 23 | Xlist.append((i, j)) 24 | if Xlist: 25 | Xlist = np.asarray(Xlist) 26 | bandwidth = estimate_bandwidth(Xlist, quantile=0.33, n_samples=500) 27 | if bandwidth<=0: 28 | bandwidth=5 29 | clustering = MeanShift(bandwidth=bandwidth).fit(Xlist) 30 | Num_cluster = len(clustering.cluster_centers_) 31 | if Num_cluster>=2: 32 | kkk = np.zeros((Num_cluster, 1)) 33 | for i in range(Num_cluster): 34 | temp = [x for x in clustering.labels_ if x == i] 35 | kkk[i, 0] = len(temp)/len(clustering.labels_) 36 | #kk = np.sum(kkk) 37 | max0=clustering.cluster_centers_[0] 38 | max1=clustering.cluster_centers_[1] 39 | center=np.array([(max0[1]+max1[1])/2, (max0[0]+max1[0])/2]) # [y,x] 40 | print(center) 41 | else: 42 | center=[0, 0] 43 | return center 44 | 45 | -------------------------------------------------------------------------------- /src/detect/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/detect/__init__.py -------------------------------------------------------------------------------- /src/detect/mask_rcnn/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/detect/mask_rcnn/__init__.py -------------------------------------------------------------------------------- /src/detect/mask_rcnn/config.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mask R-CNN 3 | Base Configurations class. 4 | 5 | Copyright (c) 2017 Matterport, Inc. 6 | Licensed under the MIT License (see LICENSE for details) 7 | Written by Waleed Abdulla 8 | """ 9 | 10 | import math 11 | import numpy as np 12 | 13 | 14 | # Base Configuration Class 15 | # Don't use this class directly. Instead, sub-class it and override 16 | # the configurations you need to change. 17 | 18 | class Config(object): 19 | """Base configuration class. For custom configurations, create a 20 | sub-class that inherits from this one and override properties 21 | that need to be changed. 22 | """ 23 | # Name the configurations. For example, 'COCO', 'Experiment 3', ...etc. 24 | # Useful if your code needs to do things differently depending on which 25 | # experiment is running. 26 | NAME = None # Override in sub-classes 27 | 28 | # NUMBER OF GPUs to use. For CPU training, use 1 29 | GPU_COUNT = 1 30 | 31 | # Number of images to train with on each GPU. A 12GB GPU can typically 32 | # handle 2 images of 1024x1024px. 33 | # Adjust based on your GPU memory and image sizes. Use the highest 34 | # number that your GPU can handle for best performance. 35 | IMAGES_PER_GPU = 2 36 | 37 | # Number of training steps per epoch 38 | # This doesn't need to match the size of the training set. Tensorboard 39 | # updates are saved at the end of each epoch, so setting this to a 40 | # smaller number means getting more frequent TensorBoard updates. 41 | # Validation stats are also calculated at each epoch end and they 42 | # might take a while, so don't set this too small to avoid spending 43 | # a lot of time on validation stats. 44 | STEPS_PER_EPOCH = 1000 45 | 46 | # Number of validation steps to run at the end of every training epoch. 47 | # A bigger number improves accuracy of validation stats, but slows 48 | # down the training. 49 | VALIDATION_STEPS = 50 50 | 51 | # The strides of each layer of the FPN Pyramid. These values 52 | # are based on a Resnet101 backbone. 53 | BACKBONE_STRIDES = [4, 8, 16, 32, 64] 54 | 55 | # Number of classification classes (including background) 56 | NUM_CLASSES = 1 # Override in sub-classes 57 | 58 | # Length of square anchor side in pixels 59 | RPN_ANCHOR_SCALES = (32, 64, 128, 256, 512) 60 | 61 | # Ratios of anchors at each cell (width/height) 62 | # A value of 1 represents a square anchor, and 0.5 is a wide anchor 63 | RPN_ANCHOR_RATIOS = [0.5, 1, 2] 64 | 65 | # Anchor stride 66 | # If 1 then anchors are created for each cell in the backbone feature map. 67 | # If 2, then anchors are created for every other cell, and so on. 68 | RPN_ANCHOR_STRIDE = 1 69 | 70 | # Non-max suppression threshold to filter RPN proposals. 71 | # You can reduce this during training to generate more propsals. 72 | RPN_NMS_THRESHOLD = 0.7 73 | 74 | # How many anchors per image to use for RPN training 75 | RPN_TRAIN_ANCHORS_PER_IMAGE = 256 76 | 77 | # ROIs kept after non-maximum supression (training and inference) 78 | POST_NMS_ROIS_TRAINING = 2000 79 | POST_NMS_ROIS_INFERENCE = 1000 80 | 81 | # If enabled, resizes instance masks to a smaller size to reduce 82 | # memory load. Recommended when using high-resolution images. 83 | USE_MINI_MASK = True 84 | MINI_MASK_SHAPE = (56, 56) # (height, width) of the mini-mask 85 | 86 | # Input image resing 87 | # Images are resized such that the smallest side is >= IMAGE_MIN_DIM and 88 | # the longest side is <= IMAGE_MAX_DIM. In case both conditions can't 89 | # be satisfied together the IMAGE_MAX_DIM is enforced. 90 | IMAGE_MIN_DIM = 800 91 | IMAGE_MAX_DIM = 1024 92 | # If True, pad images with zeros such that they're (max_dim by max_dim) 93 | IMAGE_PADDING = True # currently, the False option is not supported 94 | 95 | # Image mean (RGB) 96 | MEAN_PIXEL = np.array([123.7, 116.8, 103.9]) 97 | 98 | # Number of ROIs per image to feed to classifier/mask heads 99 | # The Mask RCNN paper uses 512 but often the RPN doesn't generate 100 | # enough positive proposals to fill this and keep a positive:negative 101 | # ratio of 1:3. You can increase the number of proposals by adjusting 102 | # the RPN NMS threshold. 103 | TRAIN_ROIS_PER_IMAGE = 200 104 | 105 | # Percent of positive ROIs used to train classifier/mask heads 106 | ROI_POSITIVE_RATIO = 0.33 107 | 108 | # Pooled ROIs 109 | POOL_SIZE = 7 110 | MASK_POOL_SIZE = 14 111 | MASK_SHAPE = [28, 28] 112 | 113 | # Maximum number of ground truth instances to use in one image 114 | MAX_GT_INSTANCES = 100 115 | 116 | # Bounding box refinement standard deviation for RPN and final detections. 117 | RPN_BBOX_STD_DEV = np.array([0.1, 0.1, 0.2, 0.2]) 118 | BBOX_STD_DEV = np.array([0.1, 0.1, 0.2, 0.2]) 119 | 120 | # Max number of final detections 121 | DETECTION_MAX_INSTANCES = 100 122 | 123 | # Minimum probability value to accept a detected instance 124 | # ROIs below this threshold are skipped 125 | DETECTION_MIN_CONFIDENCE = 0.7 126 | 127 | # Non-maximum suppression threshold for detection 128 | DETECTION_NMS_THRESHOLD = 0.3 129 | 130 | # Learning rate and momentum 131 | # The Mask RCNN paper uses lr=0.02, but on TensorFlow it causes 132 | # weights to explode. Likely due to differences in optimzer 133 | # implementation. 134 | LEARNING_RATE = 0.001 135 | LEARNING_MOMENTUM = 0.9 136 | 137 | # Weight decay regularization 138 | WEIGHT_DECAY = 0.0001 139 | 140 | # Use RPN ROIs or externally generated ROIs for training 141 | # Keep this True for most situations. Set to False if you want to train 142 | # the head branches on ROI generated by code rather than the ROIs from 143 | # the RPN. For example, to debug the classifier head without having to 144 | # train the RPN. 145 | USE_RPN_ROIS = True 146 | 147 | def __init__(self): 148 | """Set values of computed attributes.""" 149 | # Effective batch size 150 | self.BATCH_SIZE = self.IMAGES_PER_GPU * self.GPU_COUNT 151 | 152 | # Input image size 153 | self.IMAGE_SHAPE = np.array( 154 | [self.IMAGE_MAX_DIM, self.IMAGE_MAX_DIM, 3]) 155 | 156 | # Compute backbone size from input image size 157 | self.BACKBONE_SHAPES = np.array( 158 | [[int(math.ceil(self.IMAGE_SHAPE[0] / stride)), 159 | int(math.ceil(self.IMAGE_SHAPE[1] / stride))] 160 | for stride in self.BACKBONE_STRIDES]) 161 | 162 | def display(self): 163 | """Display Configuration values.""" 164 | print("\nConfigurations:") 165 | for a in dir(self): 166 | if not a.startswith("__") and not callable(getattr(self, a)): 167 | print("{:30} {}".format(a, getattr(self, a))) 168 | print("\n") 169 | -------------------------------------------------------------------------------- /src/detect/yolo4/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/detect/yolo4/__init__.py -------------------------------------------------------------------------------- /src/detect/yolo4/utils.py: -------------------------------------------------------------------------------- 1 | """Miscellaneous utility functions.""" 2 | 3 | from functools import reduce 4 | 5 | from PIL import Image 6 | import numpy as np 7 | from matplotlib.colors import rgb_to_hsv, hsv_to_rgb 8 | 9 | def compose(*funcs): 10 | """Compose arbitrarily many functions, evaluated left to right. 11 | 12 | Reference: https://mathieularose.com/function-composition-in-python/ 13 | """ 14 | # return lambda x: reduce(lambda v, f: f(v), funcs, x) 15 | if funcs: 16 | return reduce(lambda f, g: lambda *a, **kw: g(f(*a, **kw)), funcs) 17 | else: 18 | raise ValueError('Composition of empty sequence not supported.') 19 | 20 | def letterbox_image(image, size): 21 | '''resize image with unchanged aspect ratio using padding''' 22 | iw, ih = image.size 23 | w, h = size 24 | scale = min(w/iw, h/ih) 25 | nw = int(iw*scale) 26 | nh = int(ih*scale) 27 | 28 | image = image.resize((nw,nh), Image.BICUBIC) 29 | new_image = Image.new('RGB', size, (128,128,128)) 30 | new_image.paste(image, ((w-nw)//2, (h-nh)//2)) 31 | return new_image 32 | 33 | def rand(a=0, b=1): 34 | return np.random.rand()*(b-a) + a 35 | 36 | def get_random_data(annotation_line, input_shape, random=True, max_boxes=100, jitter=.3, hue=.1, sat=1.5, val=1.5, proc_img=True): 37 | '''random preprocessing for real-time data augmentation''' 38 | line = annotation_line.split() 39 | image = Image.open(line[0]) 40 | iw, ih = image.size 41 | h, w = input_shape 42 | box = np.array([np.array(list(map(int,box.split(',')))) for box in line[1:]]) 43 | 44 | if not random: 45 | # resize image 46 | scale = min(w/iw, h/ih) 47 | nw = int(iw*scale) 48 | nh = int(ih*scale) 49 | dx = (w-nw)//2 50 | dy = (h-nh)//2 51 | image_data=0 52 | if proc_img: 53 | image = image.resize((nw,nh), Image.BICUBIC) 54 | new_image = Image.new('RGB', (w,h), (128,128,128)) 55 | new_image.paste(image, (dx, dy)) 56 | image_data = np.array(new_image)/255. 57 | 58 | # correct boxes 59 | box_data = np.zeros((max_boxes,5)) 60 | if len(box)>0: 61 | np.random.shuffle(box) 62 | if len(box)>max_boxes: box = box[:max_boxes] 63 | box[:, [0,2]] = box[:, [0,2]]*scale + dx 64 | box[:, [1,3]] = box[:, [1,3]]*scale + dy 65 | box_data[:len(box)] = box 66 | 67 | return image_data, box_data 68 | 69 | # resize image 70 | new_ar = w/h * rand(1-jitter,1+jitter)/rand(1-jitter,1+jitter) 71 | scale = rand(.25, 2) 72 | if new_ar < 1: 73 | nh = int(scale*h) 74 | nw = int(nh*new_ar) 75 | else: 76 | nw = int(scale*w) 77 | nh = int(nw/new_ar) 78 | image = image.resize((nw,nh), Image.BICUBIC) 79 | 80 | # place image 81 | dx = int(rand(0, w-nw)) 82 | dy = int(rand(0, h-nh)) 83 | new_image = Image.new('RGB', (w,h), (128,128,128)) 84 | new_image.paste(image, (dx, dy)) 85 | image = new_image 86 | 87 | # flip image or not 88 | flip = rand()<.5 89 | if flip: image = image.transpose(Image.FLIP_LEFT_RIGHT) 90 | 91 | # distort image 92 | hue = rand(-hue, hue) 93 | sat = rand(1, sat) if rand()<.5 else 1/rand(1, sat) 94 | val = rand(1, val) if rand()<.5 else 1/rand(1, val) 95 | x = rgb_to_hsv(np.array(image)/255.) 96 | x[..., 0] += hue 97 | x[..., 0][x[..., 0]>1] -= 1 98 | x[..., 0][x[..., 0]<0] += 1 99 | x[..., 1] *= sat 100 | x[..., 2] *= val 101 | x[x>1] = 1 102 | x[x<0] = 0 103 | image_data = hsv_to_rgb(x) # numpy array, 0 to 1 104 | 105 | # correct boxes 106 | box_data = np.zeros((max_boxes,5)) 107 | if len(box)>0: 108 | np.random.shuffle(box) 109 | box[:, [0,2]] = box[:, [0,2]]*nw/iw + dx 110 | box[:, [1,3]] = box[:, [1,3]]*nh/ih + dy 111 | if flip: box[:, [0,2]] = w - box[:, [2,0]] 112 | box[:, 0:2][box[:, 0:2]<0] = 0 113 | box[:, 2][box[:, 2]>w] = w 114 | box[:, 3][box[:, 3]>h] = h 115 | box_w = box[:, 2] - box[:, 0] 116 | box_h = box[:, 3] - box[:, 1] 117 | box = box[np.logical_and(box_w>1, box_h>1)] # discard invalid box 118 | if len(box)>max_boxes: box = box[:max_boxes] 119 | box_data[:len(box)] = box 120 | 121 | return image_data, box_data 122 | -------------------------------------------------------------------------------- /src/detect/yolo4/yolo.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Class definition of YOLO_v3 style detection model on image and video 4 | """ 5 | 6 | import colorsys 7 | import os 8 | from timeit import default_timer as timer 9 | 10 | import numpy as np 11 | from keras import backend as K 12 | from keras.models import load_model 13 | from keras.layers import Input 14 | from PIL import Image, ImageFont, ImageDraw 15 | import tensorflow as tf 16 | from .model import yolo_eval, yolo4_body 17 | from .utils import letterbox_image 18 | import os 19 | from keras.utils import multi_gpu_model 20 | 21 | class YOLO(object): 22 | _defaults = { 23 | "model_path": 'model_data/yolo4_weights.h5', 24 | "anchors_path": 'model_data/yolo_anchors.txt', 25 | "classes_path": 'model_data/coco_classes.txt', 26 | "score" : 0.5, 27 | "iou" : 0.5, 28 | "model_image_size" : (608, 608), 29 | "gpu_num" : 1, 30 | } 31 | 32 | @classmethod 33 | def get_defaults(cls, n): 34 | if n in cls._defaults: 35 | return cls._defaults[n] 36 | else: 37 | return "Unrecognized attribute name '" + n + "'" 38 | 39 | def __init__(self, **kwargs): 40 | self.__dict__.update(self._defaults) # set up default values 41 | self.__dict__.update(kwargs) # and update with user overrides 42 | self.class_names = self._get_class() 43 | self.anchors = self._get_anchors() 44 | # tf.reset_default_graph() 45 | # config = tf.ConfigProto(allow_soft_placement=True,\ 46 | # device_count={'GPU':self.gpu_num,'CPU':1}) 47 | # session = tf.Session(config=config) 48 | # session.run(tf.global_variables_initializer())#init, must put ahead of keras 49 | # self.sess = K.set_session(session) 50 | self.sess = K.get_session() 51 | self.boxes, self.scores, self.classes = self.generate() 52 | 53 | def _get_class(self): 54 | classes_path = os.path.expanduser(self.classes_path) 55 | with open(classes_path) as f: 56 | class_names = f.readlines() 57 | class_names = [c.strip() for c in class_names] 58 | return class_names 59 | 60 | def _get_anchors(self): 61 | anchors_path = os.path.expanduser(self.anchors_path) 62 | with open(anchors_path) as f: 63 | anchors = f.readline() 64 | anchors = [float(x) for x in anchors.split(',')] 65 | return np.array(anchors).reshape(-1, 2) 66 | 67 | def generate(self): 68 | model_path = os.path.expanduser(self.model_path) 69 | assert model_path.endswith('.h5'), 'Keras model or weights must be a .h5 file.' 70 | # Load model, or construct model and load weights. 71 | num_anchors = len(self.anchors) 72 | num_classes = len(self.class_names) 73 | try: 74 | self.yolo4_model = yolo4_body(Input(shape=(608, 608, 3)), num_anchors//3, num_classes) 75 | self.yolo4_model.load_weights(model_path) 76 | except Exception as e: 77 | print('load model failed', e) 78 | print('{} model, anchors, and classes loaded.'.format(model_path)) 79 | 80 | # Generate colors for drawing bounding boxes. 81 | hsv_tuples = [(x / len(self.class_names), 1., 1.) 82 | for x in range(len(self.class_names))] 83 | self.colors = list(map(lambda x: colorsys.hsv_to_rgb(*x), hsv_tuples)) 84 | self.colors = list( 85 | map(lambda x: (int(x[0] * 255), int(x[1] * 255), int(x[2] * 255)), 86 | self.colors)) 87 | np.random.seed(10101) # Fixed seed for consistent colors across runs. 88 | np.random.shuffle(self.colors) # Shuffle colors to decorrelate adjacent classes. 89 | np.random.seed(None) # Reset seed to default. 90 | # Generate output tensor targets for filtered bounding boxes. 91 | self.input_image_shape = K.placeholder(shape=(2, )) 92 | if self.gpu_num>=2: 93 | self.yolo4_model = multi_gpu_model(self.yolo4_model, gpus=self.gpu_num) 94 | boxes, scores, classes = yolo_eval(self.yolo4_model.output, self.anchors, 95 | len(self.class_names), self.input_image_shape, 96 | score_threshold=self.score, iou_threshold=self.iou) 97 | return boxes, scores, classes 98 | 99 | def close_session(self): 100 | self.sess.close() 101 | 102 | def detect_image(self, image, model_image_size=(608,608)): 103 | #start = timer() 104 | 105 | boxed_image = letterbox_image(image, tuple(reversed(model_image_size))) 106 | image_data = np.array(boxed_image, dtype='float32') 107 | 108 | #print(image_data.shape) 109 | image_data /= 255. 110 | image_data = np.expand_dims(image_data, 0) # Add batch dimension. 111 | 112 | out_boxes, out_scores, out_classes = self.sess.run( 113 | [self.boxes, self.scores, self.classes], 114 | feed_dict={ 115 | self.yolo4_model.input: image_data, 116 | self.input_image_shape: [image.size[1], image.size[0]], 117 | K.learning_phase(): 0 118 | }) 119 | 120 | #print('Found {} boxes for {}'.format(len(out_boxes), 'img')) 121 | 122 | out_valid_boxes = [] 123 | out_valid_scores = [] 124 | out_valid_classes = [] 125 | for i, c in reversed(list(enumerate(out_classes))): 126 | # predicted_class = self.class_names[c] 127 | box = out_boxes[i] 128 | score = out_scores[i] 129 | 130 | #label = '{} {:.2f}'.format(predicted_class, score) 131 | top, left, bottom, right = box 132 | top = max(0, np.floor(top + 0.5).astype('int32')) 133 | left = max(0, np.floor(left + 0.5).astype('int32')) 134 | bottom = min(image.size[1], np.floor(bottom + 0.5).astype('int32')) 135 | right = min(image.size[0], np.floor(right + 0.5).astype('int32')) 136 | 137 | out_valid_boxes.append((top, left, bottom, right)) 138 | out_valid_scores.append(score) 139 | out_valid_classes.append(c) 140 | 141 | out_valid_boxes = np.asarray(out_valid_boxes) 142 | #end = timer() 143 | return out_valid_boxes,np.asarray(out_valid_classes), np.asarray(out_valid_scores) 144 | 145 | -------------------------------------------------------------------------------- /src/detect/yolo4/yolo_anchors.txt: -------------------------------------------------------------------------------- 1 | 10,13, 16,30, 33,23, 30,61, 62,45, 59,119, 116,90, 156,198, 373,326 2 | -------------------------------------------------------------------------------- /src/error_check.py: -------------------------------------------------------------------------------- 1 | #error checking 2 | import numpy as np 3 | low = np.s_[...,:2] 4 | high = np.s_[...,2:] 5 | 6 | def iou(A,B): 7 | A,B = A.copy(),B.copy() 8 | A[high] += 1; B[high] += 1 9 | intrs = (np.maximum(0,np.minimum(A[high],B[high]) 10 | -np.maximum(A[low],B[low]))).prod(-1) 11 | return intrs / ((A[high]-A[low]).prod(-1)+(B[high]-B[low]).prod(-1)-intrs) 12 | 13 | def single_frame_check(objectlist, ov_thresh=0.7): 14 | if len(objectlist)<=1: #only 1 object is given 15 | return 1 16 | #1. duplicate id 17 | ids = [item[0] for item in objectlist] 18 | unq_ids = list(set(ids)) 19 | if len(ids)!=len(unq_ids): 20 | return 0 21 | #2. too overlapped 22 | boxes = [(item[2], item[3], item[4], item[5]) for item in objectlist] 23 | boxes = np.asarray(boxes) 24 | 25 | ovs = iou(boxes[:,None],boxes[None]) 26 | N = boxes.shape[0] 27 | 28 | max_ov = max(np.asarray([ovs[i,j] for i in range(N) for j in range(i+1, N)])) 29 | if max_ov > ov_thresh: 30 | return 0 31 | 32 | #others:? type consitencty across the frames? 33 | #center points->trjactory too close 34 | return 1 35 | 36 | #return: 37 | #1: pass validatoin test;0: fail in validation test 38 | #-1: not valid data provided(empty arr) 39 | def multi_frame_error_check(objectList): 40 | valid_arr = [] 41 | for frame_data in objectList: 42 | if len(frame_data)>0: 43 | valid = single_frame_check(frame_data) 44 | else: 45 | valid = -1 46 | valid_arr.append(valid) 47 | return valid_arr -------------------------------------------------------------------------------- /src/road/PINet/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/road/PINet/__init__.py -------------------------------------------------------------------------------- /src/road/PINet/evaluation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from sklearn.linear_model import LinearRegression 3 | import ujson as json 4 | 5 | 6 | class LaneEval(object): 7 | lr = LinearRegression() 8 | pixel_thresh = 20 9 | pt_thresh = 0.85 10 | 11 | @staticmethod 12 | def get_angle(xs, y_samples): 13 | xs, ys = xs[xs >= 0], y_samples[xs >= 0] 14 | if len(xs) > 1: 15 | LaneEval.lr.fit(ys[:, None], xs) 16 | k = LaneEval.lr.coef_[0] 17 | theta = np.arctan(k) 18 | else: 19 | theta = 0 20 | return theta 21 | 22 | @staticmethod 23 | def line_accuracy(pred, gt, thresh): 24 | pred = np.array([p if p >= 0 else -100 for p in pred]) 25 | gt = np.array([g if g >= 0 else -100 for g in gt]) 26 | return np.sum(np.where(np.abs(pred - gt) < thresh, 1., 0.)) / len(gt) 27 | 28 | @staticmethod 29 | def bench(pred, gt, y_samples, running_time): 30 | if any(len(p) != len(y_samples) for p in pred): 31 | raise Exception('Format of lanes error.') 32 | if running_time > 200 or len(gt) + 2 < len(pred): 33 | return 0., 0., 1. 34 | angles = [LaneEval.get_angle(np.array(x_gts), np.array(y_samples)) for x_gts in gt] 35 | threshs = [LaneEval.pixel_thresh / np.cos(angle) for angle in angles] 36 | line_accs = [] 37 | fp, fn = 0., 0. 38 | matched = 0. 39 | for x_gts, thresh in zip(gt, threshs): 40 | accs = [LaneEval.line_accuracy(np.array(x_preds), np.array(x_gts), thresh) for x_preds in pred] 41 | max_acc = np.max(accs) if len(accs) > 0 else 0. 42 | if max_acc < LaneEval.pt_thresh: 43 | fn += 1 44 | else: 45 | matched += 1 46 | line_accs.append(max_acc) 47 | fp = len(pred) - matched 48 | if len(gt) > 4 and fn > 0: 49 | fn -= 1 50 | s = sum(line_accs) 51 | if len(gt) > 4: 52 | s -= min(line_accs) 53 | return s / max(min(4.0, len(gt)), 1.), fp / len(pred) if len(pred) > 0 else 0., fn / max(min(len(gt), 4.) , 1.) 54 | 55 | @staticmethod 56 | def bench_one_submit(pred_file, gt_file): 57 | try: 58 | json_pred = [json.loads(line) for line in open(pred_file).readlines()] 59 | except BaseException as e: 60 | raise Exception('Fail to load json file of the prediction.') 61 | json_gt = [json.loads(line) for line in open(gt_file).readlines()] 62 | if len(json_gt) != len(json_pred): 63 | raise Exception('We do not get the predictions of all the test tasks') 64 | gts = {l['raw_file']: l for l in json_gt} 65 | accuracy, fp, fn = 0., 0., 0. 66 | for pred in json_pred: 67 | if 'raw_file' not in pred or 'lanes' not in pred or 'run_time' not in pred: 68 | raise Exception('raw_file or lanes or run_time not in some predictions.') 69 | raw_file = pred['raw_file'] 70 | pred_lanes = pred['lanes'] 71 | run_time = pred['run_time'] 72 | if raw_file not in gts: 73 | raise Exception('Some raw_file from your predictions do not exist in the test tasks.') 74 | gt = gts[raw_file] 75 | gt_lanes = gt['lanes'] 76 | y_samples = gt['h_samples'] 77 | try: 78 | a, p, n = LaneEval.bench(pred_lanes, gt_lanes, y_samples, run_time) 79 | except BaseException as e: 80 | raise Exception('Format of lanes error.') 81 | accuracy += a 82 | fp += p 83 | fn += n 84 | num = len(gts) 85 | # the first return parameter is the default ranking parameter 86 | return json.dumps([ 87 | {'name': 'Accuracy', 'value': accuracy / num, 'order': 'desc'}, 88 | {'name': 'FP', 'value': fp / num, 'order': 'asc'}, 89 | {'name': 'FN', 'value': fn / num, 'order': 'asc'} 90 | ]) 91 | 92 | 93 | if __name__ == '__main__': 94 | import sys 95 | print(LaneEval.bench_one_submit("test_result.json", "test_label.json")) 96 | -------------------------------------------------------------------------------- /src/road/PINet/fix_dataset.py: -------------------------------------------------------------------------------- 1 | ######################################################################### 2 | ## 3 | ## Data loader source code for TuSimple dataset 4 | ## 5 | ######################################################################### 6 | 7 | 8 | import math 9 | import numpy as np 10 | import cv2 11 | import matplotlib.pyplot as plt 12 | #from skimage.transform import rotate as rotate_ 13 | import json 14 | import random 15 | from copy import deepcopy 16 | from parameters import Parameters 17 | import os 18 | 19 | ######################################################################### 20 | ## Data loader class 21 | ######################################################################### 22 | class Generator(object): 23 | ################################################################################ 24 | ## initialize (load data set from url) 25 | ################################################################################ 26 | def __init__(self): 27 | self.p = Parameters() 28 | 29 | # load annotation data (training set) 30 | self.train_data = [] 31 | self.test_data = [] 32 | filer = os.path.join(self.p.train_root_url,'label_data_0313.json') 33 | with open(filer) as f: 34 | while True: 35 | line = f.readline() 36 | if not line: 37 | break 38 | jsonString = json.loads(line) 39 | self.train_data.append(jsonString) 40 | 41 | random.shuffle(self.train_data) 42 | filename = os.path.join(self.p.train_root_url,'label_data_0531.json') 43 | with open(filename) as f: 44 | while True: 45 | line = f.readline() 46 | if not line: 47 | break 48 | jsonString = json.loads(line) 49 | self.train_data.append(jsonString) 50 | 51 | random.shuffle(self.train_data) 52 | filename = os.path.join(self.p.train_root_url,'label_data_0601.json') 53 | with open(filename) as f: 54 | while True: 55 | line = f.readline() 56 | if not line: 57 | break 58 | jsonString = json.loads(line) 59 | self.train_data.append(jsonString) 60 | 61 | random.shuffle(self.train_data) 62 | 63 | self.size_train = len(self.train_data) 64 | print(self.size_train) 65 | 66 | # load annotation data (test set) 67 | #with open(self.p.test_root_url+'test_tasks_0627.json') as f: 68 | filename = os.path.join(self.p.test_root_url,'test_label.json') 69 | 70 | with open(filename) as f: 71 | while True: 72 | line = f.readline() 73 | if not line: 74 | break 75 | jsonString = json.loads(line) 76 | self.test_data.append(jsonString) 77 | 78 | #random.shuffle(self.test_data) 79 | 80 | self.size_test = len(self.test_data) 81 | print(self.size_test) 82 | 83 | def split(self): 84 | one = 0 85 | two = 0 86 | three = 0 87 | four = 0 88 | five = 0 89 | six = 0 90 | for i in range(self.size_train): 91 | if len(self.train_data[i]['lanes']) == 6: 92 | with open("dataset/six.json", 'a') as make_file: 93 | six += 1 94 | json.dump(self.train_data[i], make_file, separators=(',', ': ')) 95 | make_file.write("\n") 96 | if len(self.train_data[i]['lanes']) == 5: 97 | with open("dataset/five.json", 'a') as make_file: 98 | five += 1 99 | json.dump(self.train_data[i], make_file, separators=(',', ': ')) 100 | make_file.write("\n") 101 | if len(self.train_data[i]['lanes']) == 4: 102 | with open("dataset/four.json", 'a') as make_file: 103 | four += 1 104 | json.dump(self.train_data[i], make_file, separators=(',', ': ')) 105 | make_file.write("\n") 106 | if len(self.train_data[i]['lanes']) == 3: 107 | with open("dataset/three.json", 'a') as make_file: 108 | three += 1 109 | json.dump(self.train_data[i], make_file, separators=(',', ': ')) 110 | make_file.write("\n") 111 | if len(self.train_data[i]['lanes']) == 2: 112 | with open("dataset/two.json", 'a') as make_file: 113 | two += 1 114 | json.dump(self.train_data[i], make_file, separators=(',', ': ')) 115 | make_file.write("\n") 116 | if len(self.train_data[i]['lanes']) == 1: 117 | with open("dataset/one.json", 'a') as make_file: 118 | one += 1 119 | json.dump(self.train_data[i], make_file, separators=(',', ': ')) 120 | make_file.write("\n") 121 | print("six = " + str(six)) 122 | print("five = " + str(five)) 123 | print("four = " + str(four)) 124 | print("three = " + str(three)) 125 | print("two = " + str(two)) 126 | print("one = " + str(one)) 127 | print("total = " + str(one+two+three+four+five+six)) 128 | 129 | 130 | if __name__ == '__main__': 131 | G = Generator() 132 | G.split() -------------------------------------------------------------------------------- /src/road/PINet/hard_sampling.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | 3 | class hard_sampling(): 4 | def __init__(self): 5 | self.total_num = 0 6 | self.first_node = None 7 | self.last_node = None 8 | self.minimum_loss = 10000 9 | self.maximum_size = 5000 10 | 11 | def insert(self, node): 12 | if self.total_num < 1: 13 | self.total_num += 1 14 | self.first_node = node 15 | self.last_node = node 16 | self.minimum_loss = node.get_loss() 17 | else: 18 | target_loss = node.get_loss() 19 | if self.minimum_loss < target_loss or self.total_num < self.maximum_size: 20 | if self.first_node.get_loss() < target_loss: 21 | self.total_num += 1 22 | node.set_next(self.first_node) 23 | self.first_node.set_previous(node) 24 | self.first_node = node 25 | else: 26 | current_node = self.first_node 27 | while True: 28 | if current_node.get_loss() >= target_loss and current_node.get_next() == None: 29 | self.total_num += 1 30 | node.set_previous(current_node) 31 | current_node.set_next(node) 32 | self.last_node = node 33 | self.minimum_loss = target_loss 34 | break 35 | if current_node.get_loss() >= target_loss and target_loss >= current_node.get_next().get_loss(): 36 | self.total_num += 1 37 | node.set_previous(current_node) 38 | node.set_next(current_node.get_next()) 39 | current_node.get_next().set_previous(node) 40 | current_node.set_next(node) 41 | break 42 | current_node = current_node.get_next() 43 | if current_node == None: 44 | break 45 | if self.total_num > self.maximum_size: 46 | self.total_num -= 1 47 | self.minimum_loss = self.last_node.get_previous().get_loss() 48 | self.last_node = self.last_node.get_previous() 49 | 50 | def get_list(self): 51 | data_list = [] 52 | current_node = self.first_node 53 | while True: 54 | data_list.append(current_node.get_data()) 55 | current_node = current_node.get_next() 56 | if current_node == None: 57 | break 58 | return data_list 59 | 60 | def get_num(self): 61 | return self.total_num 62 | 63 | class sampling_node(): 64 | def __init__(self, loss = 10000, data = None, previous_node = None, next_node = None): 65 | self.loss = loss 66 | self.data = data 67 | self.previous_node = previous_node 68 | self.next_node = next_node 69 | 70 | def set_previous(self, previous_node): 71 | self.previous_node = previous_node 72 | 73 | def set_next(self, next_node): 74 | self.next_node = next_node 75 | 76 | def set_loss(self, loss): 77 | self.loss = loss 78 | 79 | def set_data(self, data): 80 | self.data = data 81 | 82 | def get_previous(self): 83 | return self.previous_node 84 | 85 | def get_next(self): 86 | return self.next_node 87 | 88 | def get_loss(self): 89 | return self.loss 90 | 91 | def get_data(self): 92 | return self.data 93 | -------------------------------------------------------------------------------- /src/road/PINet/hourglass_network.py: -------------------------------------------------------------------------------- 1 | ######################################################################### 2 | ## 3 | ## Structure of network. 4 | ## 5 | ######################################################################### 6 | import torch 7 | import torch.nn as nn 8 | from src.road.PINet.util_hourglass import * 9 | 10 | #################################################################### 11 | ## 12 | ## lane_detection_network 13 | ## 14 | #################################################################### 15 | class lane_detection_network(nn.Module): 16 | def __init__(self): 17 | super(lane_detection_network, self).__init__() 18 | 19 | self.resizing = resize_layer(3, 128) 20 | 21 | #feature extraction 22 | self.layer1 = hourglass_block(128, 128) 23 | self.layer2 = hourglass_block(128, 128) 24 | self.layer3 = hourglass_block(128, 128) 25 | self.layer4 = hourglass_block(128, 128) 26 | 27 | 28 | def forward(self, inputs): 29 | #feature extraction 30 | out = self.resizing(inputs) 31 | result1, out, feature1 = self.layer1(out) 32 | result2, out, feature2 = self.layer2(out) 33 | result3, out, feature3 = self.layer3(out) 34 | result4, out, feature4 = self.layer4(out) 35 | 36 | return [result1, result2, result3, result4], [feature1, feature2, feature3, feature4] 37 | #return [result2], [feature2] 38 | -------------------------------------------------------------------------------- /src/road/PINet/parameters.py: -------------------------------------------------------------------------------- 1 | ############################################################################################################# 2 | ## 3 | ## Parameters 4 | ## 5 | ############################################################################################################# 6 | import numpy as np 7 | 8 | class Parameters(): 9 | n_epoch = 1000 10 | l_rate = 0.001 11 | weight_decay=1e-5 12 | save_path = "savefile/" 13 | model_path = "savefile/" 14 | batch_size = 12 15 | x_size = 512 16 | y_size = 256 17 | resize_ratio = 8 18 | grid_x = x_size//resize_ratio #64 19 | grid_y = y_size//resize_ratio #32 20 | feature_size = 4 21 | regression_size = 110 22 | mode = 1#2#3 23 | threshold_point = 0.96 #0.88 #0.93 #0.95 #0.93 24 | threshold_instance = 0.08 25 | 26 | #loss function parameter 27 | K1 = 1.0 # #################################### 28 | K2 = 2.0 29 | constant_offset = 0.2 30 | constant_exist = 2.5 #2 31 | constant_nonexist = 1.0 32 | constant_angle = 1.0 33 | constant_similarity = 1.0 34 | constant_attention = 0.01 35 | constant_alpha = 0.5 #in SGPN paper, they increase this factor by 2 every 5 epochs 36 | constant_beta = 0.5 37 | constant_l = 1.0 38 | constant_lane_loss = 1.0 #10 ###################################### 39 | constant_instance_loss = 1.0 40 | 41 | #data loader parameter 42 | flip_ratio=0.6 43 | translation_ratio=0.6 44 | rotate_ratio=0.6 45 | noise_ratio=0.6 46 | intensity_ratio=0.6 47 | shadow_ratio=0.6 48 | 49 | train_root_url="/home/lidanzha/data/lane/culane/train_set/" 50 | test_root_url="/home/lidanzha/data/lane/culane/test_set/" 51 | 52 | # test parameter 53 | color = [(0,0,0), (255,0,0), (0,255,0),(0,0,255),(255,255,0),(255,0,255),(0,255,255),(255,255,255),(100,255,0),(100,0,255),(255,100,0),(0,100,255),(255,0,100),(0,255,100)] 54 | grid_location = np.zeros((grid_y, grid_x, 2)) 55 | for y in range(grid_y): 56 | for x in range(grid_x): 57 | grid_location[y][x][0] = x 58 | grid_location[y][x][1] = y 59 | num_iter = 30 60 | threshold_RANSAC = 0.1 61 | ratio_inliers = 0.1 62 | -------------------------------------------------------------------------------- /src/road/PINet/train.py: -------------------------------------------------------------------------------- 1 | ############################################################################################################# 2 | ## 3 | ## Source code for training. In this source code, there are initialize part, training part, ... 4 | ## 5 | ############################################################################################################# 6 | 7 | import cv2 8 | import torch 9 | import visdom 10 | #import sys 11 | #sys.path.append('/home/kym/research/autonomous_car_vision/lanedection/code/') 12 | import agent 13 | import numpy as np 14 | from data_loader import Generator 15 | from parameters import Parameters 16 | import test 17 | import evaluation 18 | import util 19 | import os 20 | import copy 21 | 22 | p = Parameters() 23 | 24 | ############################################################### 25 | ## 26 | ## Training 27 | ## 28 | ############################################################### 29 | def Training(): 30 | print('Training') 31 | 32 | #################################################################### 33 | ## Hyper parameter 34 | #################################################################### 35 | print('Initializing hyper parameter') 36 | 37 | vis = visdom.Visdom() 38 | loss_window = vis.line(X=torch.zeros((1,)).cpu(), 39 | Y=torch.zeros((1)).cpu(), 40 | opts=dict(xlabel='epoch', 41 | ylabel='Loss', 42 | title='Training Loss', 43 | legend=['Loss'])) 44 | 45 | ######################################################################### 46 | ## Get dataset 47 | ######################################################################### 48 | print("Get dataset") 49 | loader = Generator() 50 | 51 | ############################## 52 | ## Get agent and model 53 | ############################## 54 | print('Get agent') 55 | 56 | if p.model_path == "": 57 | lane_agent = agent.Agent() 58 | else: 59 | lane_agent = agent.Agent() 60 | lane_agent.load_weights(0, "tensor(1.3984)") 61 | 62 | ############################## 63 | ## Check GPU 64 | ############################## 65 | print('Setup GPU mode') 66 | if torch.cuda.is_available(): 67 | lane_agent.cuda() 68 | #torch.backends.cudnn.benchmark=True 69 | 70 | ############################## 71 | ## Loop for training 72 | ############################## 73 | print('Training loop') 74 | step = 0 75 | sampling_list = None 76 | for epoch in range(p.n_epoch): 77 | lane_agent.training_mode() 78 | for inputs, target_lanes, target_h, test_image, data_list in loader.Generate(sampling_list): 79 | 80 | #util.visualize_points(inputs[0], target_lanes[0], target_h[0]) 81 | #training 82 | print("epoch : " + str(epoch)) 83 | print("step : " + str(step)) 84 | loss_p = lane_agent.train(inputs, target_lanes, target_h, epoch, lane_agent, data_list) 85 | torch.cuda.synchronize() 86 | loss_p = loss_p.cpu().data 87 | 88 | if step%50 == 0: 89 | vis.line( 90 | X=torch.ones((1, 1)).cpu() * int(step/50), 91 | Y=torch.Tensor([loss_p]).unsqueeze(0).cpu(), 92 | win=loss_window, 93 | update='append') 94 | 95 | if step%100 == 0: 96 | lane_agent.save_model(int(step/100), loss_p) 97 | testing(lane_agent, test_image, step, loss_p) 98 | step += 1 99 | 100 | sampling_list = copy.deepcopy(lane_agent.get_data_list()) 101 | lane_agent.sample_reset() 102 | 103 | #evaluation 104 | if epoch%1 == 0: 105 | print("evaluation") 106 | lane_agent.evaluate_mode() 107 | th_list = [0.9] 108 | index = [3] 109 | lane_agent.save_model(int(step/100), loss_p) 110 | 111 | for idx in index: 112 | print("generate result") 113 | test.evaluation(loader, lane_agent, index = idx, name="test_result_"+str(epoch)+"_"+str(idx)+".json") 114 | name = "epoch_idx_"+str(epoch) + str(idx) + str(step/100) 115 | os.system("sh /home/kym/research/autonomous_car_vision/lane_detection/code/ITS/CuLane/evaluation_code/SCNN_Pytorch/utils/lane_evaluation/CULane/Run.sh " + name) 116 | 117 | if int(step)>700000: 118 | break 119 | 120 | 121 | def testing(lane_agent, test_image, step, loss): 122 | lane_agent.evaluate_mode() 123 | 124 | _, _, ti = test.test(lane_agent, np.array([test_image])) 125 | 126 | cv2.imwrite('test_result/result_'+str(step)+'_'+str(loss)+'.png', ti[0]) 127 | 128 | lane_agent.training_mode() 129 | 130 | 131 | if __name__ == '__main__': 132 | Training() 133 | 134 | -------------------------------------------------------------------------------- /src/road/PINet/util.py: -------------------------------------------------------------------------------- 1 | import torch.nn as nn 2 | import cv2 3 | import torch 4 | from copy import deepcopy 5 | import numpy as np 6 | from torch.autograd import Variable 7 | from torch.autograd import Function as F 8 | from src.road.PINet.parameters import Parameters 9 | import math 10 | 11 | p = Parameters() 12 | 13 | ############################################################### 14 | ## 15 | ## visualize 16 | ## 17 | ############################################################### 18 | 19 | def visualize_points(image, x, y): 20 | image = image 21 | image = np.rollaxis(image, axis=2, start=0) 22 | image = np.rollaxis(image, axis=2, start=0)#*255.0 23 | image = image.astype(np.uint8).copy() 24 | 25 | for k in range(len(y)): 26 | for i, j in zip(x[k], y[k]): 27 | if i > 0: 28 | image = cv2.circle(image, (int(i), int(j)), 2, p.color[1], -1) 29 | 30 | cv2.imshow("test2", image) 31 | cv2.waitKey(0) 32 | 33 | def visualize_points_origin_size(x, y, test_image, ratio_w, ratio_h): 34 | color = 0 35 | image = deepcopy(test_image) 36 | image = np.rollaxis(image, axis=2, start=0) 37 | image = np.rollaxis(image, axis=2, start=0)#*255.0 38 | image = image.astype(np.uint8).copy() 39 | 40 | image = cv2.resize(image, (int(p.x_size/ratio_w), int(p.y_size/ratio_h))) 41 | 42 | for i, j in zip(x, y): 43 | color += 1 44 | for index in range(len(i)): 45 | cv2.circle(image, (int(i[index]), int(j[index])), 10, p.color[color], -1) 46 | cv2.imshow("test2", image) 47 | cv2.waitKey(0) 48 | 49 | return test_image 50 | 51 | def visualize_gt(gt_point, gt_instance, ground_angle, image): 52 | image = np.rollaxis(image, axis=2, start=0) 53 | image = np.rollaxis(image, axis=2, start=0)#*255.0 54 | image = image.astype(np.uint8).copy() 55 | 56 | for y in range(p.grid_y): 57 | for x in range(p.grid_x): 58 | if gt_point[0][y][x] > 0: 59 | xx = int(gt_point[1][y][x]*p.resize_ratio+p.resize_ratio*x) 60 | yy = int(gt_point[2][y][x]*p.resize_ratio+p.resize_ratio*y) 61 | image = cv2.circle(image, (xx, yy), 10, p.color[1], -1) 62 | 63 | cv2.imshow("image", image) 64 | cv2.waitKey(0) 65 | 66 | def visualize_regression(image, gt): 67 | image = np.rollaxis(image, axis=2, start=0) 68 | image = np.rollaxis(image, axis=2, start=0)*255.0 69 | image = image.astype(np.uint8).copy() 70 | 71 | for i in gt: 72 | for j in range(p.regression_size):#gt 73 | y_value = p.y_size - (p.regression_size-j)*(220/p.regression_size) 74 | if i[j] >0: 75 | x_value = int(i[j]*p.x_size) 76 | image = cv2.circle(image, (x_value, y_value), 5, p.color[1], -1) 77 | cv2.imshow("image", image) 78 | cv2.waitKey(0) 79 | 80 | def draw_points(x, y, image): 81 | color_index = 0 82 | for i, j in zip(x, y): 83 | color_index += 1 84 | if color_index > 12: 85 | color_index = 12 86 | for index in range(len(i)): 87 | image = cv2.circle(image, (int(i[index]), int(j[index])), 5, p.color[color_index], -1) 88 | 89 | return image 90 | 91 | ############################################################### 92 | ## 93 | ## calculate 94 | ## 95 | ############################################################### 96 | def convert_to_original_size(x, y, ratio_w, ratio_h): 97 | # convert results to original size 98 | out_x = [] 99 | out_y = [] 100 | 101 | for i, j in zip(x,y): 102 | out_x.append((np.array(i)/ratio_w).tolist()) 103 | out_y.append((np.array(j)/ratio_h).tolist()) 104 | 105 | return out_x, out_y 106 | 107 | def get_closest_point_along_angle(x, y, point, angle): 108 | index = 0 109 | for i, j in zip(x, y): 110 | a = get_angle_two_points(point, (i,j)) 111 | if abs(a-angle) < 0.1: 112 | return (i, j), index 113 | index += 1 114 | return (-1, -1), -1 115 | 116 | 117 | def get_num_along_point(x, y, point1, point2, image=None): # point1 : source 118 | x = np.array(x) 119 | y = np.array(y) 120 | 121 | x = x[y 0 and c < zero_count * 0.1: 85 | high_val = u / 100.0 86 | break 87 | 88 | img_retinex[:, :, i] = np.maximum(np.minimum(img_retinex[:, :, i], high_val), low_val) 89 | 90 | img_retinex[:, :, i] = (img_retinex[:, :, i] - np.min(img_retinex[:, :, i])) / \ 91 | (np.max(img_retinex[:, :, i]) - np.min(img_retinex[:, :, i])) \ 92 | * 255 93 | 94 | img_retinex = np.uint8(img_retinex) 95 | 96 | return img_retinex 97 | 98 | def MSRCP(img, sigma_list, low_clip, high_clip): 99 | 100 | img = np.float64(img) + 1.0 101 | 102 | intensity = np.sum(img, axis=2) / img.shape[2] 103 | 104 | retinex = multiScaleRetinex(intensity, sigma_list) 105 | 106 | intensity = np.expand_dims(intensity, 2) 107 | retinex = np.expand_dims(retinex, 2) 108 | 109 | intensity1 = simplestColorBalance(retinex, low_clip, high_clip) 110 | 111 | intensity1 = (intensity1 - np.min(intensity1)) / \ 112 | (np.max(intensity1) - np.min(intensity1)) * \ 113 | 255.0 + 1.0 114 | 115 | img_msrcp = np.zeros_like(img) 116 | 117 | for y in range(img_msrcp.shape[0]): 118 | for x in range(img_msrcp.shape[1]): 119 | B = np.max(img[y, x]) 120 | A = np.minimum(256.0 / B, intensity1[y, x, 0] / intensity[y, x, 0]) 121 | img_msrcp[y, x, 0] = A * img[y, x, 0] 122 | img_msrcp[y, x, 1] = A * img[y, x, 1] 123 | img_msrcp[y, x, 2] = A * img[y, x, 2] 124 | 125 | img_msrcp = np.uint8(img_msrcp - 1.0) 126 | 127 | return img_msrcp 128 | -------------------------------------------------------------------------------- /src/road/road_main.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from src.ui.image_utils import convert_pixel_to_canvas, convert_canvas_to_pixel, normTransMat 3 | import src.road.GenerateCurRoad as Road 4 | from src.road.lane import sort_points,resample_points_yaxis, extend_road 5 | from src.convert_util import project_2d_to_3d_depth_arr 6 | import copy 7 | 8 | def initialize_bounary_points(image_raw, scale, pad): 9 | #currently use templates to initialzie 4 boundary points 10 | #TODO: auto detection road area and locate boundary poitns and detect lane number 11 | 12 | pts = [] 13 | w,h = image_raw.shape[1], image_raw.shape[0] 14 | width_gridsize = w/6 15 | height_gridsize = h/6 16 | pts.append([int(width_gridsize*3), int(height_gridsize*2)])#north 17 | pts.append([int(width_gridsize*2), int(height_gridsize*3)])#west 18 | pts.append([int(width_gridsize*3), int(height_gridsize*4)])#south 19 | pts.append([int(width_gridsize*4), int(height_gridsize*3)])#east 20 | canvas_pts = convert_pixel_to_canvas(pts, scale, pad) 21 | canvas_pts_dict ={'North': canvas_pts[0], 'West': canvas_pts[1], 'South':canvas_pts[2], 'East': canvas_pts[3]} 22 | return canvas_pts_dict 23 | 24 | def road_bound(camera_param, bound_pts_pixels): 25 | trsnsMat = camera_param.Mat_P[:, (0, 1, 3)] 26 | trsnsMat = np.asarray(trsnsMat, dtype=np.float64) 27 | Ex, Ey, Wx, Wy, Sx, Sy, Nx, Ny = 0, 0, 0, 0, 0, 0, 0, 0 28 | for order in ['North', 'South', 'East', 'West']: 29 | [x,y] = bound_pts_pixels[order] 30 | temp = normTransMat(np.dot(np.linalg.inv(trsnsMat), np.array([x, y, 1]).reshape((3, 1)))) 31 | if order == 'East': 32 | Ex=-temp[0][0]/1000 33 | elif order == 'West': 34 | Wx=-temp[0][0]/1000 35 | elif order == 'North': 36 | Ny=-temp[1][0]/1000 37 | Nx=-temp[0][0]/1000 38 | elif order == 'South': 39 | Sy=-temp[1][0]/1000 40 | Sx=-temp[0][0]/1000 41 | Ey = (Ny+Sy)/2 42 | Wy = (Ny+Sy) / 2 43 | 44 | return [[Ex,Ey],[Wx,Wy],[Nx,Ny],[Sx,Sy]] 45 | 46 | 47 | def generate_road(camera_param, bound_pts, lane_num, 48 | raw_image_shape, scale, pad, 49 | xodr_filename,roadpts_N,roadpts_W,roadpts_S,roadpts_E): 50 | #bound_pts, lan_num: dict, key=directions 51 | #need raw_image? -> need resize to predefined resolution? 52 | CALIBRATION_SHAPE=(1920,1080) 53 | width_scale = CALIBRATION_SHAPE[0]/raw_image_shape[0] 54 | height_scale = CALIBRATION_SHAPE[1]/raw_image_shape[1] 55 | bound_pts_pixels = dict() 56 | bound_pts_pixels['North'] = convert_canvas_to_pixel([bound_pts['North']], scale, pad)[0] 57 | bound_pts_pixels['West'] = convert_canvas_to_pixel([bound_pts['West']], scale, pad)[0] 58 | bound_pts_pixels['South'] = convert_canvas_to_pixel([bound_pts['South']], scale, pad)[0] 59 | bound_pts_pixels['East']= convert_canvas_to_pixel([bound_pts['East']], scale, pad)[0] 60 | 61 | bound_pts_pixels_calib = copy.deepcopy(bound_pts_pixels) 62 | for o in ['North', 'West', 'East', 'South']: 63 | bound_pts_pixels_calib[o][0] *= width_scale 64 | bound_pts_pixels_calib[o][1] *= height_scale 65 | 66 | 67 | road_x_range= road_bound(camera_param, bound_pts_pixels_calib)#bound_pts_pixels) 68 | LaneCount = np.array([[lane_num['en'], lane_num['es']], #east left, east right 69 | [lane_num['wn'], lane_num['ws']], #west left, west right 70 | [lane_num['nw'], lane_num['ne']], #north left, north right 71 | [lane_num['sw'], lane_num['se']] #south left, south right 72 | ], dtype=np.uint8) 73 | boundPts = np.array(road_x_range) 74 | 75 | sample_set=[roadpts_N,roadpts_W,roadpts_S,roadpts_E] 76 | for i in range(4): 77 | if sample_set[i] : 78 | sample_flag= 1 79 | sample_input= sample_set[i] 80 | Road.GenerateIntersection(xodr_filename, \ 81 | boundPts,\ 82 | np.array([40, 40, 40, 40]).reshape((4, 1)),\ 83 | LaneCount,sample_flag,sample_input) 84 | 85 | return bound_pts_pixels 86 | 87 | 88 | def generate_egoview_straight_road(lane2d_dict, center_lane_id, 89 | intrinsic, poses, depths, 90 | xodr_filename): 91 | #todo: need check all frames has lane labels? 92 | 93 | center_lane_world_coordinates = [] 94 | left_lane_num = [] 95 | right_lane_num = [] 96 | num_frame = min(len(lane2d_dict.keys()), min(len(poses), len(depths))) 97 | for i in range(num_frame): 98 | curr_lane2d = lane2d_dict[i] 99 | depth = depths[i] 100 | pose = poses[i] 101 | #project center line 102 | #for ego-view, the direction is always y-ascending direction 103 | ctr_pt2d = sort_points(curr_lane2d[center_lane_id]) 104 | ds = [depth[int(ctr_pt2d[j,1]), int(ctr_pt2d[j,0])] for j in range(ctr_pt2d.shape[0])] 105 | ctr_pt3d = project_2d_to_3d_depth_arr(np.transpose(ctr_pt2d),intrinsic, pose, np.array(ds)) 106 | ctr_pt3d = np.transpose(ctr_pt3d) 107 | left_num = 0 108 | right_num = 0 109 | for lane_id, lane_pts in curr_lane2d.items(): 110 | if lane_id != center_lane_id: 111 | pt2d = curr_lane2d[lane_id] 112 | coeff = np.polyfit(pt2d[:,1], pt2d[:,0],2) 113 | poly = np.poly1d(coeff) 114 | x = poly(ctr_pt2d[:, 1]) 115 | if x[0] 1.0): 61 | pitch = 1 62 | elif (sinp < -1.0): 63 | pitch = -1 64 | else: 65 | pitch = math.asin(sinp) 66 | # yaw (z-axis rotation) 67 | siny_cosp = 2 * (w * z + x * y) 68 | cosy_cosp = 1 - 2 * (y * y + z * z) 69 | yaw = math.atan2(siny_cosp, cosy_cosp) 70 | 71 | # roll=np.rad2deg(roll) 72 | # pitch=np.rad2deg(pitch) 73 | # yaw=np.rad2deg(yaw) 74 | v_roll.append(roll) 75 | v_pitch.append(pitch) 76 | v_yaw.append(yaw) 77 | 78 | total_time_list = [] 79 | with open(timefile, 'r') as f: 80 | list_read = f.readlines() 81 | 82 | for i in range(len(list_read)): 83 | a6 = list_read[i].split(' ') 84 | total_time_list.append(float("{:.{}f}".format(float(a6[0]), 6))) 85 | # print(float(a6[0])) 86 | 87 | for i in range(len(kf_time_list)): 88 | idx = total_time_list.index(float(kf_time_list[i])) 89 | #idx = '%06d' % idx 90 | kf_idx_list.append(idx) 91 | x = v_roll[i] 92 | y = v_pitch[i] 93 | z = v_yaw[i] 94 | kf_R = eulerAnglesToRotationMatrix(x, y, z) 95 | kf_T = np.array((v_x_pos[i], v_y_pos[i], v_z_pos[i])).reshape(3, 1) 96 | kf_pose = np.concatenate((kf_R, kf_T), axis=1) 97 | kf_pose_matrix_list.append(kf_pose) 98 | 99 | return kf_idx_list, kf_pose_matrix_list -------------------------------------------------------------------------------- /src/scale/cameracalib.py: -------------------------------------------------------------------------------- 1 | 2 | import os 3 | import sys 4 | import random 5 | import math 6 | import numpy as np 7 | import skimage.io 8 | import matplotlib 9 | import matplotlib.pyplot as plt 10 | import pickle 11 | import scipy.io as sio 12 | from PIL import Image, ImageDraw 13 | import pylab as plt 14 | import numpy.linalg as nplg 15 | 16 | # Root directory of the project 17 | #ROOT_DIR = os.getcwd() 18 | 19 | # Directory of images to run detection on 20 | #IMAGE_DIR = os.path.join(ROOT_DIR, "images") 21 | # Pmat=sio.loadmat('MatForTestPara_1900_190313.mat') 22 | #Pmat = sio.loadmat('PMat_1900_190513_case2.mat') 23 | #Pinst = Pmat['P_Test'] 24 | #Hinst = Pmat['H_Test'] 25 | 26 | 27 | def I2WCal(x, y, h_real, isI2W, Pinst): 28 | P = np.asarray(Pinst) 29 | H = P[:, (0, 2, 3)] 30 | if isI2W == 1: 31 | x1 = x 32 | y1 = y 33 | if h_real == 0: 34 | # print(H) 35 | # print(Tran) 36 | # print(Tran*(np.linalg.inv(H)*np.array([[x1,y1,1]]).T)) 37 | Coor = normalization(np.dot(np.linalg.inv(H), np.array([x1, y1, 1]).reshape((3, 1)))) 38 | # VA=np.dot(np.linalg.inv(H),np.array([x1,y1,1]).reshape((3,1))) 39 | # Coor=normalization(np.dot(Tran,np.dot(np.linalg.inv(H),np.array([x1,y1,1]).reshape((3,1))))) 40 | # print(VA) 41 | else: 42 | h = h_real 43 | Hi = [x1, y1] 44 | a1 = Hi[0] * P[2, 0] - P[0, 0] 45 | b1 = Hi[0] * P[2, 1] - P[0, 1] 46 | a2 = Hi[1] * P[2, 0] - P[1, 0] 47 | b2 = Hi[1] * P[2, 1] - P[1, 1] 48 | c1 = (P[0, 2] - Hi[0] * P[2, 2]) * h + P[0, 3] - Hi[0] * P[2, 3] 49 | c2 = (P[1, 2] - Hi[1] * P[2, 2]) * h + P[1, 3] - Hi[1] * P[2, 3] 50 | Hl = np.array([[a1, b1], [a2, b2]]) 51 | C = np.array([[c1], [c2]]) 52 | temp1 = np.dot(np.linalg.inv(Hl), C) 53 | # print((P[1,2]-Hi[1]*P[2,2])*h) 54 | # print(Hi[1]*P[2,3]) 55 | # print(P[1,3]) 56 | # print(Hl) 57 | # print(np.linalg.inv(Hl)) 58 | # print(C) 59 | # Coor=np.array([temp1[0],temp1[1]]) 60 | # Coor=normalization(np.dot(Tran,np.array([temp1[0],temp1[1],1]).reshape((3,1)))) 61 | Coor = normalization(np.array([temp1[0], temp1[1], 1]).reshape((3, 1))) 62 | # Coor=normalization(temp1.reshape((3,1))) 63 | 64 | elif isI2W == 0: 65 | 66 | Coor = normalization(np.dot(P, np.array([x, y, h_real, 1]).reshape((4, 1)))) 67 | Coor[0] = Coor[0] * 1 68 | Coor[1] = Coor[1] * 1 69 | # X=Coor[0] 70 | # Y=Coor[1] 71 | 72 | return Coor 73 | 74 | 75 | # In[272]: 76 | 77 | def normalization(x): 78 | num = x.shape[1] 79 | y = np.zeros((3, num)); 80 | for i in range(0, num): 81 | y[0, i] = x[0, i] / x[2, i]; 82 | y[1, i] = x[1, i] / x[2, i]; 83 | y[2, i] = 1; 84 | return y 85 | 86 | 87 | def BBoxRegression(CData, Outline, Class_type, ry): 88 | H = Hinst 89 | P = Pinst 90 | CData1 = np.zeros((1080, 1902)) 91 | # CData1[Outline(0):Outline(2),Outline(1):Outline(3)]=CData; 92 | CData1 = CData 93 | # P=Pmat['P_'+CamNum] 94 | # Tran=TranMat['tran_'+CamNum] 95 | # HeightHeadCurve=HeightHeadCurveMat['HeightHeadCurve_'+CamNum] 96 | # HeightFootCurve=HeightFootCurveMat['HeightCurve_'+CamNum] 97 | # H=np.array(P[:,[0,1,3]]) 98 | CenterInitial1 = np.array([(Outline[1] + Outline[3]) / 2, (Outline[0] + Outline[2]) / 2]) 99 | # print(CenterInitial1) 100 | BIN = 3 101 | BinInter = 5 102 | XRange = np.arange(-(Outline[3] - Outline[1]) / BIN, (Outline[3] - Outline[1]) / BIN, BinInter) 103 | YRange = np.arange(-(Outline[2] - Outline[0]) / BIN, (Outline[2] - Outline[0]) / BIN, BinInter) 104 | CenterCandX = XRange + CenterInitial1[0] 105 | CenterCandY = YRange + CenterInitial1[1] + 10 106 | UniCenterCandX = CenterCandX * 1 107 | UniCenterCandY = CenterCandY * 1 108 | LengthX = len(XRange) 109 | LengthY = len(YRange) 110 | face_idx = np.array([[1, 2, 6, 5], [2, 3, 7, 6], [3, 4, 8, 7], [4, 1, 5, 8]]) 111 | Dimension = np.zeros((9, 3)) 112 | Dimension_Car = np.array([[4200, 1700, 1400]]) 113 | Dimension_Truck = np.array([[6500, 3500, 2500]]) 114 | Dimension[3, :] = Dimension_Car 115 | Dimension[6, :] = Dimension_Truck 116 | R = np.array([[math.cos(ry), -math.sin(ry), 0], [math.sin(ry), math.cos(ry), 0], [0, 0, 1]]) 117 | l = Dimension[Class_type, 0] 118 | w = Dimension[Class_type, 1] 119 | h = Dimension[Class_type, 2] 120 | y_corners = np.array([[l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]]) 121 | x_corners = np.array([[w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]]) 122 | z_corners = np.array([[0, 0, 0, 0, h, h, h, h]]) 123 | Dimension_3D = np.concatenate((x_corners, y_corners, z_corners), axis=0) 124 | Dimension_3D = np.dot(R, Dimension_3D) 125 | MountFinal = 0 126 | worldLocation = 0 127 | for i in range(0, LengthX): 128 | for j in range(0, LengthY): 129 | Coor = normalization( 130 | np.dot(np.linalg.inv(H), np.array([UniCenterCandX[i], UniCenterCandY[j], 1]).reshape((3, 1)))) 131 | # temp=[Coor(1)/Coor(3),Coor(2)/Coor(3),0]; 132 | corners_3D = np.zeros((3, 8)) 133 | corners_3D[0, :] = Dimension_3D[0, :] + Coor[0] 134 | corners_3D[1, :] = Dimension_3D[1, :] + Coor[1] 135 | corners_3D[2, :] = Dimension_3D[2, :] 136 | # corners_3D(3,:)=corners_3D(3,:)*HeightFootCurve(floor(CenterCandY(j))); 137 | corners_2D = np.dot(P, np.concatenate((corners_3D, np.ones((1, corners_3D.shape[1]))), axis=0)) 138 | corners_2D = normalization(corners_2D) 139 | corners_2D[0, :] = corners_2D[0, :] * 1 140 | corners_2D[1, :] = corners_2D[1, :] * 1 141 | if corners_2D[0, 0] > corners_2D[0, 7]: 142 | draw_order = np.array([1, 5, 6, 7, 3, 4, 1]) - 1 143 | else: 144 | draw_order = np.array([2, 3, 4, 8, 5, 6, 2]) - 1 145 | # print(corners_2D) 146 | NewCornersX = corners_2D[0, draw_order] - round(min(corners_2D[0, draw_order])) 147 | NewCornersY = corners_2D[1, draw_order] - round(min(corners_2D[1, draw_order])) 148 | NewRangeX = int(3 + round(max(corners_2D[0, draw_order]) - min(corners_2D[0, draw_order]))) 149 | NewRangeY = int(3 + round(max(corners_2D[1, draw_order]) - min(corners_2D[1, draw_order]))) 150 | Xs = int(max(round(min(corners_2D[0, draw_order])) - 10, 1)) 151 | Ys = int(max(round(min(corners_2D[1, draw_order])) - 10, 1)) 152 | # print(Xs,Ys) 153 | # print(NewRangeX,NewRangeY) 154 | CData1_Resize = CData1[Ys:min(Ys + NewRangeY, 1080), Xs:min(Xs + NewRangeX, 1902)] 155 | # mask = poly2mask(NewCornersX+10,NewCornersY+10,(min(Ys+NewRangeY,1080)-Ys,min(Xs+NewRangeX,1902)-Xs)) 156 | mask = poly2mask(np.maximum(corners_2D[0, draw_order], 1), np.maximum(corners_2D[1, draw_order], 1), 157 | [1080, 1902]) 158 | # mask=poly2mask(NewCornersX+10,NewCornersY+10,min(Ys+NewRangeY,1200)-Ys,min(Xs+NewRangeX,1600)-Xs); 159 | # print('mask',mask.shape) 160 | Mount = mask * CData1 161 | 162 | Mount = np.sum(Mount) 163 | if MountFinal < Mount: 164 | # Location=[CenterCandX(i),CenterCandY(j)] 165 | worldLocation = Coor 166 | MountFinal = Mount 167 | # plt.imshow(Mount) 168 | # Corners_2D_Final=corners_2D 169 | # DrawOrder=draw_order 170 | MaskOut = mask 171 | 172 | else: 173 | MountFinal = MountFinal 174 | 175 | return worldLocation 176 | 177 | 178 | # In[234]: 179 | 180 | 181 | from skimage import draw 182 | import numpy as np 183 | 184 | 185 | def poly2mask(vertex_row_coords, vertex_col_coords, shape): 186 | fill_row_coords, fill_col_coords = draw.polygon(vertex_row_coords, vertex_col_coords, shape) 187 | mask = np.zeros(shape, dtype=np.bool) 188 | mask[fill_row_coords, fill_col_coords] = 1 189 | return mask 190 | 191 | # In[ ]: 192 | -------------------------------------------------------------------------------- /src/scale/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import csv 3 | import numpy as np 4 | import binascii 5 | import math 6 | import math, cmath 7 | import scipy.io as sio 8 | import numpy.linalg as nplg 9 | from PIL import Image, ImageDraw 10 | import matplotlib.pyplot as plt 11 | 12 | 13 | def isRotationMatrix(R): 14 | Rt = np.transpose(R) # 15 | shouldBeIdentity = np.dot(Rt, R) # 16 | I = np.identity(3, dtype=R.dtype) # 17 | n = np.linalg.norm(I - shouldBeIdentity) # 18 | return n < 1e-6 19 | 20 | 21 | def rotationMatrixToAngles(R): 22 | assert (isRotationMatrix(R)) # 23 | 24 | sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) # 25 | 26 | singular = sy < 1e-6 # 27 | 28 | if not singular: # 29 | x = math.atan2(R[2, 1], R[2, 2]) 30 | y = math.atan2(-R[2, 0], sy) 31 | z = math.atan2(R[1, 0], R[0, 0]) 32 | else: # 33 | x = math.atan2(-R[1, 2], R[1, 1]) 34 | y = math.atan2(-R[2, 0], sy) # 35 | z = 0 36 | 37 | return np.array([x, y, z]) 38 | 39 | def rotationMatrixToEulerAngles(R): 40 | #R = np.zeros((3, 3), dtype=np.float64) 41 | #cv2.Rodrigues(rvecs, R) 42 | sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) 43 | singular = sy < 1e-6 44 | if not singular: 45 | x = math.atan2(R[2,1] , R[2,2]) 46 | y = math.atan2(-R[2,0], sy) 47 | z = math.atan2(R[1,0], R[0,0]) 48 | else : 49 | x = math.atan2(-R[1,2], R[1,1]) 50 | y = math.atan2(-R[2,0], sy) 51 | z = 0 52 | #print('dst:', R) 53 | x = x*180.0/3.141592653589793 54 | y = y*180.0/3.141592653589793 55 | z = z*180.0/3.141592653589793 56 | return x,y,z 57 | def eulerAnglesToRotationMatrix(x,y,z) : 58 | theta = np.zeros((3, 1), dtype=np.float64) # 59 | theta[0] = x 60 | theta[1] = y 61 | theta[2] = z 62 | R_x = np.array([[1, 0, 0 ], 63 | [0, math.cos(theta[0]), -math.sin(theta[0]) ], 64 | [0, math.sin(theta[0]), math.cos(theta[0]) ] 65 | ]) 66 | R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1]) ], 67 | [0, 1, 0 ], 68 | [-math.sin(theta[1]), 0, math.cos(theta[1]) ] 69 | ]) 70 | R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0], 71 | [math.sin(theta[2]), math.cos(theta[2]), 0], 72 | [0, 0, 1] 73 | ]) 74 | R = np.dot(R_z, np.dot( R_y, R_x )) 75 | #rvecs = np.zeros((1, 1, 3), dtype=np.float64) 76 | #rvecs,_ = cv2.Rodrigues(R, rvecstmp) 77 | return R -------------------------------------------------------------------------------- /src/track/JRETrack/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/track/JRETrack/__init__.py -------------------------------------------------------------------------------- /src/track/JRETrack/basetrack.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from collections import OrderedDict 3 | 4 | 5 | class TrackState(object): 6 | New = 0 7 | Tracked = 1 8 | Lost = 2 9 | Removed = 3 10 | 11 | 12 | class BaseTrack(object): 13 | _count = 0 14 | 15 | track_id = 0 16 | is_activated = False 17 | state = TrackState.New 18 | 19 | history = OrderedDict() 20 | features = [] 21 | curr_feature = None 22 | score = 0 23 | start_frame = 0 24 | frame_id = 0 25 | time_since_update = 0 26 | 27 | # multi-camera 28 | location = (np.inf, np.inf) 29 | 30 | @property 31 | def end_frame(self): 32 | return self.frame_id 33 | 34 | @staticmethod 35 | def next_id(): 36 | BaseTrack._count += 1 37 | return BaseTrack._count 38 | 39 | def activate(self, *args): 40 | raise NotImplementedError 41 | 42 | def predict(self): 43 | raise NotImplementedError 44 | 45 | def update(self, *args, **kwargs): 46 | raise NotImplementedError 47 | 48 | def mark_lost(self): 49 | self.state = TrackState.Lost 50 | 51 | def mark_removed(self): 52 | self.state = TrackState.Removed 53 | 54 | -------------------------------------------------------------------------------- /src/track/JRETrack/feature_extractor.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import cv2 4 | import tensorflow as tf 5 | 6 | def _run_in_batches(func, data_dict, out, batch_size): 7 | data_len = len(out) 8 | num_batches = int(data_len / batch_size) 9 | 10 | s, e = 0, 0 11 | for i in range(num_batches): 12 | s, e = i * batch_size, (i + 1) * batch_size 13 | batch_data_dict = {k: v[s:e] for k, v in data_dict.items()} 14 | out[s:e] = func(batch_data_dict) 15 | if e < len(out): 16 | batch_data_dict = {k: v[e:] for k, v in data_dict.items()} 17 | out[e:] = func(batch_data_dict) 18 | 19 | 20 | class FeatureExtractor(object): 21 | def __init__(self, ckpt): 22 | self.sess = tf.Session() 23 | with tf.gfile.GFile(ckpt, "rb") as f: 24 | graph_def = tf.GraphDef() 25 | graph_def.ParseFromString(f.read()) 26 | 27 | self.sess.graph.as_default() 28 | tf.import_graph_def(graph_def, name="") 29 | self.input_var = tf.get_default_graph().get_tensor_by_name("input:0" ) 30 | self.output_var = tf.get_default_graph().get_tensor_by_name("head/out_emb:0") 31 | 32 | self.feature_dim = self.output_var.get_shape().as_list()[-1] 33 | self.image_shape = self.input_var.get_shape().as_list()[1:]#(h,w,3) 34 | self.aspect_ratio = self.image_shape[0]/self.image_shape[1] 35 | 36 | def extract(self, image, tlbrs,batch_size=32): 37 | image_patches =[] 38 | for tlbr in tlbrs: 39 | #crop image patch 40 | xywh = tlbr.copy() 41 | xywh[2:] = tlbr[2:]- tlbr[:2] 42 | patch = self.crop_resize_image(image, xywh) 43 | image_patches.append(patch) 44 | image_patches = np.asarray(image_patches) 45 | out = np.zeros((len(image_patches), self.feature_dim), np.float32) 46 | _run_in_batches( 47 | lambda x: self.sess.run(self.output_var, feed_dict=x), 48 | {self.input_var: image_patches}, out, batch_size) 49 | return out 50 | 51 | def crop_resize_image(self, image, xywh): 52 | tlbr = xywh.copy() 53 | new_width = self.aspect_ratio * xywh[3] 54 | tlbr[0] -= (new_width - xywh[2]) / 2 55 | tlbr[2] = new_width 56 | tlbr[2:] += tlbr[:2] 57 | tlbr = tlbr.astype(np.int) 58 | tlbr[:2] = np.maximum(0, tlbr[:2]) 59 | tlbr[2:] = np.minimum(np.asarray(image.shape[:2][::-1]) - 1, tlbr[2:]) 60 | sx, sy, ex, ey = tlbr 61 | cropped = image[sy:ey, sx:ex].copy() 62 | cropped = cv2.resize(cropped, (self.image_shape[1], self.image_shape[0])) 63 | return cropped 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /src/track/JRETrack/log.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | 4 | def get_logger(name='root'): 5 | formatter = logging.Formatter( 6 | # fmt='%(asctime)s [%(levelname)s]: %(filename)s(%(funcName)s:%(lineno)s) >> %(message)s') 7 | fmt='%(asctime)s [%(levelname)s]: %(message)s', datefmt='%Y-%m-%d %H:%M:%S') 8 | 9 | handler = logging.StreamHandler() 10 | handler.setFormatter(formatter) 11 | 12 | logger = logging.getLogger(name) 13 | logger.setLevel(logging.DEBUG) 14 | logger.addHandler(handler) 15 | return logger 16 | 17 | 18 | logger = get_logger('root') 19 | -------------------------------------------------------------------------------- /src/track/JRETrack/matching.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy 3 | from scipy.spatial.distance import cdist 4 | import lap 5 | 6 | from cython_bbox import bbox_overlaps as bbox_ious 7 | from .kalman_filter import chi2inv95 8 | 9 | def merge_matches(m1, m2, shape): 10 | O,P,Q = shape 11 | m1 = np.asarray(m1) 12 | m2 = np.asarray(m2) 13 | 14 | M1 = scipy.sparse.coo_matrix((np.ones(len(m1)), (m1[:, 0], m1[:, 1])), shape=(O, P)) 15 | M2 = scipy.sparse.coo_matrix((np.ones(len(m2)), (m2[:, 0], m2[:, 1])), shape=(P, Q)) 16 | 17 | mask = M1*M2 18 | match = mask.nonzero() 19 | match = list(zip(match[0], match[1])) 20 | unmatched_O = tuple(set(range(O)) - set([i for i, j in match])) 21 | unmatched_Q = tuple(set(range(Q)) - set([j for i, j in match])) 22 | 23 | return match, unmatched_O, unmatched_Q 24 | 25 | 26 | def linear_assignment(cost_matrix, thresh): 27 | if cost_matrix.size == 0: 28 | return np.empty((0, 2), dtype=int), tuple(range(cost_matrix.shape[0])), tuple(range(cost_matrix.shape[1])) 29 | matches, unmatched_a, unmatched_b = [], [], [] 30 | cost, x, y = lap.lapjv(cost_matrix, extend_cost=True, cost_limit=thresh) 31 | for ix, mx in enumerate(x): 32 | if mx >= 0: 33 | matches.append([ix, mx]) 34 | unmatched_a = np.where(x < 0)[0] 35 | unmatched_b = np.where(y < 0)[0] 36 | matches = np.asarray(matches) 37 | return matches, unmatched_a, unmatched_b 38 | 39 | 40 | def ious(atlbrs, btlbrs): 41 | """ 42 | Compute cost based on IoU 43 | :type atlbrs: list[tlbr] | np.ndarray 44 | :type atlbrs: list[tlbr] | np.ndarray 45 | 46 | :rtype ious np.ndarray 47 | """ 48 | ious = np.zeros((len(atlbrs), len(btlbrs)), dtype=np.float) 49 | if ious.size == 0: 50 | return ious 51 | 52 | ious = bbox_ious( 53 | np.ascontiguousarray(atlbrs, dtype=np.float), 54 | np.ascontiguousarray(btlbrs, dtype=np.float) 55 | ) 56 | 57 | return ious 58 | 59 | 60 | def iou_distance(atracks, btracks): 61 | """ 62 | Compute cost based on IoU 63 | :type atracks: list[STrack] 64 | :type btracks: list[STrack] 65 | 66 | :rtype cost_matrix np.ndarray 67 | """ 68 | 69 | if (len(atracks)>0 and isinstance(atracks[0], np.ndarray)) or (len(btracks) > 0 and isinstance(btracks[0], np.ndarray)): 70 | atlbrs = atracks 71 | btlbrs = btracks 72 | else: 73 | atlbrs = [track.tlbr for track in atracks] 74 | btlbrs = [track.tlbr for track in btracks] 75 | _ious = ious(atlbrs, btlbrs) 76 | cost_matrix = 1 - _ious 77 | 78 | return cost_matrix 79 | 80 | def embedding_distance(tracks, detections, metric='cosine'): 81 | """ 82 | :param tracks: list[STrack] 83 | :param detections: list[BaseTrack] 84 | :param metric: 85 | :return: cost_matrix np.ndarray 86 | """ 87 | 88 | cost_matrix = np.zeros((len(tracks), len(detections)), dtype=np.float) 89 | if cost_matrix.size == 0: 90 | return cost_matrix 91 | det_features = np.asarray([track.curr_feat for track in detections], dtype=np.float) 92 | track_features = np.asarray([track.smooth_feat for track in tracks], dtype=np.float) 93 | cost_matrix = np.maximum(0.0, cdist(track_features, det_features)) # Nomalized features 94 | 95 | return cost_matrix 96 | 97 | 98 | def fuse_motion(kf, cost_matrix, tracks, detections, only_position=False, lambda_=0.98): 99 | if cost_matrix.size == 0: 100 | return cost_matrix 101 | gating_dim = 2 if only_position else 4 102 | gating_threshold = chi2inv95[gating_dim] 103 | measurements = np.asarray([det.to_xyah() for det in detections]) 104 | for row, track in enumerate(tracks): 105 | gating_distance = kf.gating_distance( 106 | track.mean, track.covariance, measurements, only_position, metric='maha') 107 | cost_matrix[row, gating_distance > gating_threshold] = np.inf 108 | cost_matrix[row] = lambda_ * cost_matrix[row] + (1-lambda_)* gating_distance 109 | return cost_matrix 110 | -------------------------------------------------------------------------------- /src/track/ObjectTracking.py: -------------------------------------------------------------------------------- 1 | from tracker import Tracker 2 | 3 | 4 | extention = ".jpg" 5 | imagelist = traverseFolder(IMAGE_DIR, extention); # traverse( IMAGE_DIR , extention);# 6 | imagelist = sorted(imagelist) 7 | det_list = [1, 2, 3, 4, 6, 8] 8 | counter = 0 9 | tracker = Tracker(60, 30, 300, 100) 10 | def ObjTracking(): 11 | for frame in range(len(results)): 12 | bb_number = 0 13 | centers = [] 14 | r = results[frame] 15 | out_boxes = r['rois'] 16 | out_scores = r['scores'] 17 | out_classes = r['class_ids'] 18 | 19 | for i, c in reversed(list(enumerate(out_classes))): 20 | predicted_class = class_names[c] 21 | box = out_boxes[i] 22 | score = out_scores[i] 23 | bb_number = len(out_boxes) 24 | if c in det_list: 25 | top, left, bottom, right = box 26 | center = (int((left + right) // 2), int((top + bottom) // 2)) 27 | b = np.array([[(left + right) // 2], [(top + bottom) // 2]]) 28 | centers.append(b) 29 | 30 | image = skimage.io.imread(imagelist[frame]) 31 | # visualize.display_instances(image, r['rois'], r['masks'], r['class_ids'], class_names, r['scores']) 32 | if counter % 40 == 0: 33 | print('processed image', counter) 34 | counter += 1 35 | 36 | if (len(centers) > 0): 37 | # print(len(tracker.tracks)) 38 | tracker.Update(centers) 39 | 40 | 41 | -------------------------------------------------------------------------------- /src/track/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/track/__init__.py -------------------------------------------------------------------------------- /src/track/kalman_filter.py: -------------------------------------------------------------------------------- 1 | ''' 2 | File name : kalman_filter.py 3 | File Description : Kalman Filter Algorithm Implementation 4 | Author : Srini Ananthakrishnan 5 | Date created : 07/14/2017 6 | Date last modified: 07/16/2017 7 | Python Version : 2.7 8 | ''' 9 | 10 | # Import python libraries 11 | import numpy as np 12 | 13 | 14 | class KalmanFilter(object): 15 | """Kalman Filter class keeps track of the estimated state of 16 | the system and the variance or uncertainty of the estimate. 17 | Predict and Correct methods implement the functionality 18 | Reference: https://en.wikipedia.org/wiki/Kalman_filter 19 | Attributes: None 20 | """ 21 | 22 | def __init__(self): 23 | """Initialize variable used by Kalman Filter class 24 | Args: 25 | None 26 | Return: 27 | None 28 | """ 29 | self.dt = 0.005 # delta time 30 | 31 | self.A = np.array([[1, 0], [0, 1]]) # matrix in observation equations 32 | self.u = np.zeros((2, 1)) # previous state vector 33 | 34 | # (x,y) tracking object center 35 | self.b = np.array([[0], [0]]) # vector of observations 36 | 37 | self.P = np.diag((10.0, 10.0)) # covariance matrix 38 | self.F = np.array([[1.0, self.dt], [0.0, 1.0]]) # state transition mat 39 | 40 | self.Q = np.eye(self.u.shape[0]) # process noise matrix 41 | self.R = np.eye(self.b.shape[0]) # observation noise matrix 42 | self.lastResult = np.array([[0], [0]]) 43 | 44 | def predict(self): 45 | """Predict state vector u and variance of uncertainty P (covariance). 46 | where, 47 | u: previous state vector 48 | P: previous covariance matrix 49 | F: state transition matrix 50 | Q: process noise matrix 51 | Equations: 52 | u'_{k|k-1} = Fu'_{k-1|k-1} 53 | P_{k|k-1} = FP_{k-1|k-1} F.T + Q 54 | where, 55 | F.T is F transpose 56 | Args: 57 | None 58 | Return: 59 | vector of predicted state estimate 60 | """ 61 | # Predicted state estimate 62 | self.u = np.round(np.dot(self.F, self.u)) 63 | # Predicted estimate covariance 64 | self.P = np.dot(self.F, np.dot(self.P, self.F.T)) + self.Q 65 | self.lastResult = self.u # same last predicted result 66 | return self.u 67 | 68 | def correct(self, b, flag): 69 | """Correct or update state vector u and variance of uncertainty P (covariance). 70 | where, 71 | u: predicted state vector u 72 | A: matrix in observation equations 73 | b: vector of observations 74 | P: predicted covariance matrix 75 | Q: process noise matrix 76 | R: observation noise matrix 77 | Equations: 78 | C = AP_{k|k-1} A.T + R 79 | K_{k} = P_{k|k-1} A.T(C.Inv) 80 | u'_{k|k} = u'_{k|k-1} + K_{k}(b_{k} - Au'_{k|k-1}) 81 | P_{k|k} = P_{k|k-1} - K_{k}(CK.T) 82 | where, 83 | A.T is A transpose 84 | C.Inv is C inverse 85 | Args: 86 | b: vector of observations 87 | flag: if "true" prediction result will be updated else detection 88 | Return: 89 | predicted state vector u 90 | """ 91 | 92 | if not flag: # update using prediction 93 | self.b = self.lastResult 94 | else: # update using detection 95 | self.b = b 96 | C = np.dot(self.A, np.dot(self.P, self.A.T)) + self.R 97 | K = np.dot(self.P, np.dot(self.A.T, np.linalg.inv(C))) 98 | 99 | self.u = np.round(self.u + np.dot(K, (self.b - np.dot(self.A, 100 | self.u)))) 101 | self.P = self.P - np.dot(K, np.dot(C, K.T)) 102 | self.lastResult = self.u 103 | return self.u -------------------------------------------------------------------------------- /src/track/kcf/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | python3 setup.py build_ext --inplace 3 | rm -rf build 4 | -------------------------------------------------------------------------------- /src/track/kcf/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/track/kcf/__init__.py -------------------------------------------------------------------------------- /src/track/kcf/fhog_util.pyx: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | cimport numpy as np 3 | 4 | cdef extern from "math.h": 5 | cpdef float sqrt(float x) 6 | 7 | def pyfunc12(np.ndarray[np.float32_t, ndim=3] dx, 8 | np.ndarray[np.float32_t, ndim=3] dy, 9 | np.ndarray[np.float32_t, ndim=1] boundary_x, 10 | np.ndarray[np.float32_t, ndim=1] boundary_y, 11 | np.int_t height, 12 | np.int_t width, 13 | np.int_t numChannels, 14 | np.int_t k): 15 | 16 | 17 | cdef np.ndarray[np.float32_t, ndim=2] r = np.zeros((height, width), dtype=np.float32) 18 | cdef np.ndarray[np.int32_t, ndim=3] alfa= np.zeros((height, width, 2), dtype=np.int32) 19 | 20 | cdef int sizeX = width//k 21 | cdef int sizeY = height//k 22 | cdef int px = 3*9 23 | cdef int p = px 24 | cdef int stringSize = sizeX * p 25 | cdef int _i, _j, _ii,_jj, _c, _ch, maxi, _kk 26 | cdef float x, y, tx, ty, magnitude, mmax, dotProd 27 | cdef float a_x, b_x 28 | 29 | #memory view 30 | cdef float[:,:, :] dx_view = dx 31 | cdef float[:,:,:] dy_view = dy 32 | cdef float[:,:] r_view = r 33 | cdef int[:,:,:] alfa_view = alfa 34 | 35 | for _j in range(1, height-1): 36 | for _i in range(1, width-1): 37 | _c = 0 38 | x = dx_view[_j, _i, _c] 39 | y = dy_view[_j, _i, _c] 40 | r_view[_j, _i] = sqrt(x*x + y*y) 41 | 42 | for _ch in range(1, numChannels): 43 | tx = dx_view[_j, _i, _ch] 44 | ty = dy_view[_j, _i, _ch] 45 | magnitude = sqrt(tx*tx + ty*ty) 46 | if (magnitude> r_view[_j, _i]): 47 | r_view[_j, _i] = magnitude 48 | _c = _ch 49 | x = tx 50 | y = ty 51 | 52 | mmax = boundary_x[0]*x + boundary_y[0]*y 53 | maxi = 0 54 | for _kk in range(9): #NUM_SECTOR 55 | dotProd = boundary_x[_kk]*x + boundary_y[_kk]*y 56 | if(dotProd > mmax): 57 | mmax = dotProd 58 | maxi = _kk 59 | elif (-dotProd>mmax): 60 | mmax = -dotProd 61 | maxi = _kk+9 62 | alfa_view[_j, _i, 0] = maxi % 9 63 | alfa_view[_j, _i, 1] = maxi 64 | 65 | cdef int [:] nearest = np.ones((k), dtype = np.int32) 66 | cdef float [:,:] w = np.zeros((k,2), dtype = np.float32) 67 | for _i in range(k//2): 68 | nearest[_i] = -1 69 | for _i in range(k//2, k): 70 | nearest[_i] = 1 71 | for _j in range(k//2): 72 | b_x = k//2 + _j + 0.5 73 | a_x = k//2 - _j - 0.5 74 | w[_j, 0] = 1.0/a_x * ((a_x * b_x) / ( a_x + b_x)) 75 | w[_j, 1] = 1.0/b_x * ((a_x * b_x) / ( a_x + b_x)) 76 | w[k-_j-1,0] = w[_j,0] 77 | w[k-_j-1,1] = w[_j,1] 78 | 79 | mapp = np.zeros((sizeX*sizeY*p), np.float32) 80 | cdef float [:] mapp_view = mapp 81 | for _i in range(sizeY): 82 | for _j in range(sizeX): 83 | for _ii in range(k): 84 | for _jj in range(k): 85 | if ((_i * k + _ii > 0) and 86 | (_i * k + _ii < height - 1) and 87 | (_j * k + _jj > 0) and 88 | (_j * k + _jj < width - 1)): 89 | 90 | mapp_view[_i*stringSize + _j*p + alfa[k*_i+_ii,_j*k+_jj,0]] += r[k*_i+_ii,_j*k+_jj] * w[_ii,0] * w[_jj,0] 91 | mapp_view[_i*stringSize + _j*p + alfa[k*_i+_ii,_j*k+_jj,1] + 9] += r[k*_i+_ii,_j*k+_jj] * w[_ii,0] * w[_jj,0] 92 | if((_i + nearest[_ii] >= 0) and (_i + nearest[_ii] <= sizeY - 1)): 93 | mapp[(_i+nearest[_ii])*stringSize + _j*p + alfa[k*_i+_ii,_j*k+_jj,0]] += r[k*_i+_ii,_j*k+_jj] * w[_ii,1] * w[_jj,0] 94 | mapp[(_i+nearest[_ii])*stringSize + _j*p + alfa[k*_i+_ii,_j*k+_jj,1] + 9] += r[k*_i+_ii,_j*k+_jj] * w[_ii,1] * w[_jj,0] 95 | if((_j + nearest[_jj] >= 0) and (_j + nearest[_jj] <= sizeX - 1)): 96 | mapp_view[_i*stringSize + (_j+nearest[_jj])*p + alfa[k*_i+_ii,_j*k+_jj,0]] += r[k*_i+_ii,_j*k+_jj] * w[_ii,0] * w[_jj,1] 97 | mapp_view[_i*stringSize + (_j+nearest[_jj])*p + alfa[k*_i+_ii,_j*k+_jj,1] + 9] += r[k*_i+_ii,_j*k+_jj] * w[_ii,0] * w[_jj,1] 98 | if((_i + nearest[_ii] >= 0) and (_i + nearest[_ii] <= sizeY - 1) and (_j + nearest[_jj] >= 0) and (_j + nearest[_jj] <= sizeX - 1)): 99 | mapp_view[(_i+nearest[_ii])*stringSize + (_j+nearest[_jj])*p + alfa[k*_i+_ii,_j*k+_jj,0]] += r[k*_i+_ii,_j*k+_jj] * w[_ii,1] * w[_jj,1] 100 | mapp_view[(_i+nearest[_ii])*stringSize + (_j+nearest[_jj])*p + alfa[k*_i+_ii,_j*k+_jj,1] + 9] += r[k*_i+_ii,_j*k+_jj] * w[_ii,1] * w[_jj,1] 101 | 102 | return mapp#, r,alfa 103 | 104 | -------------------------------------------------------------------------------- /src/track/kcf/run.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 3 | import sys 4 | from time import time 5 | 6 | import kcftracker 7 | 8 | selectingObject = False 9 | initTracking = False 10 | onTracking = False 11 | ix, iy, cx, cy = -1, -1, -1, -1 12 | w, h = 0, 0 13 | 14 | inteval = 1 15 | duration = 0.01 16 | 17 | # mouse callback function 18 | def draw_boundingbox(event, x, y, flags, param): 19 | global selectingObject, initTracking, onTracking, ix, iy, cx,cy, w, h 20 | 21 | if event == cv2.EVENT_LBUTTONDOWN: 22 | selectingObject = True 23 | onTracking = False 24 | ix, iy = x, y 25 | cx, cy = x, y 26 | 27 | elif event == cv2.EVENT_MOUSEMOVE: 28 | cx, cy = x, y 29 | 30 | elif event == cv2.EVENT_LBUTTONUP: 31 | selectingObject = False 32 | if(abs(x-ix)>10 and abs(y-iy)>10): 33 | w, h = abs(x - ix), abs(y - iy) 34 | ix, iy = min(x, ix), min(y, iy) 35 | initTracking = True 36 | else: 37 | onTracking = False 38 | 39 | elif event == cv2.EVENT_RBUTTONDOWN: 40 | onTracking = False 41 | if(w>0): 42 | ix, iy = x-w/2, y-h/2 43 | initTracking = True 44 | 45 | 46 | 47 | if __name__ == '__main__': 48 | 49 | if(len(sys.argv)==1): 50 | cap = cv2.VideoCapture(0) 51 | elif(len(sys.argv)==2): 52 | if(sys.argv[1].isdigit()): # True if sys.argv[1] is str of a nonnegative integer 53 | cap = cv2.VideoCapture(int(sys.argv[1])) 54 | else: 55 | cap = cv2.VideoCapture(sys.argv[1]) 56 | inteval = 30 57 | else: assert(0), "too many arguments" 58 | 59 | tracker = kcftracker.KCFTracker(True, True, True) # hog, fixed_window, multiscale 60 | #if you use hog feature, there will be a short pause after you draw a first boundingbox, that is due to the use of Numba. 61 | 62 | cv2.namedWindow('tracking') 63 | cv2.setMouseCallback('tracking',draw_boundingbox) 64 | 65 | while(cap.isOpened()): 66 | ret, frame = cap.read() 67 | if not ret: 68 | break 69 | 70 | if(selectingObject): 71 | cv2.rectangle(frame,(ix,iy), (cx,cy), (0,255,255), 1) 72 | elif(initTracking): 73 | cv2.rectangle(frame,(ix,iy), (ix+w,iy+h), (0,255,255), 2) 74 | 75 | tracker.init([ix,iy,w,h], frame) 76 | 77 | initTracking = False 78 | onTracking = True 79 | elif(onTracking): 80 | t0 = time() 81 | boundingbox = tracker.update(frame) 82 | t1 = time() 83 | 84 | boundingbox = map(int, boundingbox) 85 | cv2.rectangle(frame,(boundingbox[0],boundingbox[1]), (boundingbox[0]+boundingbox[2],boundingbox[1]+boundingbox[3]), (0,255,255), 1) 86 | 87 | duration = 0.8*duration + 0.2*(t1-t0) 88 | #duration = t1-t0 89 | cv2.putText(frame, 'FPS: '+str(1/duration)[:4].strip('.'), (8,20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,0,255), 2) 90 | 91 | cv2.imshow('tracking', frame) 92 | c = cv2.waitKey(inteval) & 0xFF 93 | if c==27 or c==ord('q'): 94 | break 95 | 96 | cap.release() 97 | cv2.destroyAllWindows() 98 | -------------------------------------------------------------------------------- /src/track/kcf/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | from distutils.extension import Extension 3 | from Cython.Distutils import build_ext 4 | import numpy as np 5 | 6 | # Obtain the numpy include directory. This logic works across numpy versions. 7 | try: 8 | numpy_include = np.get_include() 9 | except AttributeError: 10 | numpy_include = np.get_numpy_include() 11 | 12 | extra_compile_args = ['-std=c++11'] 13 | ext_modules = [ 14 | Extension( 15 | "fhog_util", 16 | ["fhog_util.pyx"], 17 | extra_compile_args = extra_compile_args, 18 | include_dirs = [numpy_include] 19 | ), 20 | ] 21 | 22 | setup( 23 | name='fhog', 24 | ext_modules=ext_modules, 25 | cmdclass={'build_ext': build_ext}, 26 | ) 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/track/simple_tracker.py: -------------------------------------------------------------------------------- 1 | 2 | # Import python libraries 3 | import numpy as np 4 | from .kalman_filter import KalmanFilter 5 | from scipy.optimize import linear_sum_assignment 6 | 7 | 8 | class Track(object): 9 | """Track class for every object to be tracked 10 | Attributes: 11 | None 12 | """ 13 | 14 | def __init__(self, prediction, trackIdCount): 15 | """Initialize variables used by Track class 16 | Args: 17 | prediction: predicted centroids of object to be tracked 18 | trackIdCount: identification of each track object 19 | Return: 20 | None 21 | """ 22 | self.track_id = trackIdCount # identification of each track object 23 | self.KF = KalmanFilter() # KF instance to track this object 24 | self.prediction = np.asarray(prediction) # predicted centroids (x,y) 25 | self.skipped_frames = 0 # number of frames skipped undetected 26 | self.trace = [] # trace path 27 | self.instIDList = [] # track inst IDs in each frame 28 | self.prediction_2dBB = [] 29 | self.prediction_2dBB_list = [] 30 | 31 | 32 | class Tracker(object): 33 | """Tracker class that updates track vectors of object tracked 34 | Attributes: 35 | None 36 | """ 37 | 38 | def __init__(self, dist_thresh, max_frames_to_skip, max_trace_length, 39 | trackIdCount): 40 | """Initialize variable used by Tracker class 41 | Args: 42 | dist_thresh: distance threshold. When exceeds the threshold, 43 | track will be deleted and new track is created 44 | max_frames_to_skip: maximum allowed frames to be skipped for 45 | the track object undetected 46 | max_trace_lenght: trace path history length 47 | trackIdCount: identification of each track object 48 | Return: 49 | None 50 | """ 51 | self.dist_thresh = dist_thresh 52 | self.max_frames_to_skip = max_frames_to_skip 53 | self.max_trace_length = max_trace_length 54 | self.tracks = [] 55 | self.trackIdCount = trackIdCount 56 | 57 | def update(self, detections): 58 | """Update tracks vector using following steps: 59 | - Create tracks if no tracks vector found 60 | - Calculate cost using sum of square distance 61 | between predicted vs detected centroids 62 | - Using Hungarian Algorithm assign the correct 63 | detected measurements to predicted tracks 64 | https://en.wikipedia.org/wiki/Hungarian_algorithm 65 | - Identify tracks with no assignment, if any 66 | - If tracks are not detected for long time, remove them 67 | - Now look for un_assigned detects 68 | - Start new tracks 69 | - Update KalmanFilter state, lastResults and tracks trace 70 | Args: 71 | detections: detected centroids of object to be tracked 72 | Return: 73 | None 74 | """ 75 | 76 | # Create tracks if no tracks vector found 77 | if (len(self.tracks) == 0): 78 | for i in range(len(detections)): 79 | track = Track(detections[i][0], self.trackIdCount) 80 | self.trackIdCount += 1 81 | self.tracks.append(track) 82 | 83 | # Calculate cost using sum of square distance between 84 | # predicted vs detected centroids; 85 | N = len(self.tracks) 86 | M = len(detections) 87 | cost = np.zeros(shape=(N, M)) # Cost matrix 88 | for i in range(len(self.tracks)): 89 | for j in range(len(detections)): 90 | try: 91 | diff = self.tracks[i].prediction - detections[j] 92 | distance = np.sqrt(diff[0][0]*diff[0][0] + 93 | diff[1][0]*diff[1][0]) 94 | cost[i][j] = distance 95 | except: 96 | pass 97 | 98 | # Let's average the squared ERROR 99 | cost = (0.5) * cost 100 | # Using Hungarian Algorithm assign the correct detected measurements 101 | # to predicted tracks 102 | assignment = [] 103 | for _ in range(N): 104 | assignment.append(-1) 105 | row_ind, col_ind = linear_sum_assignment(cost) 106 | for i in range(len(row_ind)): 107 | assignment[row_ind[i]] = col_ind[i] 108 | # Identify tracks with no assignment, if any 109 | un_assigned_tracks = [] 110 | for i in range(len(assignment)): 111 | if (assignment[i] != -1): 112 | # check for cost distance threshold. 113 | # If cost is very high then un_assign (delete) the track 114 | if (cost[i][assignment[i]] > self.dist_thresh): 115 | assignment[i] = -1 116 | un_assigned_tracks.append(i) 117 | pass 118 | else: 119 | self.tracks[i].skipped_frames += 1 120 | 121 | # If tracks are not detected for long time, remove them 122 | del_tracks = [] 123 | for i in range(len(self.tracks)): 124 | if (self.tracks[i].skipped_frames > self.max_frames_to_skip): 125 | del_tracks.append(i) 126 | if len(del_tracks) > 0: # only when skipped frame exceeds max 127 | for id in del_tracks: 128 | if id < len(self.tracks): 129 | del self.tracks[id] 130 | del assignment[id] 131 | else: 132 | print(111) 133 | #dprint("ERROR: id is greater than length of tracks") 134 | 135 | # Now look for un_assigned detects 136 | un_assigned_detects = [] 137 | for i in range(len(detections)): 138 | if i not in assignment: 139 | un_assigned_detects.append(i) 140 | 141 | # Start new tracks 142 | if(len(un_assigned_detects) != 0): 143 | for i in range(len(un_assigned_detects)): 144 | track = Track(detections[un_assigned_detects[i]], 145 | self.trackIdCount) 146 | self.trackIdCount += 1 147 | self.tracks.append(track) 148 | 149 | # Update KalmanFilter state, lastResults and tracks trace 150 | for i in range(len(assignment)): 151 | self.tracks[i].KF.predict() 152 | 153 | if(assignment[i] != -1): 154 | self.tracks[i].skipped_frames = 0 155 | self.tracks[i].prediction_2dBB = detections[assignment[i]] 156 | self.tracks[i].prediction = self.tracks[i].KF.correct(detections[assignment[i]], 1) 157 | else: 158 | self.tracks[i].prediction = self.tracks[i].KF.correct( 159 | np.array([[0], [0]]), 0) 160 | 161 | if(len(self.tracks[i].trace) > self.max_trace_length): 162 | for j in range(len(self.tracks[i].trace) - 163 | self.max_trace_length): 164 | del self.tracks[i].trace[j] 165 | 166 | self.tracks[i].trace.append(self.tracks[i].prediction) 167 | self.tracks[i].KF.lastResult = self.tracks[i].prediction 168 | self.tracks[i].prediction_2dBB_list.append(self.tracks[i].prediction_2dBB) 169 | # print(assignment) 170 | 171 | def reset(self): 172 | self.tracks = [] 173 | 174 | def get_centers(self, min_age): 175 | tracklets = [] 176 | for k in range(len(self.tracks)): 177 | if len(self.tracks[k].trace)>=min_age: 178 | curr_traj = [] 179 | for idx in range(len(self.tracks[k].trace)): 180 | x = self.tracks[k].trace[idx][0] 181 | y = self.tracks[k].trace[idx][1] 182 | curr_traj.append(np.array(x,y)) 183 | tracklets.append(curr_traj) 184 | #history: frame_idx, track_id, label_id, detection_index, x1,y1,x2,y2 185 | def get_tracklets(self): 186 | print("TOBE IMPLEMENTED") -------------------------------------------------------------------------------- /src/track/track_factory.py: -------------------------------------------------------------------------------- 1 | from .simple_tracker import Tracker as SimpleTracker 2 | from .JRETrack.multitracker import JDETracker 3 | from .kcf.kcftracker import KCFTracker 4 | import numpy as np 5 | 6 | class TRACKER(object): 7 | def __init__(self, track_config): 8 | self.track_alg = track_config['TRACK_MODE'] 9 | self.device = track_config['DEVICE'] 10 | self.reid_modelname = track_config['REID_MODEL'] 11 | self.load_model() 12 | self.tracks = [] 13 | self.frame_num = 0 14 | 15 | def load_model(self): 16 | 17 | if self.track_alg =="SIMPLE": 18 | self.tracker = SimpleTracker(60, 30, 300, 100) 19 | elif self.track_alg =="DEEP_SORT": 20 | #max_distance = 0.5 21 | #nn_budget = 100 22 | #metric = NearestNeighborDistanceMetric("euclidean", max_distance, nn_budget) 23 | #self.tracker = DeepSortTracker(metric, reid_model_filename=self.reid_modelname, device=self.device) 24 | #self.tracker = DeepSortTracker(self.reid_modelname) 25 | self.tracker = JDETracker(self.reid_modelname) 26 | elif self.track_alg =="KCF": 27 | self.tracker = KCFTracker(True, True, False) ## hog, fixed_window, multiscale 28 | else: 29 | print("wrong tracking algorithm") 30 | return 31 | 32 | def reset(self): 33 | try: 34 | if self.track_alg == "DEEP_SORT": 35 | self.tracker.release() #release gpu memory first 36 | del self.tracker 37 | self.load_model() 38 | except NameError: #not defined load 39 | self.load_model() 40 | #self.tracker.reset() 41 | self.frame_num =0 42 | 43 | #input detection: each row: [label_id, left, top, right, bottom, confidence] 44 | #curr_result, history(curr_result): [frame_idx, track_id, label_id, detection_index, x1,y1,x2,y2 ] 45 | def update(self, detections, bgr_image, frame_idx, force_matching=False): 46 | tlbrs = [] 47 | centers = [] 48 | confidence = [] 49 | labels = [] 50 | for det in detections: 51 | if len(det)<=0: 52 | continue 53 | left, top,right,bottom = det[1], det[2], det[3], det[4] 54 | labels.append(int(det[0])) 55 | tlbrs.append(np.array([int(left), int(top), int(right), int(bottom)])) 56 | centers.append(np.array([(left + right) / 2, (top + bottom) /2])) 57 | confidence.append(det[5]) 58 | # if not boxes: 59 | # return #empty box #need to track on no-detection cases 60 | if self.track_alg == "SIMPLE": 61 | self.tracker.update(centers) 62 | elif self.track_alg == "DEEP_SORT": 63 | self.tracker.update(bgr_image, np.array(tlbrs), confidence, labels) 64 | elif self.track_alg == "KCF": 65 | if(self.frame_num ==0): #KCF only works in single object case 66 | x,y,w,h = xywhs[0][0], tlbrs[0][1], xywhs[0][2]-xywhs[0][0], xywhs[0][3]-xywhs[0][1] 67 | self.tracker.init([x,y,w,h], bgr_image,labels[0]) 68 | self.frame_num = 0 69 | else: 70 | self.tracker.update(bgr_image) 71 | 72 | def get_current_result(self): 73 | curr_result = [] 74 | for tr in self.tracker.get_tracklets(): 75 | curr_result.append(tr) 76 | return curr_result 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /src/ui/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IntelLabs/csg/82a6e198eb62a9079a0f3442109ad4ac58f08e5a/src/ui/__init__.py -------------------------------------------------------------------------------- /src/ui/canvas.py: -------------------------------------------------------------------------------- 1 | #support zoom in/out, 2 | # https://stackoverflow.com/questions/41656176/tkinter-canvas-zoom-move-pan 3 | import tkinter as tk 4 | from tkinter import ttk 5 | from PIL import Image, ImageTk 6 | import math 7 | import warnings 8 | import cv2 9 | import hashlib 10 | 11 | MAX_ZOOM_RATE = 20 #20x 12 | MAX_IMAGE_PIXELS = 1500000000 # maximum pixels in the image, use it carefully 13 | 14 | class AutoScrollbar(ttk.Scrollbar): 15 | ''' A scrollbar that hides itself if it's not needed. 16 | Works only if you use the grid geometry manager ''' 17 | def set(self, lo, hi): 18 | if float(lo) <= 0.0 and float(hi) >= 1.0: 19 | # grid_remove is currently missing from Tkinter! 20 | self.tk.call("grid", "remove", self) 21 | else: 22 | self.grid() 23 | ttk.Scrollbar.set(self, lo, hi) 24 | 25 | def pack(self, **kw): 26 | raise tk.TclError('Cannot use pack with this widget') 27 | 28 | def place(self, **kw): 29 | raise tk.TclError('Cannot use place with this widget') 30 | 31 | class ZoomFrame(ttk.Frame): 32 | ''' Simple zoom with mouse wheel ''' 33 | def __init__(self, mainframe, **opt): 34 | ''' Initialize the main Frame ''' 35 | self.frameWidth = opt['width'] 36 | self.frameHeight = opt['height'] 37 | self.bg = opt['background'] 38 | self.zoomable = False #default 39 | ttk.Frame.__init__(self, master=mainframe, width=self.frameWidth, height=self.frameHeight) 40 | self.init_layout() 41 | self.roi = [] 42 | 43 | def init_layout(self): 44 | # Vertical and horizontal scrollbars for canvas 45 | self.vbar = AutoScrollbar(self.master, orient='vertical') 46 | self.hbar = AutoScrollbar(self.master, orient='horizontal') 47 | self.vbar.grid(row=0, column=1, sticky='ns') 48 | self.hbar.grid(row=1, column=0, sticky='we') 49 | # Create canvas and put image on it 50 | self.canvas = tk.Canvas(self.master, highlightthickness=0, 51 | width = self.frameWidth, height = self.frameHeight, 52 | xscrollcommand=self.hbar.set, yscrollcommand=self.vbar.set, 53 | background=self.bg) 54 | self.container = None 55 | self.canvas.grid(row=0, column=0, sticky='nswe') 56 | self.vbar.configure(command=self.scroll_y) # bind scrollbars to the canvas 57 | self.hbar.configure(command=self.scroll_x) 58 | self.origin = self.canvas.xview()[0], self.canvas.yview()[0] 59 | # Make the canvas expandable 60 | self.master.rowconfigure(0, weight=1) 61 | self.master.columnconfigure(0, weight=1) 62 | # Bind events to the Canvas 63 | self.canvas.bind('', self.show_image) # canvas is resized 64 | self.canvas.bind('', self.wheel) # with Windows and MacOS, but not Linux 65 | self.canvas.bind('', self.wheel) # only with Linux, wheel scroll down 66 | self.canvas.bind('', self.wheel) # only with Linux, wheel scroll up 67 | self.image = None 68 | 69 | def load_image(self, image_pil): 70 | self.image_raw = image_pil 71 | self.image = image_pil 72 | self.width, self.height = self.image.size 73 | self.container = self.canvas.create_rectangle(0, 0, self.width, self.height, width=0) 74 | self.imscale = 1.0 75 | self.delta = 0.75 76 | self.show_image() 77 | 78 | def scroll_y(self, *args, **kwargs): 79 | ''' Scroll canvas vertically and redraw the image ''' 80 | self.canvas.yview(*args, **kwargs) # scroll vertically 81 | self.show_image() # redraw the image 82 | 83 | def scroll_x(self, *args, **kwargs): 84 | ''' Scroll canvas horizontally and redraw the image ''' 85 | self.canvas.xview(*args, **kwargs) # scroll horizontally 86 | self.show_image() # redraw the image 87 | 88 | def move_from(self, event): 89 | ''' Remember previous coordinates for scrolling with the mouse ''' 90 | self.canvas.scan_mark(event.x, event.y) 91 | 92 | def move_to(self, event): 93 | ''' Drag (move) canvas to the new position ''' 94 | self.canvas.scan_dragto(event.x, event.y, gain=1) 95 | self.show_image() 96 | 97 | def mouse_click(self, event): 98 | print('Canvas level: mouse click') 99 | pass 100 | 101 | 102 | def wheel(self, event): 103 | ''' Zoom with mouse wheel ''' 104 | if not self.zoomable: 105 | return 106 | x = self.canvas.canvasx(event.x) 107 | y = self.canvas.canvasy(event.y) 108 | if self.container is None: #no image load return 109 | return 110 | 111 | bbox = self.canvas.bbox(self.container) # get image area 112 | if bbox[0] < x < bbox[2] and bbox[1] < y < bbox[3]: pass # Ok! Inside the image 113 | else: return # zoom only inside image area 114 | scale = 1.0 115 | # Respond to Linux (event.num) or Windows (event.delta) wheel event 116 | if event.num == 5 or event.delta == -120: # scroll down 117 | i = min(self.width, self.height) 118 | #if int(i * self.imscale) < 30: return # image is less than 30 pixels 119 | new_scale = self.imscale / self.delta 120 | if new_scale>MAX_ZOOM_RATE: return 121 | self.imscale /= self.delta 122 | scale /= self.delta 123 | if event.num == 4 or event.delta == 120: # scroll up 124 | i = min(self.canvas.winfo_width(), self.canvas.winfo_height()) 125 | if i < self.imscale: return # 1 pixel is bigger than the visible area 126 | new_scale = self.imscale*self.delta 127 | if new_scale <1.0: return #disable smaller than the original image 128 | self.imscale *= self.delta 129 | scale *= self.delta 130 | self.canvas.scale('all', x, y, scale, scale) # rescale all canvas objects 131 | self.show_image() 132 | 133 | 134 | def show_image(self, event=None): 135 | ''' Show image on the Canvas ''' 136 | if self.container is None: 137 | return 138 | bbox1 = self.canvas.coords(self.container) # get image area 139 | # Remove 1 pixel shift at the sides of the bbox1 140 | bbox1 = (bbox1[0] + 1, bbox1[1] + 1, bbox1[2] - 1, bbox1[3] - 1) 141 | bbox2 = (self.canvas.canvasx(0), # get visible area of the canvas 142 | self.canvas.canvasy(0), 143 | self.canvas.canvasx(self.canvas.winfo_width()), 144 | self.canvas.canvasy(self.canvas.winfo_height())) 145 | 146 | bbox = [min(bbox1[0], bbox2[0]), min(bbox1[1], bbox2[1]), # get scroll region box 147 | max(bbox1[2], bbox2[2]), max(bbox1[3], bbox2[3])] 148 | if bbox[0] == bbox2[0] and bbox[2] == bbox2[2]: # whole image in the visible area 149 | bbox[0] = bbox1[0] 150 | bbox[2] = bbox1[2] 151 | if bbox[1] == bbox2[1] and bbox[3] == bbox2[3]: # whole image in the visible area 152 | bbox[1] = bbox1[1] 153 | bbox[3] = bbox1[3] 154 | self.canvas.configure(scrollregion=bbox) # set scroll region 155 | x1 = max(bbox2[0] - bbox1[0], 0) # get coordinates (x1,y1,x2,y2) of the image tile 156 | y1 = max(bbox2[1] - bbox1[1], 0) 157 | x2 = min(bbox2[2], bbox1[2]) - bbox1[0] 158 | y2 = min(bbox2[3], bbox1[3]) - bbox1[1] 159 | if int(x2 - x1) > 0 and int(y2 - y1) > 0: # show image if it in the visible area 160 | x = min(int(x2 / self.imscale), self.width) # sometimes it is larger on 1 pixel... 161 | y = min(int(y2 / self.imscale), self.height) # ...and sometimes not 162 | image = self.image.crop((int(x1 / self.imscale), int(y1 / self.imscale), x, y)) 163 | #image.save('test.jpg') 164 | self.roi = [int(x1 / self.imscale), int(y1 / self.imscale), x, y, 165 | int(x2-x1), int(y2-y1), 166 | max(bbox2[0], bbox1[0]), max(bbox2[1], bbox1[1])] 167 | imagetk = ImageTk.PhotoImage(image.resize((int(x2 - x1), int(y2 - y1)))) 168 | imageid = self.canvas.create_image(max(bbox2[0], bbox1[0]), max(bbox2[1], bbox1[1]), 169 | anchor='nw', image=imagetk) 170 | self.canvas.lower(imageid) # set image into background 171 | self.canvas.imagetk = imagetk # keep an extra reference to prevent garbage-collection 172 | 173 | def reset_canvas(self): 174 | '''scale=1, fit to canvas''' 175 | bbox = self.canvas.coords(self.container) 176 | if bbox[0]==0 and bbox[1]==0 and bbox[2]== self.frameWidth and bbox[3]==self.frameHeight: 177 | return 178 | #TODO: any solution instead of delete first 179 | for children in self.master.winfo_children(): 180 | children.destroy() 181 | 182 | self.init_layout() 183 | self.load_image(self.image_raw) 184 | 185 | def set_zoomable(self, zoomable=False): 186 | self.zoomable = zoomable 187 | if (not self.zoomable) and (not self.image is None): 188 | self.reset_canvas() 189 | 190 | def get_pixel_values(self, points): 191 | #convert canvas to pixel 192 | pt_pixel = [] 193 | for pt in points: 194 | x =(pt[0] - self.roi[-2])/self.imscale +self.roi[0] 195 | y = (pt[1]-self.roi[-1])/self.imscale + self.roi[1] 196 | pt_pixel.append([x,y]) 197 | #drw on raw image and viz 198 | # from PIL import ImageDraw 199 | # draw = ImageDraw.Draw(self.image_raw) 200 | # print('pixel: ', pt_pixel) 201 | # for pt in pt_pixel: 202 | # x,y = pt[0], pt[1] 203 | # r = 5 204 | # leftUpPoint = (x-r, y-r) 205 | # rightDownPoint = (x+r, y+r) 206 | # draw.ellipse([leftUpPoint, rightDownPoint], fill=(255,0,0,255)) 207 | # self.image_raw.save('pixel.jpg') 208 | 209 | return pt_pixel 210 | -------------------------------------------------------------------------------- /src/ui/color.py: -------------------------------------------------------------------------------- 1 | import random 2 | import numpy as np 3 | import secrets 4 | #ref: http://bootflat.github.io/color-picker.html 5 | hex_colors = ['#edbc6d','#dd828e','#bada55','#588585','#e39a31','#f48670','#9dffb0', 6 | '#e8c15f','#c90579','#eb491e','#9a6d41','#7cb07c','#91a6b4','#f8b6a8', 7 | '#008080','#548b54'] 8 | marker_colors = {"Vanishing Point":"red", "Parallel Line":"red", "Ref Point":"yellow", "Ref Line":"yellow", 9 | "East":"red", "North":"blue", "West":"yellow", "South":"green"} 10 | def rand_rgb(): 11 | secretGenerator = secrets.SystemRandom() 12 | r = secretGenerator.randint(0,255) 13 | g = secretGenerator.randint(0,255) 14 | b = secretGenerator.randint(0,255) 15 | #r = random.randint(0,255) 16 | #g = random.randint(0, 255) 17 | #b = random.randint(0, 255) 18 | return r,g,b 19 | 20 | def rand_hex(): 21 | de, re, we = rand_rgb() 22 | return rgb_to_hex(de, re, we) 23 | 24 | def rgb_to_hex(de, re, we): 25 | return "#%02x%02x%02x"%(de, re,we) 26 | 27 | def hex_to_rgb(hex): 28 | hex = hex.lstrip('#') 29 | return tuple(int(hex[i:i+2], 16) for i in (0, 2, 4)) 30 | 31 | def norm_rgb(arr): 32 | for i in range(3): 33 | arr[i] = min(255, max(0, arr[i])) 34 | return arr 35 | 36 | def alpha_hex(hex, alpha=0.6): 37 | rgb = hex_to_rgb(hex) 38 | rgb = (np.asarray(rgb)*alpha+(1-alpha)*255).astype(int) 39 | rgb = norm_rgb(rgb) 40 | return rgb_to_hex(rgb[0], rgb[1], rgb[2]) 41 | 42 | def scale_hex(hex, scale): 43 | rgb = hex_to_rgb(hex) 44 | rgb = (np.asarray(rgb)* scale).astype(int) 45 | rgb = norm_rgb(rgb) 46 | return rgb_to_hex(rgb[0], rgb[1], rgb[2]) 47 | 48 | def get_hex_by_index(index): 49 | color_num = len(hex_colors) 50 | index = max(0, index) 51 | index = min(color_num-1, index) 52 | return hex_colors[index] 53 | 54 | def get_finegrained_color(tag, index): 55 | if tag=='red': 56 | lst = ['#FF0000', '#CF000F','#D24D57','#DB5A6B','#C93756','#DC3023'] 57 | elif tag=='yellow': 58 | lst = ['#FFFF00','#D9B611','#F5D76E','#FFB61E','#FFA400','#E29C45'] 59 | 60 | return lst[index%len(lst)] -------------------------------------------------------------------------------- /src/ui/drag_rect.py: -------------------------------------------------------------------------------- 1 | from src.ui.color import alpha_hex,scale_hex 2 | 3 | class DragRect(): 4 | def __init__(self, parent, canvas, points, color, title="", 5 | objectID=0, text_font=None): 6 | self.parent = parent 7 | self.previous_x = None 8 | self.previous_y = None 9 | self.selected = None 10 | self.color = color 11 | self.canvas = canvas 12 | self.title = title 13 | self.objectID = objectID #used to distinguish objects 14 | self.freezeMode = False #cannot move objects 15 | 16 | self.alpha = 0.2 17 | self.line_scale = 1.6 18 | self.radius = 5 19 | self.line_thickness = 3 20 | self.text_offset = [15,-15] 21 | self.text_color = color 22 | if text_font is None: 23 | self.text_font = ('Helvetica', 12) 24 | else: 25 | self.text_font = text_font 26 | 27 | #given 2 points, augment to 4 corner points 28 | self.points = points #len(points)==4 29 | self.fill_color = alpha_hex(color, alpha=self.alpha) #alpha blending 30 | self.line_color = scale_hex(color, scale=self.line_scale) 31 | 32 | self.text = self.canvas.create_text(self.points[0][0]+self.text_offset[0], self.points[0][1]+self.text_offset[1], 33 | text=self.title, fill=self.text_color, font=self.text_font,tag='dragRect') 34 | 35 | self.polygon = canvas.create_rectangle(self.points, width=self.line_thickness, outline=self.line_color, 36 | tag='dragRect', activefill=self.fill_color) 37 | self.canvas.tag_bind(self.polygon, '', lambda event, tag=self.polygon: self.on_press_tag(event, 0, tag)) 38 | self.canvas.tag_bind(self.polygon, '', lambda event, tag=self.polygon: self.on_release_tag(event, 0, tag)) 39 | self.canvas.tag_bind(self.polygon, '', self.on_move_rect) 40 | 41 | self.all_object_list = [self.polygon] 42 | self.all_object_list.append(self.text) 43 | 44 | # nodes - red rectangles 45 | self.nodes = [] 46 | for number, point in enumerate(self.points): 47 | x, y = point 48 | # node = canvas.create_rectangle((x-self.radius, y-self.radius, x+self.radius, y+self.radius), fill=self.color, outline= self.line_color) 49 | node = canvas.create_oval(x-self.radius, y-self.radius, x+self.radius, y+self.radius, fill=self.color, 50 | outline= self.line_color, tag='dragRect') 51 | self.nodes.append(node) 52 | self.canvas.tag_bind(node, '', lambda event, number=number, tag=node: self.on_press_tag(event, number, tag)) 53 | self.canvas.tag_bind(node, '', lambda event, number=number, tag=node: self.on_release_tag(event, number, tag)) 54 | self.canvas.tag_bind(node, '', lambda event, number=number: self.on_move_node(event, number)) 55 | self.all_object_list.append(node) 56 | 57 | def on_press_tag(self, event, number, tag): 58 | self.selected = tag 59 | self.previous_x = self.canvas.canvasx(event.x)# event.x 60 | self.previous_y = self.canvas.canvasy(event.y)#event.y 61 | 62 | def on_release_tag(self, event, number, tag): 63 | self.selected = None 64 | self.previous_x = None 65 | self.previous_y = None 66 | #move single node 67 | def on_move_node(self, event, number): 68 | if self.freezeMode: 69 | return 70 | if self.selected: 71 | dx = self.canvas.canvasx(event.x)-self.previous_x#event.x - self.previous_x 72 | dy = self.canvas.canvasy(event.y)-self.previous_y#event.y - self.previous_y 73 | 74 | self.canvas.move(self.selected, dx, dy) 75 | 76 | #update all points, always binding to 2 corner points 77 | for i in range(2): 78 | curr_coords = self.canvas.coords(self.nodes[i]) 79 | self.points[i][0] = int((curr_coords[0]+curr_coords[2])/2) 80 | self.points[i][1] = int((curr_coords[1]+curr_coords[3])/2) 81 | if i==0: #always binding to the first point 82 | text_coords = curr_coords[2:4] 83 | text_coords[0] = max(curr_coords[0], curr_coords[2]) 84 | text_coords[1] = min(curr_coords[1], curr_coords[3]) 85 | 86 | coords = sum(self.points, []) #flatten 87 | # change points in polygons 88 | self.canvas.coords(self.polygon, coords) 89 | 90 | if number ==0: 91 | text_coords[0] += self.text_offset[0] 92 | text_coords[1] += self.text_offset[1] 93 | self.canvas.coords(self.text, text_coords) 94 | 95 | self.previous_x = self.canvas.canvasx(event.x)#event.x 96 | self.previous_y = self.canvas.canvasy(event.y)#event.y 97 | 98 | self.parent.update_dragrect(self.objectID) 99 | def on_move_rect(self, event): 100 | if self.freezeMode: 101 | return 102 | if self.selected: 103 | dx = self.canvas.canvasx(event.x)-self.previous_x#event.x - self.previous_x 104 | dy = self.canvas.canvasy(event.y)-self.previous_y#event.y - self.previous_y 105 | #dx = event.x - self.previous_x 106 | #dy = event.y - self.previous_y 107 | 108 | #move text 109 | self.canvas.move(self.text, dx, dy) 110 | 111 | # move polygon 112 | self.canvas.move(self.selected, dx, dy) 113 | 114 | # move corner nodes 115 | for item in self.nodes: 116 | self.canvas.move(item, dx, dy) 117 | 118 | # recalculate values in self.points 119 | for item in self.points: 120 | item[0] += dx 121 | item[1] += dy 122 | 123 | self.previous_x = self.canvas.canvasx(event.x)#event.x 124 | self.previous_y = self.canvas.canvasy(event.y)#eve 125 | # self.previous_x = event.x 126 | # self.previous_y = event.y 127 | self.parent.update_dragrect(self.objectID) 128 | 129 | def erase(self): 130 | self.canvas.delete(self.polygon) 131 | self.canvas.delete(self.text) 132 | for node in self.nodes: 133 | self.canvas.delete(node) 134 | 135 | def set_title(self, new_str): 136 | self.canvas.itemconfig(self.text, text=new_str) 137 | strss = new_str.split(' ') 138 | self.objectID = int(strss[1]) 139 | self.title = new_str 140 | 141 | def freeze(self, freezable): 142 | self.freezeMode = freezable 143 | 144 | def highlight(self, highlight=True): 145 | if highlight: 146 | self.canvas.itemconfig(self.polygon,fill=self.fill_color) 147 | else: 148 | self.canvas.itemconfig(self.polygon, fill='') -------------------------------------------------------------------------------- /src/ui/image_utils.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | from PIL import Image,ImageTk 4 | import src.road.retinex as retinex 5 | import copy 6 | import random 7 | import secrets 8 | 9 | CALIBRATION_WIDTH = 1920 10 | def normTransMat(x): 11 | num = x.shape[1] 12 | y = np.zeros((3, num)) 13 | for i in range(0, num): 14 | y[0, i] = x[0, i] / x[2, i] 15 | y[1, i] = x[1, i] / x[2, i] 16 | y[2, i] = 1 17 | return y 18 | 19 | #compatible to all resolutions, both horizontal and vertical 20 | def fit_image_to_canvas(image, desired_width=640, desired_height=480, random_pad=False, padding_value=[0,0,0]): 21 | """Resizes an image keeping the aspect ratio mostly unchanged. 22 | 23 | Returns: 24 | image: the resized image 25 | window: (x1, y1, x2, y2). If max_dim is provided, padding might 26 | be inserted in the returned image. If so, this window is the 27 | coordinates of the image part of the full image (excluding 28 | the padding). The x2, y2 pixels are not included. 29 | scale: The scale factor used to resize the image 30 | padding: Padding added to the image [left, top, right, bottom] 31 | """ 32 | # Default window (x1, y1, x2, y2) and default scale == 1. 33 | h, w = image.shape[0], image.shape[1] 34 | #import pudb;pudb.set_trace() 35 | desired_ap = desired_height/desired_width 36 | image_ap = h/w 37 | if image_ap<=desired_ap: 38 | new_width = desired_width 39 | new_height = int((desired_width*h)/w ) 40 | else: 41 | new_width = int((desired_height*w)/h) 42 | new_height = desired_height 43 | 44 | width_scale = new_width / w 45 | height_scale = new_height / h 46 | scale = min(width_scale, height_scale) 47 | 48 | # Resize image using bilinear interpolation 49 | if scale != 1: 50 | image = cv2.resize(image, (round(w * scale), round(h * scale))) 51 | h,w, _ = image.shape 52 | y_pad = desired_height - h 53 | x_pad = desired_width - w 54 | secretGenerator = secrets.SystemRandom() 55 | top_pad = secretGenerator.randint(0, y_pad) if random_pad else y_pad // 2 56 | left_pad = secretGenerator.randint(0, x_pad) if random_pad else x_pad // 2 57 | 58 | padding = (left_pad, top_pad, x_pad - left_pad, y_pad - top_pad) 59 | # assert all([x >= 0 for x in padding]) 60 | image = cv2.copyMakeBorder(image, top_pad, y_pad-top_pad, left_pad, x_pad-left_pad, cv2.BORDER_CONSTANT, value=padding_value) 61 | 62 | #image = functional.pad(image, padding) 63 | window = [left_pad, top_pad, w + left_pad, h + top_pad] 64 | 65 | return image, window, scale, padding 66 | 67 | def resize_padding_image(image, scale, padding, padding_value =[0,0,0]): 68 | w, h = image.shape[1], image.shape[0] 69 | if scale != 1: 70 | image = cv2.resize(image, (round(w * scale),round(h * scale))) 71 | image = cv2.copyMakeBorder(image, padding[1], padding[3], padding[0], padding[2], cv2.BORDER_CONSTANT, value=padding_value) 72 | return image 73 | 74 | 75 | def convert_hls_image(image_data): 76 | #resize image 77 | w,h = image_data.shape[1], image_data.shape[0] 78 | new_height = int(CALIBRATION_WIDTH*h/w) 79 | I = cv2.resize(image_data, (CALIBRATION_WIDTH, new_height))#BRG mode 80 | hls = cv2.cvtColor(I, cv2.COLOR_BGR2HLS)#cv2.COLOR_RGB2HLS) 81 | lower = np.uint8([0, 150, 0]) 82 | upper = np.uint8([255, 255, 255]) 83 | mask = cv2.inRange(hls, lower, upper) 84 | Threshold1 = 150 85 | Threshold2 = 350 86 | FilterSize = 5 87 | E = cv2.Canny(mask, Threshold1, Threshold2, FilterSize) 88 | lines = cv2.HoughLinesP(E, rho=1, theta=np.pi / 180, threshold=50, minLineLength=50, maxLineGap=20) 89 | N = lines.shape[0] 90 | for i in range(N): 91 | x1 = lines[i][0][0] 92 | y1 = lines[i][0][1] 93 | x2 = lines[i][0][2] 94 | y2 = lines[i][0][3] 95 | # cv2.line(I, (x1, y1), (x2, y2), (255, 0, 0), 2) 96 | cv2.line(I, (x1, y1), (x2, y2), (0, 0, 255), 2) 97 | #scale-back to original image 98 | I = cv2.resize(I, (w,h)) 99 | return I#, scale 100 | 101 | def convert_perspective_image_old(image_data, froll, fpitch, fyaw, cameraParam, canvas_size=400): 102 | newsize = (1920, 1080) 103 | #old_w, old_h = image_data.shape[1], image_data.shape[0] 104 | cameraParam.setMatrix_R(froll, fpitch, fyaw) 105 | cameraParam.calMatrix_P() 106 | trsnsMat=cameraParam.Mat_P[:, (0, 1, 3)] 107 | gray = cv2.cvtColor(image_data, cv2.COLOR_BGR2GRAY) 108 | gray = cv2.resize(gray, newsize) 109 | 110 | if 1:#for debug 111 | #trsnsMat=[[1,0.2,2] ,[0.2,1,0], [0,0,1]] 112 | trsnsMat=[[6.81183210e+02, 5.35969903e+02, -4.00813033e+06],[-6.89487113e+01, -1.01957980e+02, -4.23842045e+06],[8.50933143e-02, 6.46864574e-01, -3.97867346e+03]] 113 | trsnsMat=np.asarray(trsnsMat, dtype=np.float64) 114 | Umap = np.zeros((5000, 5000)) 115 | for i in range(200, 1900, 2): 116 | for j in range(100, 1080, 2): 117 | temp = normTransMat(np.dot(np.linalg.inv(trsnsMat), np.array([i, j, 1]).reshape((3, 1)))) 118 | temp = temp / 10 119 | temp[0] = temp[0] 120 | y = int(np.round(temp[1]) + 2000) 121 | x = int(np.round(temp[0]) + 2000) 122 | 123 | if (x<0) or (x>5000) or (y<0) or (y>5000): 124 | continue 125 | Umap[y, x] = gray[j, i] 126 | #gray2 = cv2.imresize(Umap, (old_w, old_h)) 127 | gray2 = cv2.resize(Umap, (canvas_size,canvas_size)) #canvas size in main.py 128 | pil_image = Image.fromarray(gray2) 129 | #xinxin image_tk = ImageTk.PhotoImage(pil_image) 130 | image_tk = gray2 131 | return image_tk, cameraParam 132 | 133 | def convert_perspective_image(image_data, froll, fpitch, fyaw, cameraParam, canvas_size=400): 134 | newsize = (1920, 1080) 135 | #old_w, old_h = image_data.shape[1], image_data.shape[0] 136 | cameraParam.setMatrix_R(froll, fpitch, fyaw) 137 | cameraParam.calMatrix_P() 138 | trsnsMat=cameraParam.Mat_P[:, (0, 1, 3)] 139 | Mat_R=cameraParam.Mat_R * 10 140 | print("Mat_R is", Mat_R, image_data.shape[::-1][1:]) 141 | 142 | image_perspective = cv2.warpPerspective(image_data, Mat_R, image_data.shape[::-1][1:], flags=cv2.INTER_LINEAR) 143 | 144 | #xinxin image_tk = ImageTk.PhotoImage(pil_image) 145 | image_tk = image_perspective 146 | return image_tk, cameraParam 147 | 148 | 149 | def convert_road_enhance_image(image_data): 150 | old_w, old_h = image_data.shape[1], image_data.shape[0] 151 | sigma_list = [15, 80, 250] 152 | new_height = int(CALIBRATION_WIDTH*old_h/old_w) 153 | resized_im = cv2.resize(image_data, (CALIBRATION_WIDTH, new_height))#BRG mode 154 | pil_im = Image.fromarray(cv2.cvtColor(resized_im, cv2.COLOR_BGR2RGB)) 155 | img_amsrcr = retinex.automatedMSRCR(pil_im, sigma_list) 156 | img_amsrcr[np.where((img_amsrcr!=[255,255,255]).all(axis=2))]=[128,128,128] 157 | cv_image = np.array(img_amsrcr) 158 | cv_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) 159 | cv_image = cv2.resize(cv_image, (old_w, old_h)) 160 | return cv_image 161 | 162 | 163 | def get_pixel_coordinate(canvas_x, canvas_y, scale, padding): 164 | #get pixel coordinate from canvas coordinate 165 | top_pad, left_pad = padding[1],padding[0] 166 | 167 | pixel_x = canvas_x-left_pad 168 | pixel_y = canvas_y - top_pad 169 | pixel_x = round(pixel_x/scale) 170 | pixel_y = round(pixel_y/scale) 171 | return pixel_x, pixel_y 172 | def get_canvas_coordinate(pixel_x, pixel_y, scale, padding): 173 | top_pad, left_pad = padding[1],padding[0] 174 | canvas_x = round(pixel_x * scale) 175 | canvas_y = round(pixel_y*scale) 176 | 177 | canvas_x = canvas_x + left_pad 178 | canvas_y = canvas_y + top_pad 179 | return canvas_x, canvas_y 180 | 181 | def convert_canvas_to_pixel(canvas_pt_list, scale, pad): 182 | pixels = copy.deepcopy(canvas_pt_list) 183 | for i, pt_groups in enumerate(canvas_pt_list): 184 | num_pt = len(pt_groups)//2 185 | for j in range(num_pt): 186 | canvas_x, canvas_y = pt_groups[2*j], pt_groups[2*j+1] 187 | pixel_x, pixel_y = get_pixel_coordinate(canvas_x, canvas_y, scale, pad) 188 | pixels[i][2*j] = pixel_x 189 | pixels[i][2*j+1] = pixel_y 190 | return pixels 191 | 192 | def convert_pixel_to_canvas(pixel_pt_list, scale, pad): 193 | canvass = copy.deepcopy(pixel_pt_list) 194 | for i, pt_groups in enumerate(pixel_pt_list): 195 | num_pt = len(pt_groups)//2 196 | for j in range(num_pt): 197 | pixel_x, pixel_y = pt_groups[2*j], pt_groups[2*j+1] 198 | canvas_x, canvas_y = get_canvas_coordinate(pixel_x, pixel_y, scale, pad) 199 | canvass[i][2*j] = canvas_x 200 | canvass[i][2*j+1] = canvas_y 201 | return canvass 202 | ''' 203 | def resize_image_calibration(image_data): 204 | w, h = image_data.shape[1], image_data.shape[0] 205 | if w >= h: 206 | baseW = 800 207 | wpercent = (baseW / float(w)) 208 | hsize = int((float(h) * float(wpercent))) 209 | im_scaled = cv2.resize(image_data, (baseW, hsize)) 210 | else: 211 | baseH = 450 212 | wpercent = (baseH / float(h)) 213 | wsize = int((float(w) * float(wpercent))) 214 | im_scaled = cv2.resize(image_data, (wsize, baseH)) 215 | scale = np.ones((2,1)) 216 | new_w, new_h = im_scaled.shape[1], im_scaled.shape[0] 217 | scale[0] = w/(float)(new_w) 218 | scale[1] = h/(float)(new_h) 219 | return im_scaled, scale 220 | 221 | ''' 222 | -------------------------------------------------------------------------------- /src/ui/video_clipper.py: -------------------------------------------------------------------------------- 1 | 2 | from tkinter import * 3 | from tkinter import ttk, messagebox,filedialog 4 | import cv2 5 | from PIL import Image as PILImage 6 | from PIL import ImageTk 7 | from .image_utils import fit_image_to_canvas 8 | #import image_utils as imgUtil 9 | #compatible to all resolutions, both horizontal and vertical 10 | 11 | class VideoClipperWindow(Toplevel): 12 | def __init__(self, master, videoCap, width=600, height=500): 13 | top = self.top = self 14 | self.width= width 15 | self.height = height 16 | self.videoCap = videoCap 17 | Toplevel.__init__(self, master, width=self.width, height = self.height) 18 | x = master.winfo_x() 19 | y = master.winfo_y() 20 | self.geometry('{}x{}+{}+{}'.format(width,height, x+200,y+50)) 21 | self.protocol("WM_DELETE_WINDOW",self.destroy) 22 | #self.attributes('-topmost',True) 23 | self.transient() 24 | self.resizable(False, False) 25 | self.title = Label(self, text="Frame") 26 | self.title.pack(side=TOP) 27 | self.title.config(font=("Courier", 20)) 28 | 29 | self.canvas_height = self.height-200 30 | self.canvas = Canvas(self, highlightthickness=0, 31 | width = self.width, height = self.canvas_height, 32 | background="#90AFC5") 33 | self.canvas.pack(side=TOP) 34 | self.navigation_frame = Frame(self) 35 | self.navigation_frame.pack(side=TOP, ipady=20) 36 | self.set_frame=Frame(self) 37 | self.set_frame.pack(side=TOP, ipady=20) 38 | self.command_frame = Frame(self) 39 | self.command_frame.pack(side=TOP, ipady=20) 40 | 41 | self.gotoFirstBtn = Button(self.navigation_frame, text="|<", command=self.goto_first) 42 | self.gotoPrev10Btn = Button(self.navigation_frame, text="<<", command=self.goto_prev10) 43 | self.gotoPrevBtn = Button(self.navigation_frame, text="<", command=self.goto_prev) 44 | self.playBtn = Button(self.navigation_frame,text="|>", command=self.play) 45 | self.gotoNextBtn = Button(self.navigation_frame, text=">", command=self.goto_next) 46 | self.gotoNext10Btn = Button(self.navigation_frame, text=">>", command=self.goto_next10) 47 | self.gotoLastBtn = Button(self.navigation_frame, text=">|", command=self.goto_last) 48 | self.gotoFirstBtn.pack(fill=X, side=LEFT, padx=1, expand=0) 49 | self.gotoPrev10Btn.pack(fill=X, side=LEFT, padx=1, expand=0) 50 | self.gotoPrevBtn.pack(fill=X, side=LEFT, padx=1, expand=0) 51 | self.playBtn.pack(fill=X, side=LEFT, padx=1, expand=0) 52 | self.gotoNextBtn.pack(fill=X, side=LEFT, padx=1, expand=0) 53 | self.gotoNext10Btn.pack(fill=X, side=LEFT, padx=1, expand=0) 54 | self.gotoLastBtn.pack(fill=X, side=LEFT, padx=1, expand=0) 55 | 56 | self.startVar = StringVar() 57 | self.endVar = StringVar() 58 | self.startVar.trace("w", lambda name, index, mode, sv=self.startVar: self.start_callback(sv)) 59 | self.endVar.trace("w", lambda name, index, mode, sv=self.endVar: self.end_callback(sv)) 60 | 61 | self.setStartBtn = Button(self.set_frame, text="Set as Start", command=self.set_start) 62 | self.setStartBtn.pack(fill=X, side=LEFT, padx=5, expand=0) 63 | self.startEntry = Entry(self.set_frame, textvariable=self.startVar) 64 | self.startEntry.pack(fill=X, side=LEFT, padx=5, expand=0) 65 | Label(self.set_frame, text="-").pack(fill=X, side=LEFT, padx=1, expand=0) 66 | self.endEntry = Entry(self.set_frame,textvariable=self.endVar) 67 | self.endEntry.pack(fill=X, side=LEFT, padx=5, expand=0) 68 | self.setEndBtn = Button(self.set_frame, text="Set as End", command=self.set_end) 69 | self.setEndBtn.pack(fill=X, side=LEFT, padx=5, expand=0) 70 | 71 | 72 | self.clipBtn = Button(self.command_frame, text="Clip&Save", command=self.clip_save) 73 | self.cancelBtn = Button(self.command_frame, text="Cancel", command=self.cancel) 74 | 75 | self.clipBtn.pack(side=LEFT, padx=10, expand=False) 76 | self.cancelBtn.pack(side=RIGHT, padx=10, expand=False) 77 | 78 | self.currFrameId = 0 79 | self.pauseMode = False 80 | self.totalFrameNum = int(self.videoCap.get(7)) 81 | self.saver_filename = None 82 | 83 | self.goto() 84 | def start_callback(self, sv): 85 | char = sv.get() 86 | try: 87 | val = int(char) 88 | except: 89 | sv.set("") 90 | return 91 | if val <0 or val>self.totalFrameNum-2: 92 | sv.set("") 93 | 94 | def end_callback(self, sv): 95 | char = sv.get() 96 | try: 97 | val = int(char) 98 | except: 99 | sv.set("") 100 | return 101 | if val <1 or val>self.totalFrameNum: 102 | sv.set("") 103 | 104 | def set_start(self): 105 | self.startEntry.delete(0,END) 106 | self.startEntry.insert(0,str(self.currFrameId+1)) 107 | self.startVar.set(str(self.currFrameId+1)) 108 | 109 | def set_end(self): 110 | self.endEntry.delete(0,END) 111 | self.endEntry.insert(0,str(self.currFrameId+1)) 112 | self.endVar.set(str(self.currFrameId+1)) 113 | 114 | def goto(self): 115 | textstr = "Frame: {}".format(self.currFrameId+1) 116 | self.title.config(text=textstr) 117 | self.videoCap.set(cv2.CAP_PROP_POS_FRAMES, self.currFrameId) 118 | _, image_raw = self.videoCap.read() 119 | self.imgDisp, _, self.imgScale, self.imgPadding = fit_image_to_canvas(image_raw, 120 | desired_width=self.width, desired_height=self.canvas_height) 121 | rgb = cv2.cvtColor(self.imgDisp, cv2.COLOR_BGR2RGB) 122 | self.tkImg = ImageTk.PhotoImage(PILImage.fromarray(rgb)) 123 | imageid = self.canvas.create_image(0,0,anchor='nw', image=self.tkImg) 124 | self.canvas.lower(imageid) # set image into background 125 | 126 | def clip_save(self): 127 | start_fid = int(self.startVar.get())-1 128 | end_fid = int(self.endVar.get())-1 129 | if start_fid <0 or end_fid<=start_fid: 130 | messagebox.showwarning("Warning", "Invalid start/end frame number!") 131 | return 132 | saver_file = filedialog.asksaveasfilename(title='Save As', filetypes=(("video files", "*.mp4"), ("video files", "*.avi"))) 133 | if saver_file: 134 | self.saver_filename = saver_file 135 | width = int(self.videoCap.get(3)) 136 | height = int(self.videoCap.get(4)) 137 | fps = self.videoCap.get(5) 138 | suffix = saver_file[-4:] 139 | if suffix =='.avi': 140 | fourcc = cv2.VideoWriter_fourcc(*'XVID') 141 | elif suffix=='.mp4': 142 | fourcc = cv2.VideoWriter_fourcc(*'mp4v')#MP4V')#(*'MPEG')#cv2.VideoWriter_fourcc('M','J','P','G') 143 | out = cv2.VideoWriter(saver_file,fourcc, fps, (width,height)) 144 | if(out.isOpened()): 145 | for i in range(start_fid, end_fid+1): 146 | self.videoCap.set(cv2.CAP_PROP_POS_FRAMES, i) 147 | _, image_raw = self.videoCap.read() 148 | out.write(image_raw) 149 | out.release() 150 | self.destroy() 151 | def cancel(self): 152 | self.saver_filename = None 153 | self.destroy() 154 | 155 | def play_video(self): 156 | self.currFrameId += 1 157 | self.currFrameId = max(0, min(self.totalFrameNum-1, self.currFrameId)) 158 | self.goto() 159 | if not self.pauseMode: 160 | self.navigation_frame.after(15, self.play_video) 161 | if self.currFrameId == (self.totalFrameNum-1): #finished all play 162 | self.playBtn['text']='|>' 163 | self.pauseMode = True 164 | def play(self): 165 | if self.playBtn['text'] =='||': 166 | self.pauseMode = True 167 | elif self.playBtn['text'] =='|>': 168 | self.pauseMode = False 169 | if not self.pauseMode: 170 | self.playBtn['text'] = '||' #change to pause 171 | #resume play 172 | self.pauseMode = False 173 | self.play_video() 174 | else: 175 | self.playBtn['text']='|>' 176 | self.pauseMode = True 177 | def goto_first(self): 178 | self.playBtn['text']='|>' 179 | self.pauseMode = True 180 | self.currFrameId = 0 181 | self.goto() 182 | 183 | def goto_prev(self): 184 | self.playBtn['text']='|>' 185 | self.pauseMode = True 186 | self.currFrameId = max(0, self.currFrameId-1) 187 | self.goto() 188 | 189 | def goto_prev10(self): 190 | self.playBtn['text']='|>' 191 | self.pauseMode = True 192 | self.currFrameId = max(0, self.currFrameId-10) 193 | self.goto() 194 | 195 | def goto_next(self): 196 | self.playBtn['text']='|>' 197 | self.pauseMode = True 198 | self.currFrameId = min(self.totalFrameNum-1, self.currFrameId+1) 199 | self.goto() 200 | 201 | def goto_next10(self): 202 | self.playBtn['text']='|>' 203 | self.pauseMode = True 204 | self.currFrameId = min(self.totalFrameNum-1, self.currFrameId+10) 205 | self.goto() 206 | 207 | def goto_last(self): 208 | self.playBtn['text']='|>' 209 | self.pauseMode = True 210 | self.currFrameId = self.totalFrameNum-1 211 | self.goto() 212 | 213 | if __name__ == "__main__": 214 | root = Tk() 215 | videoFilename = filedialog.askopenfilename(title="Select Video", filetypes=(("video files", "*.mp4"), ("video files", "*.avi"), 216 | ("all files", "*.*"))) 217 | videoCap = cv2.VideoCapture(videoFilename) 218 | v = VideoClipperWindow(root, videoCap) 219 | root.mainloop() -------------------------------------------------------------------------------- /src/xml_generation.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import xml.etree.ElementTree as ET 3 | import numpy as np 4 | import math 5 | 6 | 7 | def normTransMat(x): 8 | num = x.shape[1] 9 | y = np.zeros((3, num)) 10 | for i in range(0, num): 11 | y[0, i] = x[0, i] / x[2, i] 12 | y[1, i] = x[1, i] / x[2, i] 13 | y[2, i] = 1 14 | return y 15 | 16 | def prettyXml(element, indent, newline, level = 0): 17 | if element: 18 | if element.text == None or element.text.isspace(): 19 | element.text = newline + indent * (level + 1) 20 | else: 21 | element.text = newline + indent * (level + 1) + element.text.strip() + newline + indent * (level + 1) 22 | 23 | temp = list(element) 24 | for subelement in temp: 25 | if temp.index(subelement) < (len(temp) - 1): 26 | subelement.tail = newline + indent * (level + 1) 27 | else: 28 | subelement.tail = newline + indent * level 29 | prettyXml(subelement, indent, newline, level = level + 1) 30 | 31 | def traj_prepare(videoFilename): 32 | anno_path = videoFilename+'_annotation.pkl' 33 | anno_raw = pickle.load(open(anno_path, 'rb')) 34 | cam_path = videoFilename+'_camcalib.pkl' 35 | raw_cam = pickle.load(open(cam_path, 'rb')) 36 | 37 | total_trajectory= [] 38 | traj_pixel = anno_raw[2] 39 | trsnsMat = raw_cam.Mat_P[:, (0, 1, 3)] 40 | trsnsMat = np.asarray(trsnsMat, dtype=np.float64) 41 | 42 | for traj in traj_pixel: 43 | pixel_arr = traj[:, 2:] 44 | traj_w=[] 45 | for wp in pixel_arr: 46 | x=wp[0] 47 | y=wp[1] 48 | wp_temp = normTransMat(np.dot(np.linalg.inv(trsnsMat), np.array([x, y, 1]).reshape((3, 1)))) 49 | wp_temp=wp_temp.reshape((1, 3)) 50 | traj_w.append([wp_temp[0][0],wp_temp[0][1]]) 51 | total_trajectory.append(traj_w) 52 | 53 | return total_trajectory 54 | 55 | 56 | def convert2XML(videoFilename): 57 | 58 | total_trajectory = traj_prepare(videoFilename) 59 | actor_num=len(total_trajectory)-1 60 | rootNode=ET.Element('scenarios') 61 | subScenNode=ET.SubElement(rootNode,'scenario', name=videoFilename+"_00", type="CustomizedBasic", town=videoFilename+"_map") 62 | ego_traj=total_trajectory[0] 63 | ego=ET.SubElement(subScenNode,'ego_vehicle', x=str(ego_traj[0][0]), y=str(ego_traj[0][1]), z="0.0", yaw="270", model="vehicle.lincoln.mkz2017", velocity="60") 64 | ego_tar= ET.SubElement(subScenNode,'target', x=str(ego_traj[-1][0]), y=str(ego_traj[-1][1]), z="0.0") 65 | ego_route= ET.SubElement(subScenNode,'route') 66 | for i in range(len(ego_traj)-1): 67 | ego_wayp= ET.SubElement(ego_route,'waypoint',x=str(ego_traj[i+1][0]), y=str(ego_traj[i+1][1]) ,z="0.0", connection="RoadOption.LANEFOLLOW", speed="68") 68 | 69 | for k in range(actor_num): 70 | actor_traj=total_trajectory[1+k] 71 | actor= ET.SubElement(subScenNode,'other_actor', x=str(actor_traj[0][0]), y=str(actor_traj[0][1]), z="0.0") 72 | actor_tar= ET.SubElement(actor,'other_target', x=str(actor_traj[-1][0]), y=str(actor_traj[-1][1]), z="0.0") 73 | actor_route= ET.SubElement(actor,'other_route') 74 | for t in range(len(actor_traj)-1): 75 | actor_wayp= ET.SubElement(actor_route,'waypoint',x=str(actor_traj[t+1][0]), y=str(actor_traj[t+1][1]),z="0.0", connection="RoadOption.LANEFOLLOW", speed="68") 76 | 77 | prettyXml(rootNode, '\t', '\n') 78 | Newtree=ET.ElementTree(rootNode) 79 | Newtree.write(videoFilename+'_TrafficDynamics.xml') 80 | --------------------------------------------------------------------------------