PoseEstimator.cpp
Go to the documentation of this file.
00001 #include "PoseEstimator.h"
00002 
00003 #include <LinearMath/btTransform.h>
00004 
00005 #include "rpp/librpp.h"
00006 
00007 bool PoseEstimator::estimatePose(aruco::Marker marker, double intrinsicCameraMatrix[], btVector3& position, btQuaternion& orientation)
00008 {
00009         double low = 0 * markerSize;
00010         double high = 1 * markerSize;
00011 
00012         // Output
00013         rpp_float error;
00014         rpp_mat rotation;
00015         rpp_vec translation;
00016 
00017         // Input
00018         rpp_float cameraPrincipalPoint[2] = { intrinsicCameraMatrix[0 * 3 + 2 * 1], intrinsicCameraMatrix[1 * 3 + 2 * 1] };
00019         rpp_float cameraFocalLength[2] = { intrinsicCameraMatrix[0 * 3 + 0 * 1], intrinsicCameraMatrix[1 * 3 + 1 * 1] };
00020 
00021         rpp_vec modelPoints[4];
00022         modelPoints[0][0] = low; modelPoints[0][1] = low; modelPoints[0][2] = 0;
00023         modelPoints[1][0] = high; modelPoints[1][1] = low; modelPoints[1][2] = 0;
00024         modelPoints[2][0] = high; modelPoints[2][1] = high; modelPoints[2][2] = 0;
00025         modelPoints[3][0] = low; modelPoints[3][1] = high; modelPoints[3][2] = 0;
00026         rpp_vec imagePoints[4];
00027         imagePoints[0][0] = marker[0].x; imagePoints[0][1] = marker[0].y; imagePoints[0][2] = 1;
00028         imagePoints[1][0] = marker[1].x; imagePoints[1][1] = marker[1].y; imagePoints[1][2] = 1;
00029         imagePoints[2][0] = marker[2].x; imagePoints[2][1] = marker[2].y; imagePoints[2][2] = 1;
00030         imagePoints[3][0] = marker[3].x; imagePoints[3][1] = marker[3].y; imagePoints[3][2] = 1;
00031 
00032         robustPlanarPose(error, rotation, translation, cameraPrincipalPoint, cameraFocalLength, modelPoints, imagePoints, 4, 0, true, 1E-8, 1E-5, 0);
00033 
00034         position = btVector3(translation[0], translation[1], translation[2]);
00035 
00036         btMatrix3x3
00037         (
00038                 rotation[0][0], rotation[0][1], rotation[0][2],
00039                 rotation[1][0], rotation[1][1], rotation[1][2],
00040                 rotation[2][0], rotation[2][1], rotation[2][2]
00041         )
00042         .getRotation(orientation);
00043 
00044         return error < 1E10;
00045 }


aruco_pose
Author(s): Julian Brunner
autogenerated on Mon Oct 6 2014 08:32:34