Skip to content

Commit

Permalink
fix: fix typo and unintended spelling
Browse files Browse the repository at this point in the history
Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
YoshiRi committed Jan 25, 2024
1 parent 6c5dcbd commit 2c0d1a8
Show file tree
Hide file tree
Showing 3 changed files with 21 additions and 18 deletions.
6 changes: 3 additions & 3 deletions perception_eval/perception_eval/common/dataset.py
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
from perception_eval.common.dataset_utils import _sample_to_frame
from perception_eval.common.dataset_utils import _sample_to_frame_2d
from perception_eval.common.evaluation_task import EvaluationTask
from perception_eval.common.geometry import interpolate_hopmogeneous_matrix
from perception_eval.common.geometry import interpolate_homogeneous_matrix
from perception_eval.common.geometry import interpolate_object_list
from perception_eval.common.label import LabelConverter
from perception_eval.common.object import DynamicObject
Expand Down Expand Up @@ -362,15 +362,15 @@ def interpolate_ground_truth_frames(
after_frame: FrameGroundTruth,
unix_time: int,
):
"""Interpolate ground truth frame with lienear interpolation.
"""Interpolate ground truth frame with linear interpolation.
Args:
before_frame (FrameGroundTruth): input frame1
after_frame (FrameGroundTruth): input frame2
unix_time (int): target time
"""
# interpolate ego2map
ego2map = interpolate_hopmogeneous_matrix(
ego2map = interpolate_homogeneous_matrix(
before_frame.ego2map, after_frame.ego2map, before_frame.unix_time, after_frame.unix_time, unix_time
)

Expand Down
27 changes: 15 additions & 12 deletions perception_eval/perception_eval/common/geometry.py
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
ObjectType = Union[DynamicObject, DynamicObject2D]


def interpolate_hopmogeneous_matrix(
def interpolate_homogeneous_matrix(
matrix_1: np.ndarray, matrix_2: np.ndarray, t1: float, t2: float, t: float
) -> np.ndarray:
"""[summary]
Expand Down Expand Up @@ -94,12 +94,15 @@ def interpolate_state(state_1: ObjectState, state_2: ObjectState, t1: float, t2:
"""
assert t1 <= t <= t2
# state has position, Orientation, shape, velocity
interp_position = tuple(interpolate_list(state_1.position, state_2.position, t1, t2, t))
interp_orientation = interpolate_quaternion(state_1.orientation, state_2.orientation, t1, t2, t)
interp_shape = state_1.shape # shape will not change
interp_velocity = tuple(interpolate_list(state_1.velocity, state_2.velocity, t1, t2, t))
interpolated_position = tuple(interpolate_list(state_1.position, state_2.position, t1, t2, t))
interpolated_orientation = interpolate_quaternion(state_1.orientation, state_2.orientation, t1, t2, t)
interpolated_shape = state_1.shape # shape will not change
interpolated_velocity = tuple(interpolate_list(state_1.velocity, state_2.velocity, t1, t2, t))
return ObjectState(
position=interp_position, orientation=interp_orientation, shape=interp_shape, velocity=interp_velocity
position=interpolated_position,
orientation=interpolated_orientation,
shape=interpolated_shape,
velocity=interpolated_velocity,
)


Expand Down Expand Up @@ -155,14 +158,14 @@ def interpolate_object(object_1: ObjectType, object_2: ObjectType, t1: float, t2
raise TypeError(f"objects' type must be same, but got {type(object_1) and {type(object_2)}}")

if isinstance(object_1, DynamicObject):
return interpolate_dynamicobject(object_1, object_2, t1, t2, t)
return interpolate_dynamic_object(object_1, object_2, t1, t2, t)
elif isinstance(object_1, DynamicObject2D):
return interpolate_dynamicobject2d(object_1, object_2, t1, t2, t)
return interpolate_dynamic_object2d(object_1, object_2, t1, t2, t)
else:
raise TypeError(f"object type must be DynamicObject or DynamicObject2D, but got {type(object_1)}")


def interpolate_dynamicobject(
def interpolate_dynamic_object(
object_1: DynamicObject, object_2: DynamicObject, t1: float, t2: float, t: float
) -> DynamicObject:
"""[summary]
Expand All @@ -177,13 +180,13 @@ def interpolate_dynamicobject(
# 面倒なので基本的にcopyで済ます
# TODO: 他の要素も補間する
output_object = deepcopy(object_1)
interp_state = interpolate_state(object_1.state, object_2.state, t1, t2, t)
output_object.state = interp_state
interpolated_state = interpolate_state(object_1.state, object_2.state, t1, t2, t)
output_object.state = interpolated_state
output_object.unix_time = int(t)
return output_object


def interpolate_dynamicobject2d(
def interpolate_dynamic_object2d(
object_1: DynamicObject2D, object_2: DynamicObject2D, t1: float, t2: float, t: float
) -> DynamicObject2D:
"""[summary]
Expand Down
6 changes: 3 additions & 3 deletions perception_eval/test/common/test_geometry.py
Original file line number Diff line number Diff line change
@@ -1,18 +1,18 @@
import numpy as np
from perception_eval.common.geometry import interpolate_hopmogeneous_matrix
from perception_eval.common.geometry import interpolate_homogeneous_matrix
from perception_eval.common.geometry import interpolate_list
from perception_eval.common.geometry import interpolate_quaternion
from pyquaternion import Quaternion
import pytest


def test_interpolate_hopmogeneous_matrix():
def test_interpolate_homogeneous_matrix():
matrix_1 = np.eye(4)
matrix_2 = np.eye(4)
matrix_2[1, 3] = 1 # 小さな変更を加える
t1, t2, t = 0, 1, 0.5

result = interpolate_hopmogeneous_matrix(matrix_1, matrix_2, t1, t2, t)
result = interpolate_homogeneous_matrix(matrix_1, matrix_2, t1, t2, t)
expected = np.eye(4)
expected[1, 3] = 0.5 # 期待される結果

Expand Down

0 comments on commit 2c0d1a8

Please sign in to comment.