Ask Your Question

# 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, imagePoints, Scalar(0, 0, 255), 3);
line(_image, imagePoints, imagePoints, Scalar(0, 255, 0), 3);
line(_image, imagePoints, imagePoints, Scalar(255, 0, 0), 3);
}

edit retag close merge delete

## 1 answer

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, imagePoints, cv::Scalar(0, 0, 255), 3);
cv::line(_image, imagePoints, imagePoints, cv::Scalar(0, 255, 0), 3);
cv::line(_image, imagePoints, imagePoints, 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;
cv::FileStorage fs("path_to_calibration_file", cv::FileStorage::READ);
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 -0500

Seen: 1,739 times

Last updated: Dec 14 '15