Pose estimation

The tutorial shows the general structure of a program that uses the PnP pose estimator to determine the camera pose.

Initialize the pose estimator

We select a conservative number of ransac and non-linear optimizer iterations (10 and 10). contextHandle is assumed to be previously initialized dwContextHandle_t.

dwPnP_initialize(&pnp, 10, 10, contextHandle);

Obtain matches

dwVector3f worldPoints[BUFFER_SIZE];
dwVector2f imagePoints[BUFFER_SIZE];
size_t pointCount;
 
// The application must obtain the matches and fill the above three variables

Undistort feature points

cameraModel is assumed to be previously initialized dwCameraModelHandle_t

dwVector2f rays[BUFFER_SIZE];
 
for(size_t i = 0; i < pointCount; ++i)
{
    const auto& point = imagePoints[i];
    auto& ray = rays[i];
    dwCameraModel_pixel2Ray(&ray.x, &ray.y, &ray.z, point.x, point.y, cameraModel);
}

Estimate pose

dwTransformation3f worldToCamera;
dwPnP_solve(&worldToCamera, pointCount, rays, worldPoints, pnp);

Finally, free previously allocated memory.

dwPnP_release(pnp);