1 | initial version |
The drawing function seems fine for me. What version of OpenCV did you use ?
I tested the drawAxis function with a chessboard and it works normally.
The corresponding code:
#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;
}
2 | No.2 Revision |
Linked question on SO for further reference.
The drawing function seems fine for me. What version of OpenCV did you use ?
I tested the drawAxis function with a chessboard and it works normally.
The corresponding code: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;
}