| 107 | } |
| 108 | |
| 109 | bool BeaconBasedPoseEstimator::SetCameraMatrix( |
| 110 | const DoubleVecVec &cameraMatrix) { |
| 111 | // Our existing pose won't match anymore. |
| 112 | m_gotPose = false; |
| 113 | |
| 114 | // Construct the camera matrix from the vectors we received. |
| 115 | if (cameraMatrix.size() != 3) { |
| 116 | return false; |
| 117 | } |
| 118 | for (size_t i = 0; i < cameraMatrix.size(); i++) { |
| 119 | if (cameraMatrix[i].size() != 3) { |
| 120 | return false; |
| 121 | } |
| 122 | } |
| 123 | cv::Mat newCameraMatrix(static_cast<int>(cameraMatrix.size()), |
| 124 | static_cast<int>(cameraMatrix[0].size()), |
| 125 | CV_64F); |
| 126 | for (int i = 0; i < cameraMatrix.size(); i++) { |
| 127 | for (int j = 0; j < cameraMatrix[i].size(); j++) { |
| 128 | newCameraMatrix.at<double>(i, j) = cameraMatrix[i][j]; |
| 129 | } |
| 130 | } |
| 131 | |
| 132 | m_cameraMatrix = newCameraMatrix; |
| 133 | // std::cout << "XXX cameraMatrix =" << std::endl << m_cameraMatrix |
| 134 | // << std::endl; |
| 135 | return true; |
| 136 | } |
| 137 | |
| 138 | bool BeaconBasedPoseEstimator::SetDistCoeffs( |
| 139 | const std::vector<double> &distCoeffs) { |