diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
index 525d9e247e308..b45c84bacef09 100644
--- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -145,7 +145,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects(
   const auto [object_within_target_lane, object_outside_target_lane] =
     utils::path_safety_checker::separateObjectsByLanelets(
       *planner_data_->dynamic_object, data.current_lanelets,
-      utils::path_safety_checker::isPolygonOverlapLanelet);
+      [](const auto & obj, const auto & lane) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      });
 
   // Assume that the maximum allocation for data.other object is the sum of
   // objects_within_target_lane and object_outside_target_lane. The maximum allocation for
diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp
index a43592deb3c4e..dab0867a7e29b 100644
--- a/planning/behavior_path_goal_planner_module/src/util.cpp
+++ b/planning/behavior_path_goal_planner_module/src/util.cpp
@@ -145,7 +145,9 @@ PredictedObjects extractObjectsInExpandedPullOverLanes(
     route_handler, left_side, backward_distance, forward_distance, bound_offset);
 
   const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
-    objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+    objects, lanes, [](const auto & obj, const auto & lanelet) {
+      return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lanelet);
+    });
 
   return objects_in_lanes;
 }
diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
index 7a76daff4bc55..b6ab21f974cc5 100644
--- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
+++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
@@ -30,10 +30,12 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
+using data::lane_change::LanesPolygon;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using route_handler::Direction;
+using utils::path_safety_checker::ExtendedPredictedObjects;
 
 class NormalLaneChange : public LaneChangeBase
 {
@@ -115,10 +117,24 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
-  LaneChangeTargetObjects getTargetObjects(
+  ExtendedPredictedObjects getTargetObjects(
+    const LaneChangeLanesFilteredObjects & predicted_objects,
+    const lanelet::ConstLanelets & current_lanes) const;
+
+  LaneChangeLanesFilteredObjects filterObjects(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;
 
+  void filterObjectsAheadTerminal(
+    PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;
+
+  void filterObjectsByLanelets(
+    const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes,
+    std::vector<PredictedObject> & current_lane_objects,
+    std::vector<PredictedObject> & target_lane_objects,
+    std::vector<PredictedObject> & other_lane_objects) const;
+
   PathWithLaneId getPrepareSegment(
     const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
     const double prepare_length) const override;
@@ -154,8 +170,9 @@ class NormalLaneChange : public LaneChangeBase
   bool isValidPath(const PathWithLaneId & path) const override;
 
   PathSafetyStatus isLaneChangePathSafe(
-    const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-    const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
+    const LaneChangePath & lane_change_path,
+    const ExtendedPredictedObjects & collision_check_objects,
+    const utils::path_safety_checker::RSSparams & rss_params,
     CollisionCheckDebugMap & debug_data) const;
 
   LaneChangeTargetObjectIndices filterObject(
@@ -163,9 +180,6 @@ class NormalLaneChange : public LaneChangeBase
     const lanelet::ConstLanelets & target_lanes,
     const lanelet::ConstLanelets & target_backward_lanes) const;
 
-  std::vector<ExtendedPredictedObject> filterObjectsInTargetLane(
-    const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const;
-
   //! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
   //! @param obstacle_check_distance Distance to check ahead for any objects that might be
   //! obstructing ego path. It makes sense to use values like the maximum lane change distance.
diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp
index fdaa15ff9bef9..0947f0e5aff94 100644
--- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp
+++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -20,6 +20,7 @@
 #include <interpolation/linear_interpolation.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/Polygon.h>
 
 #include <utility>
 #include <vector>
@@ -195,11 +196,11 @@ struct LaneChangeTargetObjectIndices
   std::vector<size_t> other_lane{};
 };
 
-struct LaneChangeTargetObjects
+struct LaneChangeLanesFilteredObjects
 {
-  std::vector<utils::path_safety_checker::ExtendedPredictedObject> current_lane{};
-  std::vector<utils::path_safety_checker::ExtendedPredictedObject> target_lane{};
-  std::vector<utils::path_safety_checker::ExtendedPredictedObject> other_lane{};
+  utils::path_safety_checker::ExtendedPredictedObjects current_lane{};
+  utils::path_safety_checker::ExtendedPredictedObjects target_lane{};
+  utils::path_safety_checker::ExtendedPredictedObjects other_lane{};
 };
 
 enum class LaneChangeModuleType {
@@ -216,6 +217,13 @@ struct PathSafetyStatus
   bool is_safe{true};
   bool is_object_coming_from_rear{false};
 };
+
+struct LanesPolygon
+{
+  std::optional<lanelet::BasicPolygon2d> current;
+  std::optional<lanelet::BasicPolygon2d> target;
+  std::vector<lanelet::BasicPolygon2d> target_backward;
+};
 }  // namespace behavior_path_planner::data::lane_change
 
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_
diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp
index 37d436f9949a2..fbdd742a8457b 100644
--- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp
+++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp
@@ -35,7 +35,7 @@ struct Debug
   LaneChangePaths valid_paths;
   CollisionCheckDebugMap collision_check_objects;
   CollisionCheckDebugMap collision_check_objects_after_approval;
-  LaneChangeTargetObjects filtered_objects;
+  LaneChangeLanesFilteredObjects filtered_objects;
   geometry_msgs::msg::Polygon execution_area;
   geometry_msgs::msg::Pose ego_pose;
   lanelet::ConstLanelets current_lanes;
diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
index cd4f55599d964..41420430d69d5 100644
--- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
+++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
@@ -46,6 +46,7 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
+using data::lane_change::LanesPolygon;
 using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
@@ -292,6 +293,10 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol
 double calcPhaseLength(
   const double velocity, const double maximum_velocity, const double acceleration,
   const double time);
+
+LanesPolygon createLanesPolygon(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+  const lanelet::ConstLanelets & target_backward_lanes);
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug
diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
index 57ad52b602e39..67d8e1968c7e4 100644
--- a/planning/behavior_path_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -39,6 +39,8 @@ namespace behavior_path_planner
 {
 using motion_utils::calcSignedArcLength;
 using utils::lane_change::calcMinimumLaneChangeLength;
+using utils::lane_change::createLanesPolygon;
+using utils::path_safety_checker::isPolygonOverlapLanelet;
 using utils::traffic_light::getDistanceToNextTrafficLight;
 
 NormalLaneChange::NormalLaneChange(
@@ -293,7 +295,7 @@ void NormalLaneChange::insertStopPoint(
   }
 
   const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane;
-  const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
+  const auto target_objects = filterObjects(status_.current_lanes, status_.target_lanes);
   double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
 
   const auto is_valid_start_point = std::invoke([&]() -> bool {
@@ -859,226 +861,228 @@ PathWithLaneId NormalLaneChange::getPrepareSegment(
   return prepare_segment;
 }
 
-LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
+ExtendedPredictedObjects NormalLaneChange::getTargetObjects(
+  const LaneChangeLanesFilteredObjects & filtered_objects,
+  const lanelet::ConstLanelets & current_lanes) const
+{
+  ExtendedPredictedObjects target_objects = filtered_objects.target_lane;
+  const auto is_stuck = isVehicleStuck(current_lanes);
+  const auto chk_obj_in_curr_lanes = lane_change_parameters_->check_objects_on_current_lanes;
+  if (chk_obj_in_curr_lanes || is_stuck) {
+    target_objects.insert(
+      target_objects.end(), filtered_objects.current_lane.begin(),
+      filtered_objects.current_lane.end());
+  }
+
+  const auto chk_obj_in_other_lanes = lane_change_parameters_->check_objects_on_other_lanes;
+  if (chk_obj_in_other_lanes) {
+    target_objects.insert(
+      target_objects.end(), filtered_objects.other_lane.begin(), filtered_objects.other_lane.end());
+  }
+
+  return target_objects;
+}
+
+LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
 {
-  const auto current_pose = getEgoPose();
-  const auto & route_handler = *getRouteHandler();
+  const auto & current_pose = getEgoPose();
+  const auto & route_handler = getRouteHandler();
   const auto & common_parameters = planner_data_->parameters;
   auto objects = *planner_data_->dynamic_object;
   utils::path_safety_checker::filterObjectsByClass(
     objects, lane_change_parameters_->object_types_to_check);
 
-  // get backward lanes
-  const auto backward_length = lane_change_parameters_->backward_lane_length;
-  const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets(
-    route_handler, target_lanes, current_pose, backward_length);
+  if (objects.objects.empty()) {
+    return {};
+  }
+
+  const auto path =
+    route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
+
+  if (path.points.empty()) {
+    return {};
+  }
+
+  filterObjectsAheadTerminal(objects, current_lanes);
+
+  std::vector<PredictedObject> target_lane_objects;
+  std::vector<PredictedObject> current_lane_objects;
+  std::vector<PredictedObject> other_lane_objects;
+
+  filterObjectsByLanelets(
+    objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects,
+    other_lane_objects);
+
+  const auto is_within_vel_th = [](const auto & object) -> bool {
+    constexpr double min_vel_th = 1.0;
+    constexpr double max_vel_th = std::numeric_limits<double>::max();
+    return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th);
+  };
+
+  const auto is_ahead_of_ego = [&path, &current_pose](const auto & object) {
+    const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer();
 
-  lane_change_debug_.current_lanes = current_lanes;
-  lane_change_debug_.target_lanes = target_lanes;
-  lane_change_debug_.target_backward_lanes = target_backward_lanes;
+    double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
+    for (const auto & polygon_p : obj_polygon) {
+      const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
+      const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p);
+      max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
+    }
+    return max_dist_ego_to_obj >= 0.0;
+  };
+
+  const auto is_same_direction = [&](const PredictedObject & object) {
+    const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+    return !utils::path_safety_checker::isTargetObjectOncoming(current_pose, object_pose);
+  };
+
+  utils::path_safety_checker::filterObjects(
+    target_lane_objects, [&](const PredictedObject & object) {
+      return is_same_direction(object) && (is_within_vel_th(object) || is_ahead_of_ego(object));
+    });
+
+  utils::path_safety_checker::filterObjects(
+    other_lane_objects, [&](const PredictedObject & object) {
+      return is_within_vel_th(object) && is_same_direction(object) && is_ahead_of_ego(object);
+    });
 
-  // filter objects to get target object index
-  const auto target_obj_index =
-    filterObject(objects, current_lanes, target_lanes, target_backward_lanes);
+  utils::path_safety_checker::filterObjects(
+    current_lane_objects, [&](const PredictedObject & object) {
+      return is_within_vel_th(object) && is_same_direction(object) && is_ahead_of_ego(object);
+    });
 
-  LaneChangeTargetObjects target_objects;
-  target_objects.current_lane.reserve(target_obj_index.current_lane.size());
-  target_objects.target_lane.reserve(target_obj_index.target_lane.size());
-  target_objects.other_lane.reserve(target_obj_index.other_lane.size());
+  LaneChangeLanesFilteredObjects lane_change_target_objects;
 
-  // objects in current lane
   const auto is_check_prepare_phase = check_prepare_phase();
-  for (const auto & obj_idx : target_obj_index.current_lane) {
-    const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
-      is_check_prepare_phase);
-    target_objects.current_lane.push_back(extended_object);
-  }
+  std::for_each(target_lane_objects.begin(), target_lane_objects.end(), [&](const auto & object) {
+    auto extended_predicted_object = utils::lane_change::transform(
+      object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
+    lane_change_target_objects.target_lane.push_back(extended_predicted_object);
+  });
 
-  // objects in target lane
-  for (const auto & obj_idx : target_obj_index.target_lane) {
-    const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
-      is_check_prepare_phase);
-    target_objects.target_lane.push_back(extended_object);
-  }
+  std::for_each(current_lane_objects.begin(), current_lane_objects.end(), [&](const auto & object) {
+    auto extended_predicted_object = utils::lane_change::transform(
+      object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
+    lane_change_target_objects.current_lane.push_back(extended_predicted_object);
+  });
 
-  // objects in other lane
-  for (const auto & obj_idx : target_obj_index.other_lane) {
-    const auto extended_object = utils::lane_change::transform(
-      objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
-      is_check_prepare_phase);
-    target_objects.other_lane.push_back(extended_object);
-  }
+  std::for_each(other_lane_objects.begin(), other_lane_objects.end(), [&](const auto & object) {
+    auto extended_predicted_object = utils::lane_change::transform(
+      object, common_parameters, *lane_change_parameters_, is_check_prepare_phase);
+    lane_change_target_objects.other_lane.push_back(extended_predicted_object);
+  });
 
-  return target_objects;
+  lane_change_debug_.filtered_objects = lane_change_target_objects;
+
+  return lane_change_target_objects;
 }
 
-LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
-  const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
-  const lanelet::ConstLanelets & target_lanes,
-  const lanelet::ConstLanelets & target_backward_lanes) const
+void NormalLaneChange::filterObjectsAheadTerminal(
+  PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const
 {
-  const auto current_pose = getEgoPose();
+  const auto & current_pose = getEgoPose();
   const auto & route_handler = getRouteHandler();
-  const auto & common_parameters = planner_data_->parameters;
-  const auto minimum_lane_change_length = calcMinimumLaneChangeLength(
+  const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
     route_handler, current_lanes.back(), *lane_change_parameters_, direction_);
 
-  // Guard
-  if (objects.objects.empty()) {
-    return {};
-  }
+  const auto dist_ego_to_current_lanes_center =
+    lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
 
-  // Get path
-  const auto path =
-    route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
-  const auto target_path =
-    route_handler->getCenterLinePath(target_lanes, 0.0, std::numeric_limits<double>::max());
+  // ignore object that are ahead of terminal lane change start
+  utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) {
+    const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object).outer();
+    // ignore object that are ahead of terminal lane change start
+    auto distance_to_terminal_from_object = std::numeric_limits<double>::max();
+    for (const auto & polygon_p : obj_polygon) {
+      const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
+      Pose polygon_pose;
+      polygon_pose.position = obj_p;
+      polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
+      const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes);
+      distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist);
+    }
+
+    return (minimum_lane_change_length > distance_to_terminal_from_object);
+  });
+}
+
+void NormalLaneChange::filterObjectsByLanelets(
+  const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
+  const lanelet::ConstLanelets & target_lanes, std::vector<PredictedObject> & current_lane_objects,
+  std::vector<PredictedObject> & target_lane_objects,
+  std::vector<PredictedObject> & other_lane_objects) const
+{
+  const auto & current_pose = getEgoPose();
+  const auto & route_handler = getRouteHandler();
+  const auto & common_parameters = planner_data_->parameters;
+  const auto check_optional_polygon = [](const auto & object, const auto & polygon) {
+    return polygon && isPolygonOverlapLanelet(object, *polygon);
+  };
 
-  const auto current_polygon =
-    utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
+  // get backward lanes
+  const auto backward_length = lane_change_parameters_->backward_lane_length;
+  const auto target_backward_lanes = behavior_path_planner::utils::getBackwardLanelets(
+    *route_handler, target_lanes, current_pose, backward_length);
+
+  {
+    lane_change_debug_.current_lanes = current_lanes;
+    lane_change_debug_.target_lanes = target_lanes;
+    lane_change_debug_.target_backward_lanes = target_backward_lanes;
+  }
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
     target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset,
     lane_change_parameters_->lane_expansion_right_offset);
-  const auto target_polygon = utils::lane_change::createPolygon(
-    expanded_target_lanes, 0.0, std::numeric_limits<double>::max());
+
+  const auto lanes_polygon =
+    createLanesPolygon(current_lanes, expanded_target_lanes, target_backward_lanes);
   const auto dist_ego_to_current_lanes_center =
     lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);
-  std::vector<std::optional<lanelet::BasicPolygon2d>> target_backward_polygons;
-  for (const auto & target_backward_lane : target_backward_lanes) {
-    // Check to see is target_backward_lane is in current_lanes
-    // Without this check, current lane object might be treated as target lane object
-    const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) {
-      return current_lane.id() == target_backward_lane.id();
-    };
-
-    if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) {
-      continue;
-    }
 
-    lanelet::ConstLanelets lanelet{target_backward_lane};
-    auto lane_polygon =
-      utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits<double>::max());
-    target_backward_polygons.push_back(lane_polygon);
+  {
+    const auto reserve_size = objects.objects.size();
+    current_lane_objects.reserve(reserve_size);
+    target_lane_objects.reserve(reserve_size);
+    other_lane_objects.reserve(reserve_size);
   }
 
-  LaneChangeTargetObjectIndices filtered_obj_indices;
-  for (size_t i = 0; i < objects.objects.size(); ++i) {
-    const auto & object = objects.objects.at(i);
-    const auto obj_velocity_norm = std::hypot(
-      object.kinematics.initial_twist_with_covariance.twist.linear.x,
-      object.kinematics.initial_twist_with_covariance.twist.linear.y);
-    const auto extended_object = utils::lane_change::transform(
-      object, common_parameters, *lane_change_parameters_, check_prepare_phase());
-
-    const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
-
-    // calc distance from the current ego position
-    double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
-    double min_dist_ego_to_obj = std::numeric_limits<double>::max();
-    const auto obj_polygon_outer = obj_polygon.outer();
-    for (const auto & polygon_p : obj_polygon_outer) {
-      const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
-      const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p);
-      max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
-      min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj);
-    }
-
-    const auto is_lateral_far = [&]() {
+  for (const auto & object : objects.objects) {
+    const auto is_lateral_far = std::invoke([&]() -> bool {
       const auto dist_object_to_current_lanes_center =
         lanelet::utils::getLateralDistanceToClosestLanelet(
           current_lanes, object.kinematics.initial_pose_with_covariance.pose);
       const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center;
       return std::abs(lateral) > (common_parameters.vehicle_width / 2);
-    };
-
-    // ignore object that are ahead of terminal lane change start
-    {
-      double distance_to_terminal_from_object = std::numeric_limits<double>::max();
-      for (const auto & polygon_p : obj_polygon_outer) {
-        const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
-        Pose polygon_pose;
-        polygon_pose.position = obj_p;
-        polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
-        const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes);
-        if (dist < distance_to_terminal_from_object) {
-          distance_to_terminal_from_object = dist;
-        }
-      }
-      if (minimum_lane_change_length > distance_to_terminal_from_object) {
-        continue;
-      }
-    }
+    });
 
-    // ignore static object that are behind the ego vehicle
-    if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) {
+    if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) {
+      target_lane_objects.push_back(object);
       continue;
     }
 
-    // check if the object intersects with target lanes
-    if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
-      // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target
-      // lanes
-
-      if (max_dist_ego_to_obj >= 0 || is_lateral_far()) {
-        filtered_obj_indices.target_lane.push_back(i);
-        continue;
-      }
-    }
-
-    const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) {
-      return target_backward_polygon &&
-             boost::geometry::intersects(target_backward_polygon.value(), obj_polygon);
-    };
+    const auto is_overlap_target_backward = std::invoke([&]() -> bool {
+      const auto check_backward_polygon = [&object](const auto & target_backward_polygon) {
+        return isPolygonOverlapLanelet(object, target_backward_polygon);
+      };
+      return std::any_of(
+        lanes_polygon.target_backward.begin(), lanes_polygon.target_backward.end(),
+        check_backward_polygon);
+    });
 
     // check if the object intersects with target backward lanes
-    if (
-      !target_backward_polygons.empty() &&
-      std::any_of(
-        target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) {
-      filtered_obj_indices.target_lane.push_back(i);
+    if (is_overlap_target_backward) {
+      target_lane_objects.push_back(object);
       continue;
     }
 
-    // check if the object intersects with current lanes
-    if (
-      current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) &&
-      max_dist_ego_to_obj >= 0.0) {
+    if (check_optional_polygon(object, lanes_polygon.current)) {
       // check only the objects that are in front of the ego vehicle
-      filtered_obj_indices.current_lane.push_back(i);
+      current_lane_objects.push_back(object);
       continue;
     }
 
-    // ignore all objects that are behind the ego vehicle and not on the current and target
-    // lanes
-    if (max_dist_ego_to_obj < 0.0) {
-      continue;
-    }
-
-    filtered_obj_indices.other_lane.push_back(i);
+    other_lane_objects.push_back(object);
   }
-
-  return filtered_obj_indices;
-}
-
-std::vector<ExtendedPredictedObject> NormalLaneChange::filterObjectsInTargetLane(
-  const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const
-{
-  const auto target_polygon =
-    utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
-  std::vector<ExtendedPredictedObject> filtered_objects{};
-  if (target_polygon) {
-    for (auto & obj : objects.target_lane) {
-      const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape);
-      if (boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
-        filtered_objects.push_back(obj);
-      }
-    }
-  }
-
-  return filtered_objects;
 }
 
 PathWithLaneId NormalLaneChange::getTargetSegment(
@@ -1258,8 +1262,8 @@ bool NormalLaneChange::getLaneChangePaths(
     return false;
   }
 
-  const auto target_objects = getTargetObjects(current_lanes, target_lanes);
-  lane_change_debug_.filtered_objects = target_objects;
+  const auto filtered_objects = filterObjects(current_lanes, target_lanes);
+  const auto target_objects = getTargetObjects(filtered_objects, current_lanes);
 
   const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
@@ -1483,13 +1487,11 @@ bool NormalLaneChange::getLaneChangePaths(
         }
         candidate_paths->push_back(*candidate_path);
 
-        std::vector<ExtendedPredictedObject> filtered_objects =
-          filterObjectsInTargetLane(target_objects, target_lanes);
         if (
-          !is_stuck &&
-          utils::lane_change::passParkedObject(
-            route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route,
-            *lane_change_parameters_, lane_change_debug_.collision_check_objects)) {
+          !is_stuck && utils::lane_change::passParkedObject(
+                         route_handler, *candidate_path, filtered_objects.target_lane,
+                         lane_change_buffer, is_goal_in_route, *lane_change_parameters_,
+                         lane_change_debug_.collision_check_objects)) {
           debug_print(
             "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
             "lane change.");
@@ -1502,8 +1504,7 @@ bool NormalLaneChange::getLaneChangePaths(
         }
 
         const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe(
-          *candidate_path, target_objects, rss_params, is_stuck,
-          lane_change_debug_.collision_check_objects);
+          *candidate_path, target_objects, rss_params, lane_change_debug_.collision_check_objects);
 
         if (is_safe) {
           debug_print("ACCEPT!!!: it is valid and safe!");
@@ -1670,13 +1671,12 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto & current_lanes = status_.current_lanes;
   const auto & target_lanes = status_.target_lanes;
 
-  const auto target_objects = getTargetObjects(current_lanes, target_lanes);
-  lane_change_debug_.filtered_objects = target_objects;
+  const auto filtered_objects = filterObjects(current_lanes, target_lanes);
+  const auto target_objects = getTargetObjects(filtered_objects, current_lanes);
 
   CollisionCheckDebugMap debug_data;
-  const bool is_stuck = isVehicleStuck(current_lanes);
   const auto safety_status = isLaneChangePathSafe(
-    path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data);
+    path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data);
   {
     // only for debug purpose
     lane_change_debug_.collision_check_objects.clear();
@@ -1908,8 +1908,8 @@ bool NormalLaneChange::calcAbortPath()
 }
 
 PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
-  const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-  const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck,
+  const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & collision_check_objects,
+  const utils::path_safety_checker::RSSparams & rss_params,
   CollisionCheckDebugMap & debug_data) const
 {
   PathSafetyStatus path_safety_status;
@@ -1932,21 +1932,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(
   const auto debug_predicted_path =
     utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution);
 
-  auto collision_check_objects = target_objects.target_lane;
   const auto current_lanes = getCurrentLanes();
 
-  if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) {
-    collision_check_objects.insert(
-      collision_check_objects.end(), target_objects.current_lane.begin(),
-      target_objects.current_lane.end());
-  }
-
-  if (lane_change_parameters_->check_objects_on_other_lanes) {
-    collision_check_objects.insert(
-      collision_check_objects.end(), target_objects.other_lane.begin(),
-      target_objects.other_lane.end());
-  }
-
   const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets(
     lane_change_path.info.target_lanes, direction_,
     lane_change_parameters_->lane_expansion_left_offset,
diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
index 97a5ae732ec1b..214e20c2fb645 100644
--- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp
+++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp
@@ -1224,6 +1224,39 @@ double calcPhaseLength(
   const auto length_with_max_velocity = maximum_velocity * duration;
   return std::min(length_with_acceleration, length_with_max_velocity);
 }
+
+LanesPolygon createLanesPolygon(
+  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
+  const lanelet::ConstLanelets & target_backward_lanes)
+{
+  LanesPolygon lanes_polygon;
+
+  lanes_polygon.current =
+    utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits<double>::max());
+  lanes_polygon.target =
+    utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
+
+  for (const auto & target_backward_lane : target_backward_lanes) {
+    // Check to see is target_backward_lane is in current_lanes
+    // Without this check, current lane object might be treated as target lane object
+    const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) {
+      return current_lane.id() == target_backward_lane.id();
+    };
+
+    if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) {
+      continue;
+    }
+
+    lanelet::ConstLanelets lanelet{target_backward_lane};
+    auto lane_polygon =
+      utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits<double>::max());
+
+    if (lane_polygon) {
+      lanes_polygon.target_backward.push_back(*lane_polygon);
+    }
+  }
+  return lanes_polygon;
+}
 }  // namespace behavior_path_planner::utils::lane_change
 
 namespace behavior_path_planner::utils::lane_change::debug
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp
index e33c2596ab04d..a0b2d0afa0fa8 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp
@@ -69,6 +69,12 @@ bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::Cons
  */
 bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet);
 
+bool isPolygonOverlapLanelet(
+  const PredictedObject & object, const tier4_autoware_utils::Polygon2d & lanelet_polygon);
+
+bool isPolygonOverlapLanelet(
+  const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon);
+
 /**
  * @brief Filters objects based on various criteria.
  *
@@ -279,6 +285,15 @@ void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func
   selected.objects.erase(partitioned, selected.objects.end());
 }
 
+template <typename Func>
+void filterObjects(
+  std::vector<PredictedObject> & selected, std::vector<PredictedObject> & removed, Func filter)
+{
+  auto partitioned = std::partition(selected.begin(), selected.end(), filter);
+  removed.insert(removed.end(), partitioned, selected.end());
+  selected.erase(partitioned, selected.end());
+}
+
 /**
  * @brief Filters objects in the 'objects' container based on the provided filter function.
  *
@@ -296,6 +311,13 @@ void filterObjects(PredictedObjects & objects, Func filter)
   [[maybe_unused]] PredictedObjects removed_objects{};
   filterObjects(objects, removed_objects, filter);
 }
+
+template <typename Func>
+void filterObjects(std::vector<PredictedObject> & objects, Func filter)
+{
+  [[maybe_unused]] std::vector<PredictedObject> removed_objects{};
+  filterObjects(objects, removed_objects, filter);
+}
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_
diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
index 53c4706e49c10..07cb40bd530f3 100644
--- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
+++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp
@@ -43,7 +43,8 @@ using tier4_autoware_utils::Polygon2d;
 using vehicle_info_util::VehicleInfo;
 
 bool isTargetObjectOncoming(
-  const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose);
+  const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose,
+  const double angle_threshold = M_PI_2);
 
 bool isTargetObjectFront(
   const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon,
diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
index ea9872f268678..148004d89a830 100644
--- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
+++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp
@@ -66,8 +66,21 @@ bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::Cons
 
 bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet)
 {
-  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
   const auto lanelet_polygon = utils::toPolygon2d(lanelet);
+  return isPolygonOverlapLanelet(object, lanelet_polygon);
+}
+
+bool isPolygonOverlapLanelet(
+  const PredictedObject & object, const tier4_autoware_utils::Polygon2d & lanelet_polygon)
+{
+  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
+  return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
+}
+
+bool isPolygonOverlapLanelet(
+  const PredictedObject & object, const lanelet::BasicPolygon2d & lanelet_polygon)
+{
+  const auto object_polygon = tier4_autoware_utils::toPolygon2d(object);
   return !boost::geometry::disjoint(lanelet_polygon, object_polygon);
 }
 
diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
index 6390c45376b37..821289be2746d 100644
--- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
+++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp
@@ -43,9 +43,10 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point &
 }
 
 bool isTargetObjectOncoming(
-  const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose)
+  const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose,
+  const double angle_threshold)
 {
-  return std::abs(calcYawDeviation(vehicle_pose, object_pose)) > M_PI_2;
+  return std::abs(calcYawDeviation(vehicle_pose, object_pose)) > angle_threshold;
 }
 
 bool isTargetObjectFront(
diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp
index 3089134a16500..9a2749759d03e 100644
--- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp
+++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp
@@ -79,7 +79,9 @@ class PullOutPlannerBase
       *dynamic_objects, parameters_.th_moving_object_velocity);
     auto [pull_out_lane_stop_objects, others] =
       utils::path_safety_checker::separateObjectsByLanelets(
-        stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+        stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) {
+          return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+        });
     utils::path_safety_checker::filterObjectsByClass(
       pull_out_lane_stop_objects, parameters_.object_types_to_check_for_path_generation);
 
diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
index 4433020605056..7deb044eacbb7 100644
--- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
+++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -1133,7 +1133,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes(
   // filter for objects located in pull out lanes and moving at a speed below the threshold
   auto [stop_objects_in_pull_out_lanes, others] =
     utils::path_safety_checker::separateObjectsByLanelets(
-      stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
+      stop_objects, pull_out_lanes, [](const auto & obj, const auto & lane) {
+        return utils::path_safety_checker::isPolygonOverlapLanelet(obj, lane);
+      });
 
   const auto path = planner_data_->route_handler->getCenterLinePath(
     pull_out_lanes, object_check_backward_distance, object_check_forward_distance);