Project a pointcloud into a spherical projection image.projection. Function takes no arguments because it can be also called externally if the value of the constructor was not set (in case you change your mind about wanting the projection)
(self, points)
| 118 | return data_norm |
| 119 | |
| 120 | def do_sph_projection(self, points): |
| 121 | """ Project a pointcloud into a spherical projection image.projection. |
| 122 | Function takes no arguments because it can be also called externally |
| 123 | if the value of the constructor was not set (in case you change your |
| 124 | mind about wanting the projection) |
| 125 | """ |
| 126 | |
| 127 | # laser parameters |
| 128 | fov_up = self.sph_up / 180.0 * np.pi # field of view up in rad |
| 129 | fov_down = self.sph_down / 180.0 * np.pi # field of view down in rad |
| 130 | fov = abs(fov_down) + abs(fov_up) # get field of view total in rad |
| 131 | |
| 132 | # get depth of all points |
| 133 | depth = np.linalg.norm(points, 2, axis=1) |
| 134 | |
| 135 | # get scan components |
| 136 | scan_x = points[:, 0] |
| 137 | scan_y = points[:, 1] |
| 138 | scan_z = points[:, 2] |
| 139 | |
| 140 | # get angles of all points |
| 141 | yaw = -np.arctan2(scan_y, scan_x) |
| 142 | pitch = np.arcsin(scan_z / depth) |
| 143 | |
| 144 | # get projections in image coords |
| 145 | proj_x = 0.5 * (yaw / np.pi + 1.0) # in [0.0, 1.0] |
| 146 | proj_y = 1.0 - (pitch + abs(fov_down)) / fov # in [0.0, 1.0] |
| 147 | |
| 148 | # scale to image size using angular resolution |
| 149 | # sph_H = self.sph_H*self.sph_ocu |
| 150 | # sph_W = self.sph_W*self.sph_ocu |
| 151 | sph_H = self.sph_H |
| 152 | sph_W = self.sph_W |
| 153 | proj_x *= sph_W # in [0.0, W] 128 |
| 154 | proj_y *= sph_H # in [0.0, H] 64 |
| 155 | |
| 156 | # round and clamp for use as index |
| 157 | proj_x = np.floor(proj_x) |
| 158 | proj_x = np.minimum(sph_W - 1, proj_x) |
| 159 | proj_x = np.maximum(0, proj_x).astype(np.int32) # in [0,W-1] |
| 160 | |
| 161 | proj_y = np.floor(proj_y) |
| 162 | proj_y = np.minimum(sph_H - 1, proj_y) |
| 163 | proj_y = np.maximum(0, proj_y).astype(np.int32) # in [0,H-1] |
| 164 | |
| 165 | data_grid = np.zeros((sph_H, sph_W), dtype='float32') |
| 166 | |
| 167 | # NOTE Maybe the constraints come from the far building, instead of neighbor objects |
| 168 | # NOTE For indoor datasets, constraints from near objects will reduce the accuracy |
| 169 | |
| 170 | indices = np.argsort(depth)[::-1] |
| 171 | data_grid[proj_y[indices], proj_x[indices]] = depth[indices] |
| 172 | # data_grid[proj_y, proj_x] = depth |
| 173 | |
| 174 | # sph_grid = skimage.measure.block_reduce(data_grid, (self.sph_ocu, self.sph_ocu), np.max) |
| 175 | # sph_grid = 1./(sph_grid + 1) |
| 176 | sph_norm = (data_grid - data_grid.min()) / \ |
| 177 | (data_grid.max() - data_grid.min()) |
no outgoing calls
no test coverage detected