FilesExpand file tree

 master

/

spatial_mapping.py

Copy path

Blame

More file actions

Blame

More file actions

Latest commit

 

History

History

History

85 lines (70 loc) · 3.16 KB

 master

/

spatial_mapping.py

Copy path

Top

File metadata and controls

• Code
• Blame

85 lines (70 loc) · 3.16 KB

Raw

Copy raw file

Download raw file

Open symbols panel

Edit and raw actions

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66

67

68

69

70

71

72

73

74

75

76

77

78

79

80

81

82

83

84

85

########################################################################

#

# Copyright (c) 2017, STEREOLABS.

#

# All rights reserved.

#

# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT

# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,

# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT

# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,

# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY

# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT

# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#

########################################################################

import pyzed.sl as sl

def main():

# Create a ZEDCamera object

zed = sl.Camera()

# Create a InitParameters object and set configuration parameters

init_params = sl.InitParameters()

init_params.camera_resolution = sl.RESOLUTION.RESOLUTION_HD720 # Use HD720 video mode (default fps: 60)

# Use a right-handed Y-up coordinate system

init_params.coordinate_system = sl.COORDINATE_SYSTEM.COORDINATE_SYSTEM_RIGHT_HANDED_Y_UP

init_params.coordinate_units = sl.UNIT.UNIT_METER # Set units in meters

# Open the camera

err = zed.open(init_params)

if err != sl.ERROR_CODE.SUCCESS:

exit(1)

# Enable positional tracking with default parameters.

# Positional tracking needs to be enabled before using spatial mapping

py_transform = sl.Transform()

tracking_parameters = sl.TrackingParameters(init_pos=py_transform)

err = zed.enable_tracking(tracking_parameters)

if err != sl.ERROR_CODE.SUCCESS:

exit(1)

# Enable spatial mapping

mapping_parameters = sl.SpatialMappingParameters()

err = zed.enable_spatial_mapping(mapping_parameters)

if err != sl.ERROR_CODE.SUCCESS:

exit(1)

# Grab data during 500 frames

i = 0

py_mesh = sl.Mesh() # Create a Mesh object

runtime_parameters = sl.RuntimeParameters()

while i < 500:

# For each new grab, mesh data is updated

if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:

# In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh

mapping_state = zed.get_spatial_mapping_state()

print("\rImages captured: {0} / 500 || {1}".format(i, mapping_state))

i = i + 1

print("\n")

# Extract, filter and save the mesh in an obj file

print("Extracting Mesh...\n")

zed.extract_whole_mesh(py_mesh)

print("Filtering Mesh...\n")

py_mesh.filter(sl.MeshFilterParameters()) # Filter the mesh (remove unnecessary vertices and faces)

print("Saving Mesh...\n")

py_mesh.save("mesh.obj")

# Disable tracking and mapping and close the camera

zed.disable_spatial_mapping()

zed.disable_tracking()

zed.close()

if __name__ == "__main__":

main()