Class Node

Class Documentation

class Node

Represents a trajectory node, including its pose and downsampled point cloud.

Public Functions

Node(uint64_t timestamp, const core::Matrix4dR &pose)

Constructor for a trajectory node.

Parameters:

  • timestamp[in] Timestamp of the node.

  • pose[in] Homogeneous pose of the node.

Node(uint64_t timestamp, Eigen::Quaterniond rotation, Eigen::Vector3d position)

Constructor for a trajectory node.

Parameters:

  • timestamp[in] Timestamp of the node.

  • rotation[in] Rotational component of the pose.

  • position[in] Translational component of the pose.

const Eigen::Quaterniond &rotation() const

Retrieves the rotational component of the node pose.

Returns:

The node orientation as a quaternion.

const Eigen::Vector3d &position() const

Retrieves the translational component of the node pose.

Returns:

The node position as an XYZ vector.

void set_pose_components(const Eigen::Quaterniond &rotation, const Eigen::Vector3d &position)

Replace the node pose components and invalidate the cached 4x4 pose.

Parameters:

  • rotation[in] New rotational component of the pose.

  • position[in] New translational component of the pose.

double *mutable_rotation_coeffs_data()

Returns mutable quaternion coeff storage for solver updates.

Requesting mutable access marks the cached 4x4 pose dirty.

Returns:

Pointer to the quaternion coefficient storage.

const double *rotation_coeffs_data() const

Returns read-only quaternion coeff storage.

Returns:

Pointer to the quaternion coefficient storage.

double *mutable_position_data()

Returns mutable translation storage for solver updates.

Requesting mutable access marks the cached 4x4 pose dirty.

Returns:

Pointer to the XYZ translation storage.

const double *position_data() const

Returns read-only translation storage.

Returns:

Pointer to the XYZ translation storage.

void mark_pose_dirty()

Marks the cached 4x4 pose as stale.

Call this when quaternion/translation are externally modified and a fresh homogeneous matrix is needed on the next get_pose().

const core::Matrix4dR &get_pose() const

Retrieves the pose of the node.

Returns:

The homogeneous pose of the node as 4x4 matrix.

bool operator<(const Node &other) const

Compares nodes by their timestamps.

Parameters:

other[in] The other node to compare with.

Returns:

True if this node’s timestamp is less than the other node’s timestamp, false otherwise.

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW uint64_t ts

Timestamp of the node (nanoseconds).

core::ArrayX3dR downsampled_pts

Downsampled point cloud for pose-to-pose constraints.

core::ArrayX3dR icp_pts

ICP cloud points sampled from the lidar frame geometry.

core::ArrayX3dR icp_normals

Per-point normals for icp_pts.

Eigen::ArrayX3d ptp_constraint_pt

Selected point for point-to-point constraints (size 1 or empty).

int ptp_row = -1

Row index of the selected point for point-to-point constraints.

int ptp_col = -1

Column index of the selected point for point-to-point constraints.

int ptp_return = -1

Return index of the selected point for point-to-point constraints.

Eigen::ArrayX3d ap_constraint_pt

Selected point for absolute point constraints (size 1 or empty).

int ap_row = -1

Row index of the selected point for absolute point constraints.

int ap_col = -1

Column index of the selected point for absolute point constraints.

int ap_return = -1

Return index of the selected point for absolute point constraints.