Wykrywam sobie aruco za pomocą OpenCV 3.x
Wszystko fajnie ale model tak jakby przesuwał się lekko (punkt 0.0, 0.0, 0.0 nie jest w pozycji, gdzie rysowane są osie), gdy rotuję kamerę.
A no i właśnie osie rysowane za pomocą OpenCV są dobrze umiejscowione, tylko model rysowane za pomocą OGL źle.
Wyliczanie macierzy widoku:
bool invalid = false;
for (int i = 0; i < 3 && !invalid; i++) {
if (translationVec[i] != -999999)
invalid |= false;
if (rotationVec[i] != -999999)
invalid |= false;
}
if (invalid)
throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__, __LINE__);
cv::Mat Rot(3, 3, CV_32F), Jacob;
cv::Rodrigues(rotationVec, Rot, Jacob);
float para[3][4];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
{
para[i][j] = Rot.at< double >(i, j);
}
// now, add the translation
para[0][3] = translationVec[0];
para[1][3] = translationVec[1];
para[2][3] = translationVec[2];
float scale = 1;
m_matrix = cv::Mat(4, 4, CV_32F);
m_matrix.at<float>(cv::Point(0, 0)) = para[0][0];
// R1C2
m_matrix.at<float>(cv::Point(0, 1)) = para[0][1];
m_matrix.at<float>(cv::Point(0, 2)) = para[0][2];
m_matrix.at<float>(cv::Point(0, 3)) = para[0][3];
// R2
m_matrix.at<float>(cv::Point(1, 0)) = -para[1][0];
m_matrix.at<float>(cv::Point(1, 1)) = -para[1][1];
m_matrix.at<float>(cv::Point(1, 2)) = -para[1][2];
m_matrix.at<float>(cv::Point(1, 3)) = -para[1][3];
// R3
m_matrix.at<float>(cv::Point(2, 0)) = -para[2][0];
m_matrix.at<float>(cv::Point(2, 1)) = -para[2][1];
m_matrix.at<float>(cv::Point(2, 2)) = -para[2][2];
m_matrix.at<float>(cv::Point(2, 3)) = -para[2][3];
m_matrix.at<float>(cv::Point(3, 0)) = 0.0;
m_matrix.at<float>(cv::Point(3, 1)) = 0.0;
m_matrix.at<float>(cv::Point(3, 2)) = 0.0;
m_matrix.at<float>(cv::Point(3, 3)) = 1.0;
if (scale != 0.0) {
m_matrix.at<float>(cv::Point(0, 3)) *= scale;
m_matrix.at<float>(cv::Point(1, 3)) *= scale;
m_matrix.at<float>(cv::Point(2, 3)) *= scale;
}
Wyliczanie macierzy projekcji:
void Projection::InitializePerspectiveProjection(const Camera* camera, double near, double far)
{
/*if (cv::countNonZero(camera->GetDistortionCoefficients()) != 0)
Log::WarningLog::Instance().Write("Wrong distortion coeficients of camera!", "The camera has distortion coefficients");*/
if (camera->IsValid() == false)
Log::CriticalLog::Instance().Write("Invalid camera parameters!", "CameraParameters::glGetProjectionMatrix: Invalid camera parameters");
// Deterime the rsized info
double fx = camera->GetCameraMatrix().at< double >(0, 0);
double cx = camera->GetCameraMatrix().at< double >(0, 2);
double fy = camera->GetCameraMatrix().at< double >(1, 1);
double cy = camera->GetCameraMatrix().at< double >(1, 2);
double cparam[3][4] = { { fx, 0, cx, 0 },{ 0, fy, cy, 0 },{ 0, 0, 1, 0 } };
ArgConvGLcpara2(cparam, camera->GetFrame().size().width, camera->GetFrame().size().height, near, far);
}
void Projection::ArgConvGLcpara2(double cparam[3][4], int width, int height, double near, double far)
{
double icpara[3][4];
double trans[3][4];
double p[3][3], q[4][4];
int i, j;
cparam[0][2] *= -1.0;
cparam[1][2] *= -1.0;
cparam[2][2] *= -1.0;
if (ArParamDecompMat(cparam, icpara, trans) < 0)
throw cv::Exception(9002, "parameter error", "MarkerDetector::argConvGLcpara2", __FILE__, __LINE__);
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
p[i][j] = icpara[i][j] / icpara[2][2];
}
}
q[0][0] = (2.0 * p[0][0] / width);
q[0][1] = (2.0 * p[0][1] / width);
q[0][2] = ((2.0 * p[0][2] / width) - 1.0);
q[0][3] = 0.0;
q[1][0] = 0.0;
q[1][1] = (2.0 * p[1][1] / height);
q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
q[1][3] = 0.0;
q[2][0] = 0.0;
q[2][1] = 0.0;
q[2][2] = (far + near) / (far - near);
q[2][3] = -2.0 * far * near / (far - near);
q[3][0] = 0.0;
q[3][1] = 0.0;
q[3][2] = 1.0;
q[3][3] = 0.0;
for (i = 0; i < 4; i++) {
for (j = 0; j < 3; j++) {
m_matrix[i + j * 4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j];
}
m_matrix[i + 3 * 4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3];
}
}
int Projection::ArParamDecompMat(double source[3][4], double cpara[3][4], double trans[3][4])
{
int r, c;
double Cpara[3][4];
double rem1, rem2, rem3;
if (source[2][3] >= 0) {
for (r = 0; r < 3; r++) {
for (c = 0; c < 4; c++) {
Cpara[r][c] = source[r][c];
}
}
}
else {
for (r = 0; r < 3; r++) {
for (c = 0; c < 4; c++) {
Cpara[r][c] = -(source[r][c]);
}
}
}
for (r = 0; r < 3; r++) {
for (c = 0; c < 4; c++) {
cpara[r][c] = 0.0;
}
}
cpara[2][2] = Norm(Cpara[2][0], Cpara[2][1], Cpara[2][2]);
trans[2][0] = Cpara[2][0] / cpara[2][2];
trans[2][1] = Cpara[2][1] / cpara[2][2];
trans[2][2] = Cpara[2][2] / cpara[2][2];
trans[2][3] = Cpara[2][3] / cpara[2][2];
cpara[1][2] = Dot(trans[2][0], trans[2][1], trans[2][2], Cpara[1][0], Cpara[1][1], Cpara[1][2]);
rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
cpara[1][1] = Norm(rem1, rem2, rem3);
trans[1][0] = rem1 / cpara[1][1];
trans[1][1] = rem2 / cpara[1][1];
trans[1][2] = rem3 / cpara[1][1];
cpara[0][2] = Dot(trans[2][0], trans[2][1], trans[2][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]);
cpara[0][1] = Dot(trans[1][0], trans[1][1], trans[1][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]);
rem1 = Cpara[0][0] - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0];
rem2 = Cpara[0][1] - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1];
rem3 = Cpara[0][2] - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2];
cpara[0][0] = Norm(rem1, rem2, rem3);
trans[0][0] = rem1 / cpara[0][0];
trans[0][1] = rem2 / cpara[0][0];
trans[0][2] = rem3 / cpara[0][0];
trans[1][3] = (Cpara[1][3] - cpara[1][2] * trans[2][3]) / cpara[1][1];
trans[0][3] = (Cpara[0][3] - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0];
for (r = 0; r < 3; r++) {
for (c = 0; c < 3; c++) {
cpara[r][c] /= cpara[2][2];
}
}
return 0;
}
double Projection::Norm(double a, double b, double c) { return (sqrt(a * a + b * b + c * c)); }
double Projection::Dot(double a1, double a2, double a3, double b1, double b2, double b3) { return (a1 * b1 + a2 * b2 + a3 * b3); }
Wykrywanie tablicy aruco:
std::vector<GLint> ids;
std::vector<std::vector<cv::Point2f>> corners;
cv::aruco::detectMarkers(m_camera->GetFrame(), m_dictionary, corners, ids);
cv::Vec3d rotationVec, translationVec;
GLint valid = estimatePoseBoard(corners, ids, m_board, m_camera->GetCameraMatrix(), cv::Mat(), rotationVec, translationVec);
// If at least one board marker detected
if (valid > 0)
{
m_transform.UpdateMatrix(translationVec, rotationVec);
result = true;
}
Jeśli chodzi o czytanie klatki z kamery:
if (m_inputVideo.isOpened())
{
while (m_inputVideo.grab())
{
//m_inputVideo.retrieve(m_frame);
if (m_inputVideo.read(m_frame))
{
//cv::undistort(beforeUnd, m_frame, mm, m_distortionCoefficients);
break;
}
}
}
Nie wykonuję operacji cv::undistort - w przykładzie nie wykonywali jej, więc chyba może tak być?
Rada?
Wyliczanie projekcji i model widok wzięte bezpośrednie z aruco (starego).