def BuildVoxelNet():
config_path = Path('second.pytorch/second/configs/xyres_16.proto')
ckpt_path = Path('second.pytorch/second/voxelnet-331653.tckpt')
inference_ctx = TorchInferenceContext()
inference_ctx.build(config_path)
inference_ctx.restore(ckpt_path)
return inference_ctx
def BuildVoxelNet():
config_path = Path(
'/home/lucerna/MEGAsync/project/AVP/second/configs/xyres_16.proto')
ckpt_path = Path(
'/home/lucerna/MEGAsync/project/AVP/second/voxelnet-331653.tckpt')
inference_ctx = TorchInferenceContext()
inference_ctx.build(config_path)
inference_ctx.restore(ckpt_path)
return inference_ctx
class SecondModel:
def __init__(self, data_path, config_path, ckpt_path, calib_idx=0):
self.data_path = data_path
self.config_path = config_path
self.ckpt_path = ckpt_path
self.calib_idx = calib_idx
self.calib_info = None
self.inference_ctx = None
def initialize(self):
image_infos = get_kitti_image_info(
self.data_path,
training=True,
label_info=False,
calib=True,
image_ids=[self.calib_idx]
)
self.calib_info = image_infos[0]
self._build()
self._restore()
def predcit(self, pointclouds):
t = time.time()
result_annos = self._inference(pointclouds)
print("Inference time: {} ms".format(int((time.time() - t) * 1000)))
kitti_anno = self.remove_low_score(result_annos[0])
lidar_boxes = self.kitti_cam_to_lidar(kitti_anno)
return lidar_boxes
def _build(self):
print("Start build...")
self.inference_ctx = TorchInferenceContext()
self.inference_ctx.build(self.config_path)
print("Build succeeded.")
def _restore(self):
print("Start restore...")
self.inference_ctx.restore(self.ckpt_path)
print("Restore succeeded.")
def _inference(self, pointclouds):
inputs = self.inference_ctx.get_inference_input_dict(self.calib_info, pointclouds)
det_annos = self.inference_ctx.inference(inputs)
return det_annos
def kitti_cam_to_lidar(self, kitti_anno):
rect = self.calib_info['calib/R0_rect']
Tr_velo_to_cam = self.calib_info['calib/Tr_velo_to_cam']
dims = kitti_anno['dimensions']
loc = kitti_anno['location']
rots = kitti_anno['rotation_y']
boxes_camera = np.concatenate([loc, dims, rots[..., np.newaxis]], axis=1)
boxes_lidar = box_np_ops.box_camera_to_lidar(boxes_camera, rect, Tr_velo_to_cam)
return boxes_lidar
def remove_low_score(self, annos, threshold=0.5):
img_filtered_annotations = {}
relevant_annotation_indices = [i for i, s in enumerate(annos['score']) if s >= threshold]
for key in annos.keys():
img_filtered_annotations[key] = (annos[key][relevant_annotation_indices])
return img_filtered_annotations
class Processor_ROS:
def __init__(self, calib_path, config_path, ckpt_path):
self.points = None
self.json_setting = Settings(str('/home/hradt/' + ".kittiviewerrc"))
# self.config_path = self.json_setting.get("latest_vxnet_cfg_path", "")
self.calib_path = calib_path
self.config_path = config_path
self.ckpt_path = ckpt_path
self.calib_info = None
self.inputs = None
self.inference_ctx = None
def initialize(self):
self.read_calib()
self.build_vxnet()
self.load_vxnet()
def run(self, points):
num_features = 4
rect = self.calib_info['calib/R0_rect']
P2 = self.calib_info['calib/P2']
Trv2c = self.calib_info['calib/Tr_velo_to_cam']
image_shape = self.calib_info['img_shape']
self.points = points.reshape([-1, num_features])
# self.points = box_np_ops.remove_outside_points(
# self.points, rect, Trv2c, P2, image_shape)
# print(self.points)
[results] = self.inference_vxnet()
results = remove_low_score(results, 0.5)
dt_boxes_corners, scores, dt_box_lidar = kitti_anno_to_corners(
self.calib_info, results)
print("dt_box_lidar: ", dt_box_lidar)
return dt_boxes_corners, scores, dt_box_lidar
def _extend_matrix(self, mat):
mat = np.concatenate([mat, np.array([[0., 0., 0., 1.]])], axis=0)
return mat
def read_calib(self, extend_matrix=True):
# print(self.calib_path)
print("Start read_calib...")
calib_info = {'calib_path': self.calib_path}
with open(self.calib_path, 'r') as f:
lines = f.readlines()
P0 = np.array([float(info)
for info in lines[0].split(' ')[1:13]]).reshape([3, 4])
P1 = np.array([float(info)
for info in lines[1].split(' ')[1:13]]).reshape([3, 4])
P2 = np.array([float(info)
for info in lines[2].split(' ')[1:13]]).reshape([3, 4])
P3 = np.array([float(info)
for info in lines[3].split(' ')[1:13]]).reshape([3, 4])
if extend_matrix:
P0 = self._extend_matrix(P0)
P1 = self._extend_matrix(P1)
P2 = self._extend_matrix(P2)
P3 = self._extend_matrix(P3)
# calib_info['calib/P0'] = P0
# calib_info['calib/P1'] = P1
calib_info['calib/P2'] = P2
# calib_info['calib/P3'] = P3
R0_rect = np.array([float(info) for info in lines[4].split(' ')[1:10]
]).reshape([3, 3])
if extend_matrix:
rect_4x4 = np.zeros([4, 4], dtype=R0_rect.dtype)
rect_4x4[3, 3] = 1.
rect_4x4[:3, :3] = R0_rect
else:
rect_4x4 = R0_rect
calib_info['calib/R0_rect'] = rect_4x4
Tr_velo_to_cam = np.array([
float(info) for info in lines[5].split(' ')[1:13]
]).reshape([3, 4])
Tr_imu_to_velo = np.array([
float(info) for info in lines[6].split(' ')[1:13]
]).reshape([3, 4])
if extend_matrix:
Tr_velo_to_cam = self._extend_matrix(Tr_velo_to_cam)
Tr_imu_to_velo = self._extend_matrix(Tr_imu_to_velo)
calib_info['calib/Tr_velo_to_cam'] = Tr_velo_to_cam
# calib_info['calib/Tr_imu_to_velo'] = Tr_imu_to_velo
# add image shape info for lidar point cloud preprocessing
calib_info["img_shape"] = np.array(
[375, 1242]) # kitti image size: height, width
self.calib_info = calib_info
print("Read calib file succeeded.")
def build_vxnet(self):
print("Start build_vxnet...")
self.inference_ctx = TorchInferenceContext()
self.inference_ctx.build(self.config_path)
self.json_setting.set("latest_vxnet_cfg_path", self.config_path)
print("Build VoxelNet ckpt succeeded.")
def load_vxnet(self):
print("Start load_vxnet...")
self.json_setting.set("latest_vxnet_ckpt_path", self.ckpt_path)
self.inference_ctx.restore(ckpt_path)
print("Load VoxelNet ckpt succeeded.")
def inference_vxnet(self):
print("Start inference_vxnet...")
t = time.time()
self.inputs = self.inference_ctx.get_inference_input_dict_ros(
self.calib_info, self.points)
print("input preparation time:", time.time() - t)
# print("self.inputs['points'] shape: ", self.inputs["points"].shape)
# print("self.inputs['points']: ", self.inputs["points"])
t = time.time()
with self.inference_ctx.ctx():
det_annos = self.inference_ctx.inference(self.inputs)
# print(det_annos)
print("detection time:", time.time() - t)
return det_annos
'/home/dongwonshin/Desktop/second.pytorch_multiclass/pretrained_models/original_model/voxelnet-74240.tckpt'
)
os.environ["CUDA_VISIBLE_DEVICES"] = "1"
publish_topic_name = '/inference_results'
lidar_frame_step = 1
point_sampling_step = 1
if __name__ == '__main__':
# Network model load
with open(info_path, 'rb') as f:
kitti_infos = pickle.load(f)
inference_ctx = TorchInferenceContext()
inference_ctx.build(config_path)
inference_ctx.restore(ckpt_path)
# publisher init
rospy.init_node('SECOND network pub_example')
pcl_pub = rospy.Publisher(publish_topic_name, PointCloud2)
rospy.sleep(1.)
rate = rospy.Rate(3)
# for lidar_file_path in lidar_file_paths[1::lidar_frame_step]:
idx = 0
while not rospy.is_shutdown():
all_points = []
for flip in [True, False]:
points, boxes_corners, class_names = network_inference_by_path(
kitti_infos[0], lidar_file_paths[idx], point_sampling_step,
