------------------------------------------------------------------------ */ Get IMU Params for IMU Frontend.

Guess State (pose, velocity, imu bias) from IMU only: - Guesses initial state **assuming zero velocity**. - Guesses IMU bias **assuming upright vehicl

-------------------------------------------------------------------------- */ Compute initialization errors and stats. [in]: timestamp vector for pose

Synchronize input queues.

* @brief getSyncedInputPacket Retrieves the input packet for processing. * Just pops from a threadsafe queue that contains the input packets to be

* @brief getSyncedInputPacket Retrieves the input packet for processing. * Just pops from a threadsafe queue that contains the input packets to be

------------------------------------------------------------------------ */ @brief Returns the "intrinsics flag", which is true if the pipeline has

* @brief getLeftCamPose * @return left cam pose after rectification wrt body frame of reference. */

-------------------------------------------------------------------------- */ Output goes from (-pi to pi], as we are using atan2, which looks at sign

-------------------------------------------------------------------------- */ Get valid 3D points and corresponding lmk id. Warning! it modifies old_s

TODO(TONI): REMOVE CALLS BELOW !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! Get valid 3D points and corresponding lmk id. Warning! it modifies old_smar

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */ For a triangle defined by the 3d points p1, p2, and p3 compute ratio bet

-------------------------------------------------------------------------- */ TODO this only works for current points in the current frame!!! Not for

-------------------------------------------------------------------------- */ Open files with name output_filename, and checks that it is valid.

-------------------------------------------------------------------------- */ return relative pose between last (lkf) and current keyframe (k) - MONO

-------------------------------------------------------------------------- */ return relative pose between last (lkf) and current keyframe (k) - STERE

-------------------------------------------------------------------------- */

--------------------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */ Get a screenshot of the window.

------------------------------------------------------------------------ */ Keyboard callback to get screenshot of current windodw.

* @brief getStereoCalib * @return stereo camera calibration after undistortion and rectification. */ Ideally this would return a const shared po

TODO NOT THREAD-SAFE! Should add critical sections.

------------------------------------------------------------------------ */ Get tracker info.

------------------------------------------------------------------------ */

-------------------------------------------------------------------------- */

------------------------------------------------------------------------ */ Keyboard callback to get current viewer pose.

------------------------------------------------------------------------ */

-------------------------------------------------------------------------- */ Converts a gtsam pose3 to a opencv Affine3d

Checks if the module has work to do (should check input queues are empty)

Checks if the module has work to do (should check input queues are empty)

Checks if the module has work to do (should check input queues are empty)

Checks if the module has work to do (should check input queues are empty)

Checks if the module has work to do (should check input queues are empty)

-------------------------------------------------------------------------- */ Calculates local max in 2D image. Locations is a list of point locations

-------------------------------------------------------------------------- */

Helper function

Helper function

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */ Assumes Zero Velocity & upright vehicle.

-------------------------------------------------------------------------- */ TODO (Toni): move this as much as possible inside initialization...

-------------------------------------------------------------------------- */ Try to reject bad triangles, corresponding to outliers.

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */ Is normal around axis?

------------------------------------------------------------------------ */ Is normal equal? True whenever normals are parallel, indep of their direct

-------------------------------------------------------------------------- */ Is normal perpendicular to axis?

------------------------------------------------------------------------ */ Is normal equal? True whenever normals are aligned.

------------------------------------------------------------------------ */ Is distance equal? true whenever distances are of similar absolute value.

------------------------------------------------------------------------ */ Is distance equal? true whenever distances are of similar absolute value.

-------------------------------------------------------------------------- */ Checks whether the point is closer than tolerance to the plane.

-------------------------------------------------------------------------- */ Checks whether all points in polygon are closer than tolerance to the pl

-------------------------------------------------------------------------- */ Returns whether the lmk of the smart factor is valid: - It checks that t

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */

-------------------------------------------------------------------------- */