SDK CV3D Utils

struct EdgeDataAssociation

2D/3D association allowing tracking a 3D model from its edges

The association consists in a 3D point along one of the 3D model’s edges and a 2D point in the undistorted normalized image plane. The 3D model acts as a global coordinates system (i.e. the world).

using Metavision::EdgeDataAssociationVector = std::vector<EdgeDataAssociation, Eigen::aligned_allocator<EdgeDataAssociation>>

Buffer of EdgeDataAssociation that uses Eigen’s memory aligned allocator.

class EdgeLSProblem : public Metavision::LeastSquaresProblemBase<EdgeLSProblem, float, 6>

Cost function allowing estimating the camera’s pose with respect to a 3D model.

The pose is estimated by minimizing the orthogonal re-projection error between 3D points lying on the model’s edges and their associated matches in the undistorted normalized image plane.

template<typename Ti, typename To = Ti>
inline To Metavision::edgelet_direction_from_normal(const Ti &normal)

Computes the 2D edgelet’s direction from its normal.

Template Parameters

  • Ti – The input vector type for the normal

  • To – The output vector type for the direction

Parameters

normal – [in] The 2D edgelet’s normal

Returns

The edgelet’s direction

template<typename Ti, typename To = Ti>
inline To Metavision::edgelet_normal_from_direction(const Ti &direction)

Computes the 2D edgelet’s normal from its direction.

Template Parameters

  • Ti – The input vector type for the direction

  • To – The output vector type for the normal

Parameters

direction – [in] The 2D edgelet’s direction

Returns

The edgelet’s normal

bool Metavision::is_fast_edge(const MostRecentTimestampBuffer &ts, const EventCD &evt, timestamp threshold = 0, cv::Matx21f *normal = nullptr)

Tries to detect a 2D edgelet at the specified event’s location.

To detect a 2D edgelet, the algorithm samples timestamp values along a discrete circle around the event’s location, and checks that half of the circle has timestamps inferior to the other half.

The 2D edgelet is looked for in the time surface’s channel corresponding to the event’s polarity.

Note

Adapted the code released from “Mueggler, E., Bartolozzi, C., & Scaramuzza, D. (2017). Fast event-based corner

detection” to detect edges rather than corners

Parameters

  • ts – [in] Time surface in which an 2D edgelet is looked for

  • evt – [in] Event whose polarity and location will be used to detect an 2D edgelet

  • threshold – [in] Tolerance threshold meaning that a timestamp on the circle’s lowest half should be inferior to the lowest timestamp on the circle’s highest half plus this value

  • normal – [out] If a 2D edgelet is found and this parameter provided, it corresponds to the detected 2D edgelet’s normal

Returns

true if a 2D edgelet has been detected at the event’s location, False otherwise

bool Metavision::track_support_point_both_directions(const MostRecentTimestampBuffer &time_surface, const timestamp &ts_target, const cv::Matx21f &pt_img, const cv::Matx21f &vec_img_search, unsigned int search_radius, std::vector<cv::Matx21f> &match_candidates, timestamp &ts_match, int &match_idx)

Tries to match an edgelet’s support point (i.e. a point sampled along an edgelet) in a time surface.

Matching candidates are sampled along the direction of the edgelet’s normal and within a given search radius. A support point is then matched to a matching candidate when its timestamp is more recent than a target one.

Parameters

  • time_surface – [in] Time surface in which matches are looked for

  • ts_target – [in] Oldest timestamp allowed for a valid match

  • pt_img – [in] Coordinates of the support point in the time surface

  • vec_img_search – [in] Search direction (i.e. edgelet’s normal)

  • search_radius – [in] Radius in which matches are looked for

  • match_candidates – [out] Coordinates of the matching candidates that will be tested. This buffer will be filled by this function.

  • ts_match – [out] Matched candidate’s timestamp

  • match_idx – [out] If the tracking succeeds, it will contain the index of the match in the matching candidates buffer

Returns

true if the tracking has succeeded, false otherwise

bool Metavision::track_support_point_on_expected_slope(const MostRecentTimestampBuffer &time_surface, const PlaneFittingFlowEstimator &flow_estimator, const cv::Matx21f &pt_img, const cv::Matx21f &vec_img_dir, unsigned int search_radius, std::vector<cv::Matx21f> &match_candidates, timestamp &ts_match, int &match_idx)

Tries to track a support point on a slope generated by a moving edge having a known orientation.

Matching candidates are sampled along the direction of the edgelet’s normal and within a given search radius. A support point is then matched to a matching candidate when the latter lies on a slope generated by an edge having the same orientation as the expected one. Edge orientations are quantized into 4 directions (0°-180°), (45°-225°), (90°-270°) and (135°-315°).

Parameters

  • time_surface – [in] Time surface in which matches are looked for

  • flow_estimator – [in] Estimator used to determine the slope on which matching candidates are lying

  • pt_img – [in] Coordinates of the support point in the time surface

  • vec_img_dir – [in] Expected edge’s direction

  • search_radius – [in] Radius in which matches are looked for

  • match_candidates – [out] Coordinates of the matching candidates that have been tested

  • ts_match – [out] Matched candidate’s timestamp

  • match_idx – [out] If the tracking succeeds, it will contain the index of the match in the matching candidates buffer

Returns

true if the tracking has succeeded, false otherwise

template<typename Scalar, int DIM>
void Metavision::d_proj_point_d_xi(const Eigen::Matrix<Scalar, DIM, 1> &p, Eigen::Matrix<Scalar, 2, 6> &jac)

Computes the Jacobian matrix J with respect to the SE3 pose increment xi=[vx,vy,vz,wx,wy,wz] (where linear velocity v=[vx,vy,vz] and angular velocity w=[wx,wy,wz]) for the mapped-3D-point-perspective-projection operation ‘persp( exp(xi)*pt3 )’.

Template Parameters

  • Scalar – Either float or double

  • DIM – Dimensions of the input point, either 3 or 4

Parameters

  • p – [in] 3D point to project

  • jac – [out] Operation’s Jacobian

template<typename Scalar, int DIMP, int DIMV>
void Metavision::d_proj_vector_d_xi(const Eigen::Matrix<Scalar, DIMP, 1> &p, const Eigen::Matrix<Scalar, DIMV, 1> &v, Eigen::Matrix<Scalar, 2, 6> &jac)

Computes the Jacobian matrix J with respect to the SE3 pose increment xi=[vx,vy,vz,wx,wy,wz] (where linear velocity v=[vx,vy,vz] and angular velocity w=[wx,wy,wz]) for the mapped-3D-local-vector-perspective-projection operation ‘persp( exp(xi)*v3 @ exp(xi)*pt3 )’.

Note

This Jacobian does not include an eventual normalization of the vector after projection

Template Parameters

  • Scalar – Either float or double

  • DIMP – Dimension of the input point, either 3 or 4

  • DIMV – Dimension of the input vector, either 3 or 4

Parameters

  • p – [in] 3D point from which the vector is projected

  • v – [in] Vector to project

  • jac – [out] Operation’s Jacobian

struct Model3d

Structure defining a 3D model.

bool Metavision::load_model_3d_from_json(const std::string &path, Model3d &model)

Loads a 3D model from a JSON file.

Parameters

  • path – [in] Path to the JSON file containing the 3D model

  • model – [out] Structure containing the loaded 3D model if the function succeeds

Returns

True if the function succeeds, false otherwise

void Metavision::select_visible_edges(const Eigen::Matrix4f &T_c_w, const Model3d &model, std::set<size_t> &visible_edges)

Selects the visible edges of a 3D model given a camera’s pose.

This function assumes that the 3D model is a convex polyhedron whose faces are either completely hidden or visible like explained in “Fast algorithm for 3D-graphics, Georg Glaeser, section 5.2”.

Parameters

  • T_c_w – [in] Camera’s pose from which visible edges need to be determined

  • model – [in] The 3D model whose visible edges need to be determined

  • visible_edges – [out] 3D model’s edges visible from the given camera’s pose, others are concealed

void Metavision::sample_support_points(const CameraGeometry32f &cam_geometry, const Eigen::Matrix4f &T_c_w, const Model3d &model, const std::set<size_t> &visible_edges, std::uint32_t step_px, EdgeDataAssociationVector &edge_data_associations)

Samples 3D support points from the visible edges of a 3D model.

Parameters

  • cam_geometry – [in] Camera geometry instance allowing mapping points from world (i.e. the 3D model’s reference frame) to image coordinates

  • T_c_w – [in] Camera’s pose with respect to the 3D model

  • model – [in] 3D model from which support points are to be sampled

  • visible_edges – [in] 3D model’s visible edges

  • step_px – [in] Step in pixels between two support points in the distorted image

  • edge_data_associations – [out] Edge data associations whose pt_w attribute will be filled with the sampled support points. Edge data assoctiation in the vector are sorted by their edge_idx

void Metavision::draw_edges(const CameraGeometry32f &cam_geometry, const Eigen::Matrix4f &T_c_w, const Model3d &model, const std::set<size_t> &edges, cv::Mat &output, const cv::Scalar &color = cv::Scalar(255, 0, 0))

Draws the selected edges of a 3D model into the output frame.

Parameters

  • cam_geometry – Camera geometry instance allowing mapping points from world (i.e. the 3D model’s reference frame) to image coordinates

  • T_c_w – Camera’s pose from which edges need to be drawn

  • model – 3D model whose edges need to be drawn

  • edges – Indexes to the 3D model’s edges that need to be drawn

  • output – Output image

  • color – Color used to render the 3D model’s edges

class VelocityCostFunction : public ceres::SizedCostFunction<2, 6>

Cost function for computing the projection error of a 3D point reprojected in a camera using camera velocity and timestamp information.

This cost function computes the projection error of a 3D point reprojected in a camera based on the camera velocity and the timestamp at which the 3D point was observed. The error is computed as the difference between the observed 2D point and the reprojected 2D point in the undistorted normalized coordinates.

Public Functions

explicit VelocityCostFunction(const Observation &obs)

Constructor.

Parameters

obs – Observation data to be used for computing the cost

virtual ~VelocityCostFunction() = default

Destructor.

bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const final

Computes the residual error and optionally the Jacobian of the cost function.

Parameters

  • parameters – [in] The array should contain a single buffer of length 6, representing the camera’s linear and angular velocities in the order [vx, vy, vz, wx, wy, wz]

  • residuals – [out] Array to store the residual error, containing the 2D projection error

  • jacobians – [out] The Jacobian matrix should be a 2x6 matrix (in row major mode) representing the derivatives of the residual error with respect to the camera velocity parameters

Returns

True if evaluation is successful, false otherwise

struct Observation

Structure to represent observation data.

This structure holds the necessary information about an observation, including the undistorted normalized 2D point, the 3D point to project, and the time elapsed between the previous pose and the landmark observation.

Public Members

Eigen::Vector2d undist_norm_p

Undistorted normalized observation.

Eigen::Vector3d p_c

3d point in the camera’s coordinates system

double dt

Time elapsed since the last pose estimation and the current observation.