Skip to content

Commit

Permalink
adds lidar_visibility method
Browse files Browse the repository at this point in the history
  • Loading branch information
@danieldugas
danieldugas committed Oct 22, 2021
1 parent 4869a82 commit bde27d0
Show file tree
Hide file tree
Showing 3 changed files with 105 additions and 3 deletions.
89 changes: 88 additions & 1 deletion CMap2D.pyx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1378,7 +1378,6 @@ cdef class CMap2D:
ranges[idx] = min_solution
return True


def visibility_map(self, observer_ij, fov=None):
return self.visibility_map_ij(observer_ij, fov=fov) * self.resolution()

Expand Down Expand Up @@ -1477,6 +1476,94 @@ cdef class CMap2D:
angle_increment = min_angle_increment
angle += angle_increment

def lidar_visibility_map_ij(self, observer_ij, angles, ranges):
""" given a lidar scan, returns areas in the map observed by the scan.
angles should be given in map frame, not in sensor frame. """
visibility_map = np.ones_like(self.occupancy(), dtype=np.float32) * -1
self.clidar_visibility_map_ij(np.array(observer_ij).astype(np.int64), angles, ranges, visibility_map)
return visibility_map

@cython.boundscheck(False)
@cython.wraparound(False)
@cython.nonecheck(False)
@cython.cdivision(True)
cdef clidar_visibility_map_ij(self, np.int64_t[::1] observer_ij,
np.float32_t[::1] angles, np.float32_t[::1] ranges,
np.float32_t[:, ::1] visibility_map):
cdef np.int64_t o_i = observer_ij[0]
cdef np.int64_t o_j = observer_ij[1]
cdef np.float32_t threshold = self._thresh_occupied
cdef np.int64_t shape0 = self.occupancy_shape0
cdef np.int64_t shape1 = self.occupancy_shape1
cdef np.float32_t i_inc_unit
cdef np.float32_t j_inc_unit
cdef np.float32_t i_abs_inc
cdef np.float32_t j_abs_inc
cdef np.float32_t raystretch
cdef np.int64_t max_inc
cdef np.int64_t max_i_inc
cdef np.int64_t max_j_inc
cdef np.float32_t i_inc
cdef np.float32_t j_inc
cdef np.float32_t n_i
cdef np.float32_t n_j
cdef np.int64_t in_i
cdef np.int64_t in_j
cdef np.float32_t occ
cdef np.int64_t di
cdef np.int64_t dj
cdef np.float32_t r
cdef np.float32_t max_r
cdef np.uint8_t is_hit
for i in range(len(angles)):
angle = angles[i]
max_r = ranges[i] / self.resolution_
i_inc_unit = ccos(angle)
j_inc_unit = csin(angle)
# Stretch the ray so that every 1 unit in the ray direction lands on a cell in i or
i_abs_inc = abs(i_inc_unit)
j_abs_inc = abs(j_inc_unit)
raystretch = 1. / i_abs_inc if i_abs_inc >= j_abs_inc else 1. / j_abs_inc
i_inc = i_inc_unit * raystretch
j_inc = j_inc_unit * raystretch
# max amount of increments before crossing the grid border
if i_inc == 0:
max_inc = <np.int64_t>((shape1 - 1 - o_j) / j_inc) if j_inc >= 0 else <np.int64_t>(o_j / -j_inc)
elif j_inc == 0:
max_inc = <np.int64_t>((shape0 - 1 - o_i) / i_inc) if i_inc >= 0 else <np.int64_t>(o_i / -i_inc)
else:
max_i_inc = <np.int64_t>((shape1 - 1 - o_j) / j_inc) if j_inc >= 0 else <np.int64_t>(o_j / -j_inc)
max_j_inc = <np.int64_t>((shape0 - 1 - o_i) / i_inc) if i_inc >= 0 else <np.int64_t>(o_i / -i_inc)
max_inc = max_i_inc if max_i_inc <= max_j_inc else max_j_inc
# Trace a ray
n_i = o_i + 0
n_j = o_j + 0
for n in range(1, max_inc-1):
n_i += i_inc
in_i = <np.int64_t>n_i
in_j = <np.int64_t>n_j
di = ( in_i - o_i )
dj = ( in_j - o_j )
r = sqrt(di*di + dj*dj)
visibility_map[in_i, in_j] = r
if r != 0:
occ = self._occupancy[in_i, in_j]
if occ >= threshold:
break
n_j += j_inc
in_i = <np.int64_t>n_i
in_j = <np.int64_t>n_j
di = ( in_i - o_i )
dj = ( in_j - o_j )
r = sqrt(di*di + dj*dj)
visibility_map[in_i, in_j] = r
if r != 0:
occ = self._occupancy[in_i, in_j]
if occ >= threshold:
break
if r > max_r:
break

cdef craymarch(self, np.int64_t[::1] observer_ij, np.int64_t[::1] angles, np.float32_t[::1] ranges):
cdef np.int64_t o_i = observer_ij[0]
cdef np.int64_t o_j = observer_ij[1]
Expand Down
2 changes: 1 addition & 1 deletion setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
long_description=long_description,
long_description_content_type='text/markdown',
url="https://github.com/danieldugas/pymap2d",
version='0.1.13',
version='0.1.14',
py_modules=['map2d', 'pose2d', 'circular_index', 'map2d_ros_tools'],
ext_modules=cythonize("CMap2D.pyx", annotate=True),
install_requires=['pyyaml', 'numpy'],
Expand Down
17 changes: 16 additions & 1 deletion test/example_visibility.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,26 @@
toc = timer()
print("Vis : {} ms".format((toc-tic)*1000))
gridshow(vis + 100*cmap2d.occupancy())

# #
plt.figure()
tic = timer()
vis = cmap2d.visibility_map([40, 30])
toc = timer()
print("Vis : {} ms".format((toc-tic)*1000))
gridshow(vis + 100*cmap2d.occupancy())

cmap2d = CMap2D.CMap2D()
cmap2d.set_resolution(1.)
angle_min = 0.0
angle_max = 6.28125524521
angle_increment = 0.00581718236208
ranges = np.array([25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 22.30762481689453, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 21.910537719726562, 25.0, 25.0, 25.0, 24.10514259338379, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 17.709718704223633, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 11.36464786529541, 11.213593482971191, 11.066872596740723, 10.924304962158203, 10.785724639892578, 10.650971412658691, 10.519895553588867, 10.539292335510254, 10.568999290466309, 10.599233627319336, 10.630002975463867, 10.661312103271484, 10.693170547485352, 10.725582122802734, 10.758556365966797, 10.792098999023438, 10.82621955871582, 10.860925674438477, 10.896224975585938, 10.932125091552734, 10.968634605407715, 19.50922966003418, 19.317476272583008, 19.13010025024414, 18.946956634521484, 18.76791763305664, 18.592851638793945, 18.4216365814209, 18.254161834716797, 18.090309143066406, 17.929973602294922, 17.77305030822754, 17.61944007873535, 17.46904754638672, 12.157987594604492, 11.10103988647461, 17.036287307739258, 12.543462753295898, 16.762279510498047, 16.6293888092041, 16.4991397857666, 16.371469497680664, 16.24630355834961, 11.059650421142578, 16.00322723388672, 15.885196685791016, 15.76942253112793, 15.655850410461426, 15.544424057006836, 15.435093879699707, 15.327803611755371, 15.222505569458008, 15.11915111541748, 15.01769733428955, 14.918095588684082, 14.820303916931152, 14.724282264709473, 14.629986763000488, 14.537381172180176, 14.446426391601562, 14.357085227966309, 14.26932144165039, 14.183100700378418, 14.098390579223633, 14.057242393493652, 8.577009201049805, 14.150525093078613, 14.234481811523438, 14.319926261901855, 14.40689468383789, 14.4954195022583, 14.585533142089844, 14.677274703979492, 14.770682334899902, 14.865789413452148, 14.962639808654785, 8.627869606018066, 8.637932777404785, 15.26406192779541, 15.368302345275879, 15.474505424499512, 15.582716941833496, 15.69298267364502, 14.391044616699219, 15.919905662536621, 14.759157180786133, 16.15570640563965, 16.277080535888672, 16.400850296020508, 16.527080535888672, 16.65583610534668, 16.787185668945312, 16.921199798583984, 17.057952880859375, 17.19752311706543, 17.33998680114746, 17.485424041748047, 17.63392448425293, 17.785579681396484, 17.940473556518555, 18.09870719909668, 18.260379791259766, 18.42559814453125, 18.594465255737305, 18.76709747314453, 18.943613052368164, 19.124134063720703, 19.308788299560547, 19.497709274291992, 19.6910343170166, 19.888912200927734, 20.09149169921875, 20.29893684387207, 20.511411666870117, 20.729089736938477, 20.952152252197266, 21.180784225463867, 21.415203094482422, 21.655607223510742, 21.9022216796875, 11.024309158325195, 10.99236011505127, 10.960966110229492, 10.930119514465332, 10.899813652038574, 10.870041847229004, 10.840797424316406, 10.812074661254883, 10.783868789672852, 10.756172180175781, 10.728979110717773, 10.70228385925293, 10.676081657409668, 10.65036678314209, 10.625134468078613, 10.60037899017334, 10.576094627380371, 10.552279472351074, 10.528923988342285, 10.506027221679688, 10.4835844039917, 10.461590766906738, 10.440041542053223, 10.41893196105957, 10.398258209228516, 10.37801742553711, 10.358203887939453, 10.338815689086914, 10.31984806060791, 10.301298141479492, 10.283162117004395, 10.265435218811035, 10.248117446899414, 10.231202125549316, 10.214688301086426, 8.233311653137207, 10.182848930358887, 10.167518615722656, 10.152578353881836, 10.138022422790527, 7.76254415512085, 6.6523237228393555, 6.655850887298584, 10.083614349365234, 10.070951461791992, 10.058661460876465, 10.046740531921387, 10.035186767578125, 10.023998260498047, 10.013174057006836, 10.002710342407227, 9.992606163024902, 9.982858657836914, 9.973468780517578, 7.011007785797119, 7.025792598724365, 9.947418212890625, 9.939435005187988, 9.931801795959473, 9.924514770507812, 9.917574882507324, 5.769030570983887, 5.796713829040527, 9.89881706237793, 9.893248558044434, 9.888020515441895, 9.883132934570312, 9.878583908081055, 9.874371528625488, 9.870498657226562, 9.866961479187012, 9.863759994506836, 9.860894203186035, 9.85836410522461, 9.856167793273926, 6.115165710449219, 9.852777481079102, 9.851584434509277, 9.850722312927246, 9.850194931030273, 9.850001335144043, 9.850139617919922, 9.85061264038086, 9.851417541503906, 9.852556228637695, 9.854029655456543, 9.8558349609375, 9.857975959777832, 9.860451698303223, 9.863261222839355, 9.866406440734863, 9.869888305664062, 7.62336540222168, 9.877861022949219, 9.882353782653809, 9.887186050415039, 9.892356872558594, 9.897869110107422, 9.903721809387207, 9.909916877746582, 9.91645622253418, 7.230035305023193, 7.229462623596191, 9.938143730163574, 9.946067810058594, 9.954341888427734, 9.962965965270996, 9.971943855285645, 9.98127555847168, 9.990962982177734, 10.001008033752441, 10.011411666870117, 10.022175788879395, 10.033304214477539, 10.04479694366455, 10.056655883789062, 10.068885803222656, 10.081485748291016, 10.094457626342773, 10.107808113098145, 10.12153434753418, 10.135642051696777, 10.150134086608887, 10.165010452270508, 10.180274963378906, 10.195932388305664, 10.211983680725098, 10.228431701660156, 10.245280265808105, 10.262531280517578, 10.280189514160156, 10.298257827758789, 10.316739082336426, 10.335637092590332, 10.354955673217773, 10.374696731567383, 10.394866943359375, 10.415468215942383, 10.436505317687988, 10.45798110961914, 10.479900360107422, 10.502269744873047, 10.525091171264648, 10.548368453979492, 10.572108268737793, 10.5963134765625, 10.620990753173828, 10.646142959594727, 10.67177677154541, 10.697897911071777, 10.724510192871094, 10.75162124633789, 10.7792329788208, 10.807355880737305, 10.835990905761719, 10.865147590637207, 10.894831657409668, 10.925047874450684, 10.955804824829102, 10.98710823059082, 11.018963813781738, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 6.99774694442749, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 6.819967746734619, 6.830949783325195, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 11.109277725219727, 11.077472686767578, 11.046220779418945, 11.01551628112793, 10.98535442352295, 10.955723762512207, 10.926623344421387, 10.89804458618164, 10.86998176574707, 10.842429161071777, 10.815380096435547, 10.788829803466797, 10.762772560119629, 10.737202644348145, 10.712116241455078, 10.687506675720215, 10.663371086120605, 10.639701843261719, 10.616496086120605, 10.59375, 10.571457862854004, 10.549615859985352, 10.528218269348145, 10.507261276245117, 10.48674201965332, 10.466656684875488, 10.447001457214355, 10.42777156829834, 10.408963203430176, 10.390572547912598, 10.372598648071289, 10.355035781860352, 10.337881088256836, 10.32113265991211, 10.304784774780273, 10.288837432861328, 10.273285865783691, 10.25812816619873, 10.24336051940918, 10.228981971740723, 10.214987754821777, 10.201375961303711, 10.188145637512207, 10.17529296875, 10.16281509399414, 10.150712013244629, 10.138978958129883, 10.127616882324219, 10.116621017456055, 10.105990409851074, 10.095723152160645, 10.08581829071045, 10.076272010803223, 10.067085266113281, 10.058255195617676, 10.049779891967773, 10.041658401489258, 10.03388786315918, 7.2822346687316895, 10.019400596618652, 10.012680053710938, 6.947633743286133, 10.000280380249023, 9.994597434997559, 9.989259719848633, 9.98426628112793, 9.979613304138184, 9.975303649902344, 9.971332550048828, 9.967702865600586, 9.9644136428833, 9.96146297454834, 7.187800884246826, 9.956575393676758, 9.954638481140137, 9.953039169311523, 9.951776504516602, 9.950851440429688, 9.950263023376465, 9.950010299682617, 9.950095176696777, 9.950516700744629, 9.951273918151855, 9.952367782592773, 9.9537992477417, 9.955568313598633, 7.173408031463623, 9.960118293762207, 9.96290111541748, 9.966022491455078, 9.969482421875, 9.973282814025879, 9.977422714233398, 9.981905937194824, 9.98672866821289, 9.991896629333496, 9.997406005859375, 10.00326156616211, 6.390661239624023, 6.389111518859863, 10.022904396057129, 10.030149459838867, 10.03774356842041, 6.41492223739624, 6.4123759269714355, 10.062641143798828, 10.071651458740234, 10.08101749420166, 10.090743064880371, 10.100830078125, 10.111278533935547, 10.122093200683594, 10.133273124694824, 10.144820213317871, 10.156739234924316, 10.16903018951416, 10.181695938110352, 10.194737434387207, 10.208158493041992, 10.221962928771973, 10.236148834228516, 10.250722885131836, 10.26568603515625, 10.28104019165039, 10.296791076660156, 10.312938690185547, 10.329486846923828, 6.224225997924805, 6.232330799102783, 10.381567001342773, 10.399749755859375, 10.41834831237793, 10.437368392944336, 10.45681095123291, 10.476682662963867, 10.496984481811523, 10.517723083496094, 10.538900375366211, 6.032186508178711, 6.028751373291016, 10.605108261108398, 10.628084182739258, 10.651521682739258, 10.675424575805664, 10.699796676635742, 10.724644660949707, 10.749972343444824, 10.775786399841309, 10.802090644836426, 10.828890800476074, 10.856191635131836, 10.884000778198242, 10.912322044372559, 10.941162109375, 10.970527648925781, 11.0004243850708, 11.03085708618164, 11.061834335327148, 11.093362808227539, 11.125450134277344, 21.98554229736328, 21.736818313598633, 21.494373321533203, 21.257999420166016, 21.027463912963867, 20.802568435668945, 20.58312225341797, 20.368938446044922, 20.15984535217285, 19.955665588378906, 19.756242752075195, 19.561420440673828, 19.37105369567871, 19.184999465942383, 19.00311851501465, 18.825288772583008, 18.65138053894043, 18.481271743774414, 18.314855575561523, 18.152015686035156, 17.992650985717773, 17.836660385131836, 17.68393898010254, 17.53440284729004, 17.387956619262695, 17.24451446533203, 17.103994369506836, 16.966312408447266, 16.831398010253906, 16.699169158935547, 16.569561004638672, 16.4424991607666, 16.317920684814453, 10.482142448425293, 16.07594871520996, 15.9584379196167, 15.843162536621094, 10.758035659790039, 15.619110107421875, 15.510221481323242, 15.403355598449707, 15.298468589782715, 15.195509910583496, 15.094435691833496, 14.995200157165527, 14.897762298583984, 14.802079200744629, 14.708111763000488, 14.61582088470459, 14.52517032623291, 14.43612289428711, 14.348641395568848, 14.262694358825684, 14.17824649810791, 14.118293762207031, 14.129842758178711, 14.213432312011719, 14.298502922058105, 14.385088920593262, 14.473220825195312, 12.559161186218262, 14.65427017211914, 14.747258186340332, 14.841938972473145, 14.938353538513184, 11.3942232131958, 15.1365385055542, 15.238396644592285, 15.342157363891602, 15.447867393493652, 15.555569648742676, 15.665321350097656, 11.28608226776123, 15.891170501708984, 16.007375717163086, 16.12584114074707, 16.246627807617188, 16.369794845581055, 16.495405197143555, 16.623525619506836, 16.754222869873047, 16.8875675201416, 17.023632049560547, 17.162492752075195, 17.30422592163086, 17.448917388916016, 17.59664535522461, 17.74750518798828, 17.90158462524414, 18.058977127075195, 18.219783782958984, 18.384109497070312, 18.55205535888672, 18.72374153137207, 18.89927864074707, 19.07878875732422, 16.02063751220703, 15.434221267700195, 19.6424617767334, 19.839191436767578, 10.944183349609375, 10.9080810546875, 10.872583389282227, 10.837681770324707, 10.80336856842041, 10.769634246826172, 10.736471176147461, 10.703873634338379, 10.671832084655762, 10.640341758728027, 10.609394073486328, 10.57898235321045, 10.549100875854492, 10.519742012023926, 10.607036590576172, 10.740551948547363, 10.87784481048584, 11.019068717956543, 11.16439151763916, 11.313985824584961, 25.0, 25.0, 25.0, 25.0, 18.25581932067871, 25.0, 25.0, 25.0, 25.0, 18.54325294494629, 22.333364486694336, 25.0, 25.0, 19.56061553955078, 25.0, 25.0, 25.0, 19.75203514099121, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0, 25.0], dtype=np.float32)
angles = np.arange(angle_min, angle_max, angle_increment, dtype=np.float32) + np.pi / 8.
assert len(angles) == len(ranges)
plt.figure()
tic = timer()
vis = cmap2d.lidar_visibility_map_ij([40, 40], angles, ranges)
toc = timer()
print("Lidar Vis : {} ms".format((toc-tic)*1000))
gridshow(vis)
plt.show()

0 comments on commit bde27d0

Please sign in to comment.