Pivot point calibration. More...
#include <PivotCalibration.hpp>
Inheritance diagram for TRTK::PivotCalibrationLeastSquares< T >: Collaboration diagram for TRTK::PivotCalibrationLeastSquares< T >:Public Types | |
| typedef T | value_type |
| typedef super::Vector3T | Vector3T |
| typedef super::Vector4T | Vector4T |
| typedef super::VectorXT | VectorXT |
| typedef super::Matrix3T | Matrix3T |
| typedef super::Matrix4T | Matrix4T |
| typedef super::MatrixXT | MatrixXT |
| typedef super::Point | Point |
| typedef super::DataType | DataType |
| enum | Error { NOT_ENOUGH_INPUT_DATA, UNEQUAL_CARDINALITY_OF_INPUT_SETS, UNKNOWN_ERROR } |
Public Member Functions | |
| size_t | getNumberItemsRequired () const |
| Returns the minimum number of locations and rotations needed for the algorithm. | |
| void | setLocations (Range< Vector3T > locations) |
| void | setRotations (Range< Matrix3T > rotations) |
| void | setRemoveOutliers (bool value) |
| Specifies whether outliers shall be removed during LS optimization. Defaults to false. | |
| T | compute () |
| Returns the RMSE. | |
| T | getRMSE () const |
| Returns the RMSE of the last computation. | |
| const Vector3T & | getPivotPoint () const |
| Returns the pivot point (or tool tip) in world coordinates. | |
| const Vector3T & | getLocalPivotPoint () const |
| Returns the pivot point (or tool tip) in local coordinates. | |
Pivot point calibration.
| T | scalar (floating point) type |
This class estimates the pivot point of a rigid body whose movement is constrained to rotate around a certain pivot point and whose movement is tracked via a localization system. Typical examples of application include the tool tip calibration or the hip center determination.
The calibration procedure to obtain a tool tip location is as follows: The tool tip is placed in a divot and the tool is moved around this pivot point while always touching the divot with its tip. The location as well as the rotation of the sensor system is saved for each sampling instance. Then this list is passed to one of the pivot calibration algorithms and the sought location/translation is computed.
.png)
Movement of a tool while recording data for the tool tip calibration.
Given a new measurement of the tracker (which comprises the position \( t_i \) and orientation \( R_i \) of the sensor in the global coordinate system) the pivot point the tool tip location in global coordinates can be easily computed as \( p_{global} = R_i p_{local} + t_i \).
By default no outliers are removed. This behaviour can be changed using the setRemoveOutliers() method. If the flag is set, a least square solution is computed as before, then all measurements that are outside of three standard deviations are removed, and finally the solution is recomputed.
If a tool touches the pivot point with its tip, the pivot point's location \( M \) in global coordinates is
\[ M = R_i t + t_i \]
where \( R_i \) is the rotation and \( t_i \) the location of the tool sensor in the world or tracking coordinate system. \( t \) is the sought location of the tool tip in the sensor's local coordinate system. Note, that \( t \) and \( M \) remain the same for all measurements. The above equation can be rearranged to
\[ R_i t - M = -t_i \]
or in matrix notation
\[ \begin{pmatrix} R_i & -I \end{pmatrix} \begin{pmatrix} t \\ M \\ \end{pmatrix} = -t_i \]
Combining all measurements this yields
\[ \begin{pmatrix} R_1 & -I \\ R_2 & -I \\ \vdots & \vdots \\ R_n & -I \\ \end{pmatrix} \begin{pmatrix} t \\ M \\ \end{pmatrix} = \begin{pmatrix} -t_1 \\ -t_2 \\ \vdots \\ -t_n \\ \end{pmatrix} \quad\quad \Leftrightarrow \quad\quad Ax = b \]
The solution of the least square problem \( x = \arg\min_x || Ax - b ||^2 \) is
\[ x = (A^T A)^{-1} A^T b \quad\quad \text{(Moore-Penrose pseudoinverse)} \]
If requested, two passes are run. The first run with all samples and the second run only with those that are within 3 standard deviations.
For some example code please be referred to PivotCalibration.
Definition at line 586 of file PivotCalibration.hpp.
Documentation generated by Doxygen
