Ask Your Question
1

Pose Estimation: Where is my error in reasoning?

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

RVQ gravatar image

updated 2015-12-14 02:36:10 -0600

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.

Pattern upright

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?

Pattern horizontal

  • (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 flag offensive close merge delete

1 answer

Sort by ยป oldest newest most voted
1

answered 2015-12-14 12:02:31 -0600

Eduardo gravatar image

updated 2015-12-14 14:15:29 -0600

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;
    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;
}
edit flag offensive delete link more

Question Tools

1 follower

Stats

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

Seen: 2,186 times

Last updated: Dec 14 '15