Code Monkey home page Code Monkey logo

Comments (3)

AngelosSE avatar AngelosSE commented on July 18, 2024 1

The error was on my end. When computing the initial arclength I used the sample at the current timestep, but when integrated the speed I used timestep+1. The current speed of the vehicle was 8.77 and with a sampling time of 0.5 seconds this gives an error 4.39 m which matches well the error I reported. After the fix I get an error of 0.11 m which is reasonable.

from nuscenes-devkit.

AngelosSE avatar AngelosSE commented on July 18, 2024

The provided arclength parameterization seems correct. When running the folowing code I get the expected result:

def plot_curve_parameter_VS_distance_travelled(lane):
    """
    If the parameterization is the arclength parameterization, then the result
    is the line y=x.
    """
    pose = arcline_path_utils.pose_at_length(lane[0],1e-6)
    position = np.array(pose[:2])
    ts = np.linspace(1e-6,166,1000)
    distances = []
    for t in ts:
        next_pose = arcline_path_utils.pose_at_length(lane[0],t)
        next_position = np.array(next_pose[:2])
        distances.append(np.linalg.norm(next_position-position))
        position = next_position
    cum_distances = np.cumsum(distances)
    _,ax = plt.subplots()
    ax.set_aspect('equal')
    ax.plot(ts,cum_distances)
    print(ts[-5:])
    print(cum_distances[-5:])

from nuscenes-devkit.

AngelosSE avatar AngelosSE commented on July 18, 2024

The method arcline_path_utils.project_pose_to_lane seems correct, I get the expected result when running:

def check_project_pose_to_lane(lane,current_pose,final_pose):
    resolution_meters=0.01
    my_current_arclength = find_arclength_of_normal_proj(current_pose[:2],lane)
    _,current_arclength = arcline_path_utils.project_pose_to_lane(current_pose
                                                            ,lane
                                                            ,resolution_meters)
    _,final_arclength = arcline_path_utils.project_pose_to_lane(final_pose
                                                        ,lane
                                                        ,resolution_meters)
    my_final_arclength = find_arclength_of_normal_proj(final_pose[:2],lane)
    print(np.abs(current_arclength-my_current_arclength))
    print(np.abs(final_arclength-my_final_arclength))

def find_arclength_of_normal_proj(position,lane):
    N = 10000
    positions = np.full((N,2),np.nan)
    ts = np.linspace(1e-6,166,N)
    for i, t in enumerate(ts):
        positions[i] = arcline_path_utils.pose_at_length(lane[0],t)[:2]
    idx = np.argmin(np.linalg.norm(positions - position,axis=1))
    return ts[idx]

from nuscenes-devkit.

Related Issues (20)

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.