Name
create_poseT_create_poseCreatePosecreate_poseCreatePoseCreatePose — Create a 3D pose.
void CreatePose(const HTuple& TransX, const HTuple& TransY, const HTuple& TransZ, const HTuple& RotX, const HTuple& RotY, const HTuple& RotZ, const HTuple& OrderOfTransform, const HTuple& OrderOfRotation, const HTuple& ViewOfTransform, HTuple* Pose)
void HPose::HPose(double TransX, double TransY, double TransZ, double RotX, double RotY, double RotZ, const HString& OrderOfTransform, const HString& OrderOfRotation, const HString& ViewOfTransform)
void HPose::HPose(double TransX, double TransY, double TransZ, double RotX, double RotY, double RotZ, const char* OrderOfTransform, const char* OrderOfRotation, const char* ViewOfTransform)
void HPose::CreatePose(double TransX, double TransY, double TransZ, double RotX, double RotY, double RotZ, const HString& OrderOfTransform, const HString& OrderOfRotation, const HString& ViewOfTransform)
void HPose::CreatePose(double TransX, double TransY, double TransZ, double RotX, double RotY, double RotZ, const char* OrderOfTransform, const char* OrderOfRotation, const char* ViewOfTransform)
void HOperatorSetX.CreatePose(
[in] VARIANT TransX, [in] VARIANT TransY, [in] VARIANT TransZ, [in] VARIANT RotX, [in] VARIANT RotY, [in] VARIANT RotZ, [in] VARIANT OrderOfTransform, [in] VARIANT OrderOfRotation, [in] VARIANT ViewOfTransform, [out] VARIANT* Pose)
VARIANT HPoseX.CreatePose(
[in] double TransX, [in] double TransY, [in] double TransZ, [in] double RotX, [in] double RotY, [in] double RotZ, [in] BSTR OrderOfTransform, [in] BSTR OrderOfRotation, [in] BSTR ViewOfTransform)
static void HOperatorSet.CreatePose(HTuple transX, HTuple transY, HTuple transZ, HTuple rotX, HTuple rotY, HTuple rotZ, HTuple orderOfTransform, HTuple orderOfRotation, HTuple viewOfTransform, out HTuple pose)
public HPose(double transX, double transY, double transZ, double rotX, double rotY, double rotZ, string orderOfTransform, string orderOfRotation, string viewOfTransform)
void HPose.CreatePose(double transX, double transY, double transZ, double rotX, double rotY, double rotZ, string orderOfTransform, string orderOfRotation, string viewOfTransform)
create_posecreate_poseCreatePosecreate_poseCreatePoseCreatePose creates the 3D pose PosePosePosePosePosepose. A pose describes a
rigid 3D transformation, i.e., a transformation consisting of an arbitrary
translation and rotation, with 6 parameters: TransXTransXTransXTransXTransXtransX,
TransYTransYTransYTransYTransYtransY, and TransZTransZTransZTransZTransZtransZ specify the translation along the x-,
y-, and z-axis, respectively, while RotXRotXRotXRotXRotXrotX, RotYRotYRotYRotYRotYrotY, and
RotZRotZRotZRotZRotZrotZ describe the rotation.
3D poses are typically used in two ways: First, to describe the position and
orientation of one coordinate system relative to another (e.g., the pose of a
part's coordinate system relative to the camera coordinate system - in short:
the pose of the part relative to the camera) and secondly, to describe how
coordinates can be transformed between two coordinate systems (e.g., to
transform points from part coordinates into camera coordinates).
Representation of orientation (rotation)
A 3D rotation around an arbitrary axis can be represented by 3 parameters in
multiple ways. HALCON lets you choose between three of them with the
parameter OrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationorderOfRotation: If you pass the value 'gba'"gba""gba""gba""gba""gba", the
rotation is described by the following chain of rotations around the three
axes (see hom_mat3d_rotatehom_mat3d_rotateHomMat3dRotatehom_mat3d_rotateHomMat3dRotateHomMat3dRotate for the content for the rotation matrices
Rx, Ry, and Rz):
R('gba') = Rx(RotXRotXRotXRotXRotXrotX) * Ry(RotYRotYRotYRotYRotYrotY) * Rz(RotZRotZRotZRotZRotZrotZ)
R('gba') is referred to
as the Yaw-Pitch-Roll convention in the literature.
Please note that you can “read” this chain in two ways: If you start from
the right, the rotations are always performed relative to the global (i.e.,
fixed or “old”) coordinate system. Thus,
R('gba') can be read as follows:
First rotate around the z-axis, then around the “old” y-axis, and finally
around the “old” x-axis. In contrast, if you read from the left to the
right, the rotations are performed relative to the local (i.e., “new”)
coordinate system. Then, R('gba')
corresponds to the following: First rotate around the x-axis, the around the
“new” y-axis, and finally around the “new(est)” z-axis.
Reading R('gba') from right to
left corresponds to the following sequence of operator calls:
hom_mat3d_identity (HomMat3DIdent)
hom_mat3d_rotate (HomMat3DIdent, RotZ, 'z', 0, 0, 0, HomMat3DRotZ)
hom_mat3d_rotate (HomMat3DRotZ, RotY, 'y', 0, 0, 0, HomMat3DRotYZ)
hom_mat3d_rotate (HomMat3DRotYZ, RotX, 'x', 0, 0, 0, HomMat3DXYZ)
In contrast, reading from left to right corresponds to the following operator
sequence:
hom_mat3d_identity (HomMat3DIdent)
hom_mat3d_rotate_local (HomMat3DIdent, RotX, 'x', HomMat3DRotX)
hom_mat3d_rotate_local (HomMat3DRotX, RotY, 'y', HomMat3DRotXY)
hom_mat3d_rotate_local (HomMat3DRotXY, RotZ, 'z', HomMat3DXYZ)
When passing 'abg'"abg""abg""abg""abg""abg" in OrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationorderOfRotation, the rotation
corresponds to the following chain:
R('abg') = Rz(RotZRotZRotZRotZRotZrotZ) * Ry(RotYRotYRotYRotYRotYrotY) * Rx(RotXRotXRotXRotXRotXrotX)
R('abg') is referred to
as the Roll-Pitch-Yaw convention in the literature.
If you pass 'rodriguez'"rodriguez""rodriguez""rodriguez""rodriguez""rodriguez" in OrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationorderOfRotation, the rotation
parameters RotXRotXRotXRotXRotXrotX, RotYRotYRotYRotYRotYrotY, and RotZRotZRotZRotZRotZrotZ are interpreted as
the x-, y-, and z-component of the so-called Rodriguez rotation vector. The
direction of the vector defines the (arbitrary) axis of rotation. The length
of the vector usually defines the rotation angle with positive orientation.
Here, a variation of the Rodriguez vector is used, where the length of the
vector defines the tangent of half the rotation angle:
/ RotX \
R('rodriguez') = rotate around | RotY | by 2 * atan(sqrt(RotX^2 + RotY^2 + RotZ^2))
\ RotZ /
Please note that these 3D poses can be ambiguous, meaning a homogeneous
transformation matrix can have several pose representations.
For example, for
R('gba') with
the following poses correspond to the same homogeneous transformation
matrix:
create_pose(0, 0, 0, 30 , 90, 54, 'Rp+T', 'gba', 'point', Pose1)
create_pose(0, 0, 0, 17, 90, 67, 'Rp+T', 'gba', 'point', Pose2)
If this leads to problems, you can instead use homogeneous transformation
matrices or quaternions (axis_angle_to_quataxis_angle_to_quatAxisAngleToQuataxis_angle_to_quatAxisAngleToQuatAxisAngleToQuat) to represent rotations.
Corresponding homogeneous transformation matrix
You can obtain the homogeneous transformation matrix corresponding to a pose
with the operator pose_to_hom_mat3dpose_to_hom_mat3dPoseToHomMat3dpose_to_hom_mat3dPoseToHomMat3dPoseToHomMat3d. In the standard definition, this
is the following homogeneous transformation matrix which can be split into
two separate matrices, one for the translation
(H(t)) and one for the rotation
(H(R)):
/ TransX \
H = | R t | = | R(RotX,RotY,RotZ) TransY | =
| 0 0 0 1 | | TransZ |
\ 0 0 0 1 /
/ 1 0 0 TransX \ / 0 \
= | 0 1 0 TransY | * | R(RotX,RotY,RotZ) 0 | = H(t) * H(R)
| 0 0 1 TransZ | | 0 |
\ 0 0 0 1 / \ 0 0 0 1 /
Transformation of coordinates
The following equation describes how a point can be transformed from
coordinate system 1 into coordinate system 2 with a pose, or more
exactly, with the corresponding homogeneous transformation matrix
H(1->2) (input and output points as
homogeneous vectors, see also affine_trans_point_3daffine_trans_point_3dAffineTransPoint3daffine_trans_point_3dAffineTransPoint3dAffineTransPoint3d). Note that to
transform points from coordinate system 1 into system 2, you use the
transformation matrix that describes the pose of coordinate system 1
relative to system 2.
/ p(2) \ / p(1) \ / / TransX \ \
| | = H(1->2) * | | = | R(RotX,RotY,RotZ) * p(1) + | TransY | |
\ 1 / \ 1 / | \ TransZ / |
\ 1 /
This corresponds to the following operator calls:
pose_to_hom_mat3d(PoseOf1In2, HomMat3DFrom1In2)
affine_trans_point_3d(HomMat3DFrom1In2, P1X, P1Y, P1Z, P2X, P2Y, P2Z)
Non-standard pose definitions
So far, we described the standard pose definition. To create such poses, you
select the (default) values 'Rp+T'"Rp+T""Rp+T""Rp+T""Rp+T""Rp+T" for the parameter
OrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformorderOfTransform and 'point'"point""point""point""point""point" for ViewOfTransformViewOfTransformViewOfTransformViewOfTransformViewOfTransformviewOfTransform.
By specifying other values for these parameters, you can create non-standard
poses types which we describe briefly below. Please note that these
representation types are only supported for backwards compatibility; we
strongly recommend to use the standard types.
If you select 'R(p-T)'"R(p-T)""R(p-T)""R(p-T)""R(p-T)""R(p-T)" for OrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformorderOfTransform, the created
pose corresponds to the following chain of transformations, i.e., the
sequence of rotation and translation is reversed and the translation is
negated:
/ 0 \ / 1 0 0 - TransX \
H = | R(RotX,RotY,RotZ) 0 | * | 0 1 0 - TransY | = H(R) * H(-t)
| 0 | | 0 0 1 - TransZ |
\ 0 0 0 1 / \ 0 0 0 1 /
If you select 'coordinate_system'"coordinate_system""coordinate_system""coordinate_system""coordinate_system""coordinate_system" for ViewOfTransformViewOfTransformViewOfTransformViewOfTransformViewOfTransformviewOfTransform, the
sequence of transformations remains constant, but the rotation angles are
negated. Please note that, contrary to its name, this is not equivalent to
transforming a coordinate system!
/ 1 0 0 TransX \ / 0 \
H = | 0 1 0 TransY | * | R(-RotX,-RotY,-RotZ) 0 |
| 0 0 1 TransZ | | 0 |
\ 0 0 0 1 / \ 0 0 0 1 /
Returned data structure
The created 3D pose is returned in PosePosePosePosePosepose which is a tuple of length
seven. The first three elements hold the translation parameters
TransXTransXTransXTransXTransXtransX, TransYTransYTransYTransYTransYtransY, and TransZTransZTransZTransZTransZtransZ, followed by the
rotation parameters RotXRotXRotXRotXRotXrotX, RotYRotYRotYRotYRotYrotY, and RotZRotZRotZRotZRotZrotZ. The last
element codes the representation type of the pose that you selected with
the parameters OrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformorderOfTransform, OrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationOrderOfRotationorderOfRotation, and
ViewOfTransformViewOfTransformViewOfTransformViewOfTransformViewOfTransformviewOfTransform. The following table lists the possible
combinations. As already noted, we recommend to use only the representation
types with OrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformOrderOfTransformorderOfTransform = 'Rp+T'"Rp+T""Rp+T""Rp+T""Rp+T""Rp+T" and
ViewOfTransformViewOfTransformViewOfTransformViewOfTransformViewOfTransformviewOfTransform = 'point'"point""point""point""point""point" (codes 0, 2, and 4).
|OrderOfTransform | OrderOfRotation | ViewOfTransform |Code |
----------------------------------------------------------------
| 'Rp+T' | 'gba' | 'point' | 0 |
| 'Rp+T' | 'abg' | 'point' | 2 |
| 'Rp+T' | 'rodriguez' | 'point' | 4 |
| 'Rp+T' | 'gba' | 'coordinate_system'| 1 |
| 'Rp+T' | 'abg' | 'coordinate_system'| 3 |
| 'Rp+T' | 'rodriguez' | 'coordinate_system'| 5 |
| 'R(p-T)' | 'gba' | 'point' | 8 |
| 'R(p-T)' | 'abg' | 'point' | 10 |
| 'R(p-T)' | 'rodriguez' | 'point' | 12 |
| 'R(p-T)' | 'gba' | 'coordinate_system'| 9 |
| 'R(p-T)' | 'abg' | 'coordinate_system'| 11 |
| 'R(p-T)' | 'rodriguez' | 'coordinate_system'| 13 |
You can convert poses into other representation types using
convert_pose_typeconvert_pose_typeConvertPoseTypeconvert_pose_typeConvertPoseTypeConvertPoseType and query the type using get_pose_typeget_pose_typeGetPoseTypeget_pose_typeGetPoseTypeGetPoseType.
- Multithreading type: reentrant (runs in parallel with non-exclusive operators).
- Multithreading scope: global (may be called from any thread).
- Processed without parallelization.
Translation along the x-axis (in [m]).
Default value: 0.1
Suggested values: -1.0, -0.75, -0.5, -0.25, -0.2, -0.1, -0.5, -0.25, -0.125, -0.01, 0.0, 0.01, 0.125, 0.25, 0.5, 0.1, 0.2, 0.25, 0.5, 0.75, 1.0
Translation along the y-axis (in [m]).
Default value: 0.1
Suggested values: -1.0, -0.75, -0.5, -0.25, -0.2, -0.1, -0.5, -0.25, -0.125, -0.01, 0.0, 0.01, 0.125, 0.25, 0.5, 0.1, 0.2, 0.25, 0.5, 0.75, 1.0
Translation along the z-axis (in [m]).
Default value: 0.1
Suggested values: -1.0, -0.75, -0.5, -0.25, -0.2, -0.1, -0.5, -0.25, -0.125, -0.01, 0.0, 0.01, 0.125, 0.25, 0.5, 0.1, 0.2, 0.25, 0.5, 0.75, 1.0
Rotation around x-axis or x component of the Rodriguez
vector (in [°] or without unit).
Default value: 90.0
Suggested values: 0.0, 90.0, 180.0, 270.0
Typical range of values: 0
≤
RotX
RotX
RotX
RotX
RotX
rotX
≤
360
Rotation around y-axis or y component of the Rodriguez
vector (in [°] or without unit).
Default value: 90.0
Suggested values: 0.0, 90.0, 180.0, 270.0
Typical range of values: 0
≤
RotY
RotY
RotY
RotY
RotY
rotY
≤
360
Rotation around z-axis or z component of the Rodriguez
vector (in [°] or without unit).
Default value: 90.0
Suggested values: 0.0, 90.0, 180.0, 270.0
Typical range of values: 0
≤
RotZ
RotZ
RotZ
RotZ
RotZ
rotZ
≤
360
Order of rotation and translation.
Default value:
'Rp+T'
"Rp+T"
"Rp+T"
"Rp+T"
"Rp+T"
"Rp+T"
Suggested values: 'Rp+T'"Rp+T""Rp+T""Rp+T""Rp+T""Rp+T", 'R(p-T)'"R(p-T)""R(p-T)""R(p-T)""R(p-T)""R(p-T)"
Meaning of the rotation values.
Default value:
'gba'
"gba"
"gba"
"gba"
"gba"
"gba"
Suggested values: 'gba'"gba""gba""gba""gba""gba", 'abg'"abg""abg""abg""abg""abg", 'rodriguez'"rodriguez""rodriguez""rodriguez""rodriguez""rodriguez"
View of transformation.
Default value:
'point'
"point"
"point"
"point"
"point"
"point"
Suggested values: 'point'"point""point""point""point""point", 'coordinate_system'"coordinate_system""coordinate_system""coordinate_system""coordinate_system""coordinate_system"
3D pose.
Number of elements: 7
* goal: calibration with non-standard calibration object
* read start values for internal camera parameters
read_cam_par('campar.dat', CameraParam)
* (read 3D world points [WorldPointsX,WorldPointsY,WorldPointsZ],
* extract corresponding 2D image points [PixelsRow,PixelsColumn])
* task: create starting value for the external camera parameters, i.e., the
* pose of the calibration object in the calibration images
* first image: calibration object placed at a distance of 0.5 and 0.1
* 'below' the camera coordinate system
* orientation 'read from left to right': rotated 30 degrees
* around the optical axis of the camera (z-axis),
* then tilted 10 degrees around the new y-axis
create_pose(0.1, 0.0, 0.5, 30, 10, 0, 'Rp+T', 'abg', 'point', StartPose1)
* (accumulate all poses in StartPoses = [StartPose1, StartPose2, ...])
* perform the calibration
camera_calibration(WorldPointsX, WorldPointsY, WorldPointsZ, \
PixelsRow, PixelsColumn, CameraParam, StartPoses, \
'pose', FinalCamParam, FinalPoses, Errors)
* goal: calibration with non-standard calibration object
* read start values for internal camera parameters
read_cam_par('campar.dat', CameraParam)
* (read 3D world points [WorldPointsX,WorldPointsY,WorldPointsZ],
* extract corresponding 2D image points [PixelsRow,PixelsColumn])
* task: create starting value for the external camera parameters, i.e., the
* pose of the calibration object in the calibration images
* first image: calibration object placed at a distance of 0.5 and 0.1
* 'below' the camera coordinate system
* orientation 'read from left to right': rotated 30 degrees
* around the optical axis of the camera (z-axis),
* then tilted 10 degrees around the new y-axis
create_pose(0.1, 0.0, 0.5, 30, 10, 0, 'Rp+T', 'abg', 'point', StartPose1)
* (accumulate all poses in StartPoses = [StartPose1, StartPose2, ...])
* perform the calibration
camera_calibration(WorldPointsX, WorldPointsY, WorldPointsZ, \
PixelsRow, PixelsColumn, CameraParam, StartPoses, \
'pose', FinalCamParam, FinalPoses, Errors)
* goal: calibration with non-standard calibration object
* read start values for internal camera parameters
read_cam_par('campar.dat', CameraParam)
* (read 3D world points [WorldPointsX,WorldPointsY,WorldPointsZ],
* extract corresponding 2D image points [PixelsRow,PixelsColumn])
* task: create starting value for the external camera parameters, i.e., the
* pose of the calibration object in the calibration images
* first image: calibration object placed at a distance of 0.5 and 0.1
* 'below' the camera coordinate system
* orientation 'read from left to right': rotated 30 degrees
* around the optical axis of the camera (z-axis),
* then tilted 10 degrees around the new y-axis
create_pose(0.1, 0.0, 0.5, 30, 10, 0, 'Rp+T', 'abg', 'point', StartPose1)
* (accumulate all poses in StartPoses = [StartPose1, StartPose2, ...])
* perform the calibration
camera_calibration(WorldPointsX, WorldPointsY, WorldPointsZ, \
PixelsRow, PixelsColumn, CameraParam, StartPoses, \
'pose', FinalCamParam, FinalPoses, Errors)
// goal: calibration with non-standard calibration object
HTuple CameraParam, StartPose1, StartPoses, FinalPoses;
HTuple Errors, FinalCamParam;
HTuple WorldPointsX, WorldPointsY, WorldPointsZ, PixelsRow, PixelsColumn;
// read start values for internal camera parameters
read_cam_par("campar.dat", &CameraParam);
// (read 3D world points [WorldPointsX,WorldPointsY,WorldPointsZ],
// extract corresponding 2D image points [PixelsRow,PixelsColumn])
// task: create starting value for the external camera parameters, i.e., the
// pose of the calibration object in the calibration images
// first image: calibration object placed at a distance of 0.5 and 0.1
// 'below' the camera coordinate system
// orientation 'read from left to right': rotated 30 degrees
// around the optical axis of the camera (z-axis),
// then tilted 10 degrees around the new y-axis
create_pose(0.1, 0.0, 0.5, 30, 10, 0, 'Rp+T', 'abg', 'point', &StartPose1);
// (accumulate all poses in StartPoses = [StartPose1, StartPose2, ...])
// perform the calibration
camera_calibration(WorldPointsX, WorldPointsY, WorldPointsZ,
PixelsRow, PixelsColumn, CameraParam, StartPoses, "pose",
&FinalCamParam, &FinalPoses, &Errors);
* goal: calibration with non-standard calibration object
* read start values for internal camera parameters
read_cam_par('campar.dat', CameraParam)
* (read 3D world points [WorldPointsX,WorldPointsY,WorldPointsZ],
* extract corresponding 2D image points [PixelsRow,PixelsColumn])
* task: create starting value for the external camera parameters, i.e., the
* pose of the calibration object in the calibration images
* first image: calibration object placed at a distance of 0.5 and 0.1
* 'below' the camera coordinate system
* orientation 'read from left to right': rotated 30 degrees
* around the optical axis of the camera (z-axis),
* then tilted 10 degrees around the new y-axis
create_pose(0.1, 0.0, 0.5, 30, 10, 0, 'Rp+T', 'abg', 'point', StartPose1)
* (accumulate all poses in StartPoses = [StartPose1, StartPose2, ...])
* perform the calibration
camera_calibration(WorldPointsX, WorldPointsY, WorldPointsZ, \
PixelsRow, PixelsColumn, CameraParam, StartPoses, \
'pose', FinalCamParam, FinalPoses, Errors)
* goal: calibration with non-standard calibration object
* read start values for internal camera parameters
read_cam_par('campar.dat', CameraParam)
* (read 3D world points [WorldPointsX,WorldPointsY,WorldPointsZ],
* extract corresponding 2D image points [PixelsRow,PixelsColumn])
* task: create starting value for the external camera parameters, i.e., the
* pose of the calibration object in the calibration images
* first image: calibration object placed at a distance of 0.5 and 0.1
* 'below' the camera coordinate system
* orientation 'read from left to right': rotated 30 degrees
* around the optical axis of the camera (z-axis),
* then tilted 10 degrees around the new y-axis
create_pose(0.1, 0.0, 0.5, 30, 10, 0, 'Rp+T', 'abg', 'point', StartPose1)
* (accumulate all poses in StartPoses = [StartPose1, StartPose2, ...])
* perform the calibration
camera_calibration(WorldPointsX, WorldPointsY, WorldPointsZ, \
PixelsRow, PixelsColumn, CameraParam, StartPoses, \
'pose', FinalCamParam, FinalPoses, Errors)
create_posecreate_poseCreatePosecreate_poseCreatePoseCreatePose returns 2 (H_MSG_TRUE) if all parameter values are
correct. If necessary, an exception is raised.
pose_to_hom_mat3dpose_to_hom_mat3dPoseToHomMat3dpose_to_hom_mat3dPoseToHomMat3dPoseToHomMat3d,
write_posewrite_poseWritePosewrite_poseWritePoseWritePose,
camera_calibrationcamera_calibrationCameraCalibrationcamera_calibrationCameraCalibrationCameraCalibration,
hand_eye_calibrationhand_eye_calibrationHandEyeCalibrationhand_eye_calibrationHandEyeCalibrationHandEyeCalibration
read_poseread_poseReadPoseread_poseReadPoseReadPose,
hom_mat3d_to_posehom_mat3d_to_poseHomMat3dToPosehom_mat3d_to_poseHomMat3dToPoseHomMat3dToPose
hom_mat3d_rotatehom_mat3d_rotateHomMat3dRotatehom_mat3d_rotateHomMat3dRotateHomMat3dRotate,
hom_mat3d_translatehom_mat3d_translateHomMat3dTranslatehom_mat3d_translateHomMat3dTranslateHomMat3dTranslate,
convert_pose_typeconvert_pose_typeConvertPoseTypeconvert_pose_typeConvertPoseTypeConvertPoseType,
get_pose_typeget_pose_typeGetPoseTypeget_pose_typeGetPoseTypeGetPoseType,
hom_mat3d_to_posehom_mat3d_to_poseHomMat3dToPosehom_mat3d_to_poseHomMat3dToPoseHomMat3dToPose,
pose_to_hom_mat3dpose_to_hom_mat3dPoseToHomMat3dpose_to_hom_mat3dPoseToHomMat3dPoseToHomMat3d,
write_posewrite_poseWritePosewrite_poseWritePoseWritePose,
read_poseread_poseReadPoseread_poseReadPoseReadPose
Foundation