Pose Estimation: Where is my error in reasoning?

edit

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;
}