# Pose Estimation: Where is my error in reasoning?

Hi,

I am trying to do pose estimation. But I am clearly doing something wrong. Let's say I have a pattern consisting of 4 Markers (A,B,C,D). Each of these markers has an image coordinate and a pattern coordinate. The origin of the pattern is the center of the polygon.

The image coordinates (x/y) are the following. (In a 1280x960 image)

• Origin(616/814)
• A(561/664)

• B(702/838)

• C(637/982)

• D(520/755)

Pattern coordinates (x/y/z)

• Origin(0/0/0)

• A(-12/32/0)

• B(18/-5/0)

• C(12/-36/0)

• D(21/13/0)

Now it rotates by 90 degrees, but my coordinate system does not rotate with the pattern. I am wondering what is wrong? Is it because the Z coordinate is always 0?

• (x/y)
• Origin(632/784)
• A(718/812)

• B(567/938)

• C(441/909)

• D(632/784)

Pattern coordinates (x/y/z)

• Origin(0/0/0)

• A(32/12/0)

• B(-4/18/0)

• C(-35/11/0)

• D(11/19/0)

I am using solvePnP like this

 cv::solvePnP(patternPoints, imgPoints, cameraMatrix, distCoeffs, rvec, tvec);


Drawing the axis

//Method Call
pattern.drawAxis(image, camMatrix, distCoeffs, rvec, tvec,10);

//Implementation (taken from aruco.cpp)

void drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
InputArray _rvec, InputArray _tvec, float length) {

CV_Assert(_image.getMat().total() != 0 &&
(_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
CV_Assert(length > 0);

// project axis points
vector< Point3f > axisPoints;
axisPoints.push_back(Point3f(0, 0, 0));
axisPoints.push_back(Point3f(length, 0, 0));
axisPoints.push_back(Point3f(0, length, 0));
axisPoints.push_back(Point3f(0, 0, length));
vector< Point2f > imagePoints;
projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePoints);

// draw axis lines
line(_image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), 3);
line(_image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), 3);
line(_image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), 3);
}

edit retag close merge delete

Sort by ยป oldest newest most voted

Linked question on SO for further reference.

The drawing function seems fine for me. What version of OpenCV did you use ?

• Did you check you supply the correct rvec and tvec to drawAxis function ?
• Did you check that each element for the same index of patternPoints corresponds to the correct imgPoints ?

I tested the drawAxis function with a chessboard and it works normally.

The corresponding code (result):

#include <iostream>
#include <opencv2/opencv.hpp>

void drawAxis(cv::Mat &_image, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
cv::InputArray _rvec, cv::InputArray _tvec, float length) {
// project axis points
std::vector< cv::Point3f > axisPoints;
axisPoints.push_back(cv::Point3f(0, 0, 0));
axisPoints.push_back(cv::Point3f(length, 0, 0));
axisPoints.push_back(cv::Point3f(0, length, 0));
axisPoints.push_back(cv::Point3f(0, 0, length));
std::vector< cv::Point2f > imagePoints;
cv::projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePoints);

// draw axis lines
cv::line(_image, imagePoints[0], imagePoints[1], cv::Scalar(0, 0, 255), 3);
cv::line(_image, imagePoints[0], imagePoints[2], cv::Scalar(0, 255, 0), 3);
cv::line(_image, imagePoints[0], imagePoints[3], cv::Scalar(255, 0, 0), 3);
}

int main(int argc, char **argv) {
cv::Mat matImg;
cv::VideoCapture capture(0);

if(capture.isOpened()) {
cv::Size boardSize(9, 6);

//Construct the chessboard model
double squareSize = 0.036;
std::vector<cv::Point3d> objectPoints;
for (int i = 0; i < boardSize.height; i++) {
for (int j = 0; j < boardSize.width; j++) {
objectPoints.push_back(
cv::Point3d(double(j * squareSize), float(i * squareSize), 0));
}
}

cv::Mat cameraMatrix, distCoeffs;
fs["camera_matrix"] >> cameraMatrix;
fs["distortion_coefficients"] >> distCoeffs;

while(true) {
capture >> matImg;
if(matImg.empty()) {
break;
}

//Found chessboard corners
std::vector<cv::Point2f> imagePoints;
bool found = cv::findChessboardCorners(matImg, boardSize, imagePoints, cv::CALIB_CB_FAST_CHECK);

if(found) {
cv::drawChessboardCorners(matImg, boardSize, cv::Mat(imagePoints), found);

//SolvePnP
cv::Mat rvec, tvec;
cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);

drawAxis(matImg, cameraMatrix, distCoeffs, rvec, tvec, squareSize);
}

cv::imshow("Camera", matImg);
char c = cv::waitKey(30);

if(c == 27) {
break;
}
}
} else {
std::cout << "Cannot open cv::VideoCapture !" << std::endl;
}

return 0;
}

more

Official site

GitHub

Wiki

Documentation

## Stats

Asked: 2015-12-14 02:34:33 -0600

Seen: 2,104 times

Last updated: Dec 14 '15