Skip to content
Snippets Groups Projects
Commit 9686f757 authored by Jiandong Chen's avatar Jiandong Chen
Browse files

add postprocess infrared images

parent d33a3fbf
Branches
No related tags found
No related merge requests found
...@@ -2,6 +2,7 @@ import rospy ...@@ -2,6 +2,7 @@ import rospy
import rosbag import rosbag
import cv2 import cv2
import cv_bridge import cv_bridge
import numpy as np
import dataset_converter.util as util import dataset_converter.util as util
from pathlib import Path from pathlib import Path
from sensor_msgs.msg import Imu from sensor_msgs.msg import Imu
...@@ -45,6 +46,10 @@ STEREO_FOLDER = {"left_images", "right_images", "timestamp.txt"} ...@@ -45,6 +46,10 @@ STEREO_FOLDER = {"left_images", "right_images", "timestamp.txt"}
INFRARED_FOLDER = {"images", "timestamp.txt"} INFRARED_FOLDER = {"images", "timestamp.txt"}
OMNI_FOLDER = {"cam_0", "cam_1", "cam_2", "cam_3", "cam_4", "cam_5", "timestamp.txt"} OMNI_FOLDER = {"cam_0", "cam_1", "cam_2", "cam_3", "cam_4", "cam_5", "timestamp.txt"}
# Other setup
MIN_INFRARED_TEMPERTURE = 20
MAX_INFRARED_TEMPERTURE = 50
def is_datafolder_valid(folder: Path): def is_datafolder_valid(folder: Path):
print(f"Checking Folder: {folder}") print(f"Checking Folder: {folder}")
...@@ -74,7 +79,12 @@ def is_datafolder_valid(folder: Path): ...@@ -74,7 +79,12 @@ def is_datafolder_valid(folder: Path):
def process_images( def process_images(
data_path: Path, rosbag_path: Path, topic: str, frame_id: str, dst_fmt=None data_path: Path,
rosbag_path: Path,
topic: str,
frame_id: str,
dst_fmt=None,
is_infrared=False,
): ):
# read the images path and sort based on file name # read the images path and sort based on file name
images = util.get_sorted_file_list(data_path) images = util.get_sorted_file_list(data_path)
...@@ -93,9 +103,9 @@ def process_images( ...@@ -93,9 +103,9 @@ def process_images(
# store all images into bag # store all images into bag
bridge = cv_bridge.CvBridge() bridge = cv_bridge.CvBridge()
postprocess_func = np.vectorize(postprocess_infrared)
bagmode = check_bag_mode(rosbag_path) bagmode = check_bag_mode(rosbag_path)
with rosbag.Bag(rosbag_path, bagmode) as bag: with rosbag.Bag(rosbag_path, bagmode) as bag:
for img_path, time_list in zip(images, timestamps): for img_path, time_list in zip(images, timestamps):
t = rospy.Time.from_sec(float(time_list[0])) t = rospy.Time.from_sec(float(time_list[0]))
...@@ -110,7 +120,13 @@ def process_images( ...@@ -110,7 +120,13 @@ def process_images(
) )
break break
if is_infrared:
img = cv2.imread(img_path.as_posix(), -1)
img = postprocess_func(img)
img = img.astype(np.uint8)
else:
img = cv2.imread(img_path.as_posix()) img = cv2.imread(img_path.as_posix())
msg = bridge.cv2_to_compressed_imgmsg( msg = bridge.cv2_to_compressed_imgmsg(
img, dst_format=dst_fmt if dst_fmt is not None else "jpg" img, dst_format=dst_fmt if dst_fmt is not None else "jpg"
) )
...@@ -177,6 +193,7 @@ def process_navigation(data_folder: Path, rosbag_path: Path): ...@@ -177,6 +193,7 @@ def process_navigation(data_folder: Path, rosbag_path: Path):
print("Baseline Done!") print("Baseline Done!")
def process_stereo(data_folder: Path, rosbag_path: Path): def process_stereo(data_folder: Path, rosbag_path: Path):
data_path = data_folder.joinpath("stereo") data_path = data_folder.joinpath("stereo")
for k, v in STEREO_PROCESS.items(): for k, v in STEREO_PROCESS.items():
...@@ -188,7 +205,16 @@ def process_stereo(data_folder: Path, rosbag_path: Path): ...@@ -188,7 +205,16 @@ def process_stereo(data_folder: Path, rosbag_path: Path):
def process_infrared(data_folder: Path, rosbag_path: Path): def process_infrared(data_folder: Path, rosbag_path: Path):
data_path = data_folder.joinpath("infrared") data_path = data_folder.joinpath("infrared")
for k, v in INFRARED_PROCESS.items(): for k, v in INFRARED_PROCESS.items():
process_images(data_path / k, rosbag_path, v[1], v[0], "png") process_images(data_path / k, rosbag_path, v[1], v[0], "png", is_infrared=True)
# raw data
process_images(
data_path / "images",
rosbag_path,
"/infrared/raw/image/compressed",
INFRARED_PROCESS["images"][0],
"png",
)
print("Infrared Done!") print("Infrared Done!")
...@@ -221,3 +247,18 @@ def check_bag_mode(rosbag_path: Path): ...@@ -221,3 +247,18 @@ def check_bag_mode(rosbag_path: Path):
bagmode = "w" bagmode = "w"
return bagmode return bagmode
def postprocess_infrared(data):
# computer temperture
temp = data * 0.04 - 273.15
if temp <= MIN_INFRARED_TEMPERTURE:
return 0
elif temp >= MAX_INFRARED_TEMPERTURE:
return 255 # 2^8 - 1
else:
return int(
(temp - MIN_INFRARED_TEMPERTURE)
/ (MAX_INFRARED_TEMPERTURE - MIN_INFRARED_TEMPERTURE)
* 255
)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment