Calibration Externe ajoutée au dépôt
This commit is contained in:
Thomas FORGIONE
parent
ccaf6aebd3
commit
a6c1033873
|
|
@ -426,6 +426,7 @@ int main(int argc, char** argv)
|
|||
|
||||
cv::namedWindow("Image View", 1);
|
||||
|
||||
int img = 0;
|
||||
for (i = 0;;i++)
|
||||
{
|
||||
cv::Mat view, viewGray;
|
||||
|
|
@ -487,6 +488,9 @@ int main(int argc, char** argv)
|
|||
blink = capture.isOpened();
|
||||
}
|
||||
|
||||
cv::Mat m2;
|
||||
view.copyTo(m2);
|
||||
|
||||
if (found)
|
||||
drawChessboardCorners(view, boardSize, cv::Mat(pointbuf), found);
|
||||
|
||||
|
|
@ -504,8 +508,8 @@ int main(int argc, char** argv)
|
|||
msg = cv::format("%d/%d", (int)imagePoints.size(), nframes);
|
||||
}
|
||||
|
||||
putText(view, msg, textOrigin, 1, 1,
|
||||
mode != CALIBRATED ? cv::Scalar(0,0,255) : cv::Scalar(0,255,0));
|
||||
// putText(view, msg, textOrigin, 1, 1,
|
||||
// mode != CALIBRATED ? cv::Scalar(0,0,255) : cv::Scalar(0,255,0));
|
||||
|
||||
if (blink)
|
||||
bitwise_not(view, view);
|
||||
|
|
@ -525,6 +529,13 @@ int main(int argc, char** argv)
|
|||
if (key == 'u' && mode == CALIBRATED)
|
||||
undistortImage = !undistortImage;
|
||||
|
||||
if (key == 'p')
|
||||
{
|
||||
cv::imwrite("img" + std::to_string(img) + ".png", m2);
|
||||
img++;
|
||||
}
|
||||
|
||||
|
||||
if (capture.isOpened() && key == 'g')
|
||||
{
|
||||
mode = CAPTURING;
|
||||
|
|
|
|||
|
|
@ -2,5 +2,7 @@
|
|||
if [ ! -f main ]; then
|
||||
make
|
||||
fi
|
||||
./main -w 5 -h 5 -V ../../data/video/calib.mov -o ../../data/xml/calib.xml
|
||||
# ./main -w 9 -h 6 -V "/home/thomas/N7/old_c/augm/data/video/calib.avi" -o ../../data/xml/calib.xml
|
||||
# ./main -w 9 -h 6 -V ~/Spam/calibintern.mp4 -o ../../data/xml/calib.xml
|
||||
./main -w 9 -h 6 -V 0 -o ../../data/xml/calib.xml
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,112 @@
|
|||
#include "Camera.hpp"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
/**
|
||||
* Initialize the camera loading the internal parameters from the given file
|
||||
*
|
||||
* @param[in] calibFilename the calibration file
|
||||
* @return true if success
|
||||
*/
|
||||
bool Camera::init( std::string calibFilename )
|
||||
{
|
||||
/******************************************************************/
|
||||
// open the file storage with the given filename
|
||||
/******************************************************************/
|
||||
cv::FileStorage fs;
|
||||
|
||||
/******************************************************************/
|
||||
// check if the file storage has been opened correclty
|
||||
/******************************************************************/
|
||||
if( !fs.open(calibFilename, cv::FileStorage::READ))
|
||||
{
|
||||
std::cerr << "Cannot open calib file" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
/******************************************************************/
|
||||
// load the camera_matrix in matK
|
||||
/******************************************************************/
|
||||
fs["camera_matrix"] >> matK;
|
||||
|
||||
/******************************************************************/
|
||||
// load the distortion_coefficients in distCoeff
|
||||
/******************************************************************/
|
||||
fs["distortion_coefficients"] >> distCoeff;
|
||||
|
||||
/******************************************************************/
|
||||
// load image_width and image_height in imageSize.[width|height]
|
||||
/******************************************************************/
|
||||
fs["image_width"] >> imageSize.width;
|
||||
fs["image_height"] >> imageSize.height;
|
||||
|
||||
std::cout << matK << std::endl;
|
||||
std::cout << distCoeff << std::endl;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Return the OpenGL projection matrix for the left camera
|
||||
* @param[out] proj the OGL projection matrix (ready to be passed, ie in col major format)
|
||||
* @param znear near clipping plane
|
||||
* @param zfar far clipping plane
|
||||
* \note using http://strawlab.org/2011/11/05/augmented-reality-with-OpenGL/
|
||||
*/
|
||||
void Camera::getOGLProjectionMatrix( float *proj, const float znear, const float zfar ) const
|
||||
{
|
||||
|
||||
|
||||
// With window_coords==’y down’, we have:
|
||||
//
|
||||
// [2*K00/width, -2*K01/width, (width - 2*K02 + 2*x0)/width, 0]
|
||||
// [ 0, 2*K11/height, (-height + 2*K12 + 2*y0)/height, 0]
|
||||
// [ 0, 0, (-zfar - znear)/(zfar - znear), -2*zfar*znear/(zfar - znear)]
|
||||
// [ 0, 0, -1, 0]
|
||||
|
||||
// //corrected with artoolkitpluss src/Tracker::349
|
||||
// proj[0] = (float)(2*matK.at<double>(0,0))/imageSize.width;
|
||||
// proj[1*4 + 0] = (float)(2*matK.at<double>(0,1))/imageSize.width;
|
||||
// proj[2*4 + 0] = -(float)(imageSize.width - 2*matK.at<double>(0,2) )/imageSize.width;
|
||||
// proj[3*4 + 0] = 0;
|
||||
|
||||
// proj[0*4 + 1] = 0;
|
||||
// // minus -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||||
// proj[1*4 + 1] = -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||||
// proj[2*4 + 1] = (float)(-imageSize.height + 2*matK.at<double>(1,2) )/imageSize.height;
|
||||
// proj[3*4 + 1] = 0;
|
||||
|
||||
// proj[0*4 + 2] = 0;
|
||||
// proj[1*4 + 2] = 0;
|
||||
// proj[2*4 + 2] = (zfar + znear)/(zfar - znear);
|
||||
// proj[3*4 + 2] = -2*zfar*znear/(zfar - znear);
|
||||
|
||||
// proj[0*4 + 3] = 0;
|
||||
// proj[1*4 + 3] = 0;
|
||||
// proj[2*4 + 3] = 1;
|
||||
// proj[3*4 + 3] = 0;
|
||||
|
||||
//corrected with artoolkitpluss src/Tracker::349
|
||||
proj[0] = (float)(2*matK.at<double>(0,0))/imageSize.width;
|
||||
proj[1*4 + 0] = (float)(2*matK.at<double>(0,1))/imageSize.width;
|
||||
proj[2*4 + 0] = -(float)(imageSize.width - 2*matK.at<double>(0,2) )/imageSize.width;
|
||||
proj[3*4 + 0] = 0;
|
||||
|
||||
proj[0*4 + 1] = 0;
|
||||
// minus -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||||
proj[1*4 + 1] = -(float)(2*matK.at<double>(1,1))/imageSize.height;
|
||||
proj[2*4 + 1] = -(float)(-imageSize.height + 2*matK.at<double>(1,2) )/imageSize.height;
|
||||
proj[3*4 + 1] = 0;
|
||||
|
||||
proj[0*4 + 2] = 0;
|
||||
proj[1*4 + 2] = 0;
|
||||
proj[2*4 + 2] = (zfar + znear)/(zfar - znear);
|
||||
proj[3*4 + 2] = -2*zfar*znear/(zfar - znear);
|
||||
|
||||
proj[0*4 + 3] = 0;
|
||||
proj[1*4 + 3] = 0;
|
||||
proj[2*4 + 3] = 1;
|
||||
proj[3*4 + 3] = 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
#ifndef _CAMERA_HPP_
|
||||
#define _CAMERA_HPP_
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
class Camera
|
||||
{
|
||||
public:
|
||||
|
||||
Camera() {}
|
||||
|
||||
/**
|
||||
* Initialize the camera loading the internal parameters from the given file
|
||||
*
|
||||
* @param[in] calibFilename the calibration file
|
||||
* @return true if success
|
||||
*/
|
||||
bool init( std::string calibFilename );
|
||||
|
||||
~Camera( ) {}
|
||||
|
||||
|
||||
/**
|
||||
* Return the OpenGL projection matrix for the camera
|
||||
* @param[out] proj the OGL projection matrix (ready to be passed, ie in col major format)
|
||||
* @param znear near clipping plane
|
||||
* @param zfar far clipping plane
|
||||
* \note using http://strawlab.org/2011/11/05/augmented-reality-with-OpenGL/
|
||||
*/
|
||||
void getOGLProjectionMatrix( float *proj, const float znear, const float zfar ) const;
|
||||
|
||||
|
||||
public:
|
||||
cv::Mat matK;
|
||||
|
||||
cv::Mat distCoeff;
|
||||
|
||||
cv::Size imageSize;
|
||||
|
||||
};
|
||||
|
||||
#endif /* _CAMERA_HPP_ */
|
||||
|
|
@ -0,0 +1,77 @@
|
|||
#include "ChessboardCameraTracker.hpp"
|
||||
#include "utility.hpp"
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
/**
|
||||
* It detects a chessboard inside an image and if found it returns the pose of the camera wrt the chessboard
|
||||
*
|
||||
* @param[in,out] img the original image
|
||||
* @param[out] pose the pose of the camera
|
||||
* @param[in] cam the camera
|
||||
* @param[in] boardSize the size of the chessboard to detect
|
||||
* @return true if the chessboard has been found
|
||||
*/
|
||||
bool ChessboardCameraTracker::process( cv::Mat &img, cv::Mat &pose, const Camera & cam, const cv::Size &boardSize )
|
||||
{
|
||||
cv::Mat undistorted_img, H;
|
||||
|
||||
// true if the chessboard is found
|
||||
bool found = false;
|
||||
|
||||
// contains the points detected on the chessboard
|
||||
std::vector<cv::Point2f> corners;
|
||||
|
||||
//******************************************************************/
|
||||
// undistort the input image. view at the end must contain the undistorted version
|
||||
// of the image.
|
||||
//******************************************************************/
|
||||
cv::undistort(img, undistorted_img, cam.matK, cam.distCoeff);
|
||||
cv::imshow("Image View", undistorted_img);
|
||||
cv::waitKey(-1);
|
||||
//******************************************************************/
|
||||
// detect the chessboard
|
||||
//******************************************************************/
|
||||
found = detectChessboard(undistorted_img, corners, boardSize);
|
||||
|
||||
// cout << ( (!found ) ? ( "No " ) : ("") ) << "chessboard detected!" << endl;
|
||||
cv::drawChessboardCorners(undistorted_img, boardSize, corners, found);
|
||||
|
||||
//******************************************************************/
|
||||
// if a chessboard is found estimate the homography and rectify the image
|
||||
//******************************************************************/
|
||||
if( found )
|
||||
{
|
||||
|
||||
// contains the points on the chessboard in the chessboard reference system
|
||||
std::vector<cv::Point2f> refCorners;
|
||||
|
||||
//******************************************************************/
|
||||
// generate the coordinates of the points on the chessboard in the chessboard reference system
|
||||
//******************************************************************/
|
||||
calcChessboardCorners(boardSize, 25, refCorners);
|
||||
|
||||
//******************************************************************/
|
||||
// estimate the homography
|
||||
// --> see findHomography
|
||||
// http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=homography#findhomography
|
||||
//******************************************************************/
|
||||
H = cv::findHomography(refCorners, corners, CV_RANSAC);
|
||||
|
||||
|
||||
// cout << "H = " << H << endl << endl;
|
||||
// cout << "corners =" << corners << endl << endl;
|
||||
// cout << "ptsOb =" << objectPoints << endl << endl;
|
||||
|
||||
//******************************************************************/
|
||||
// decompose the homography
|
||||
//******************************************************************/
|
||||
decomposeHomography(H, cam.matK, pose);
|
||||
}
|
||||
|
||||
return found;
|
||||
}
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
#ifndef _CHESSBOARDCAMERATRACKER_HPP_
|
||||
#define _CHESSBOARDCAMERATRACKER_HPP_
|
||||
|
||||
#include "Camera.hpp"
|
||||
#include "utility.hpp"
|
||||
|
||||
class ChessboardCameraTracker
|
||||
{
|
||||
public:
|
||||
|
||||
ChessboardCameraTracker() = default;
|
||||
|
||||
|
||||
/**
|
||||
* It detects a chessboard inside an image and if found it returns the pose of the camera wrt the chessboard
|
||||
*
|
||||
* @param[in,out] img the original image
|
||||
* @param[out] pose the pose of the camera
|
||||
* @param[in] cam the camera
|
||||
* @param[in] boardSize the size of the chessboard to detect
|
||||
* @return true if the chessboard has been found
|
||||
*/
|
||||
bool process( cv::Mat &img, cv::Mat &pose, const Camera & cam, const cv::Size &boardSize);
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif /*_CHESSBOARDCAMERATRACKER_HPP_*/
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
SOURCES = utility.cpp Camera.cpp main.cpp ChessboardCameraTracker.cpp
|
||||
|
||||
OBJECTS = $(SOURCES:.cpp=.o)
|
||||
TARGET = main
|
||||
LIBS = $(shell pkg-config opencv --libs)
|
||||
CXX = g++
|
||||
|
||||
%.o: %.cpp
|
||||
$(CXX) -std=c++14 -o $@ -c $< -Wall `pkg-config opencv --cflags`
|
||||
|
||||
all: $(OBJECTS)
|
||||
$(CXX) -std=c++14 -o $(TARGET) $(OBJECTS) $(LIBS) -Wall
|
||||
|
||||
x: all
|
||||
./$(TARGET)
|
||||
|
||||
clean:
|
||||
rm -rf $(OBJECTS)
|
||||
|
||||
superclean: clean
|
||||
rm -rf $(TARGET)
|
||||
|
|
@ -0,0 +1,181 @@
|
|||
#include "ChessboardCameraTracker.hpp"
|
||||
#include "utility.hpp"
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <iostream>
|
||||
#include <time.h>
|
||||
|
||||
// Display the help for the programm
|
||||
void help( const char* programName );
|
||||
|
||||
// parse the input command line arguments
|
||||
bool parseArgs( int argc, char**argv, cv::Size &boardSize, std::string &inputFilename );
|
||||
|
||||
|
||||
int main( int argc, char** argv )
|
||||
{
|
||||
/******************************************************************/
|
||||
/* CONSTANTS to use */
|
||||
/******************************************************************/
|
||||
|
||||
// the name of the window
|
||||
const std::string WINDOW_NAME = "Image View";
|
||||
|
||||
|
||||
/******************************************************************/
|
||||
/* VARIABLES TO use */
|
||||
/******************************************************************/
|
||||
|
||||
cv::Mat view; // it will contain the original image loaded from file
|
||||
Camera cam;
|
||||
cam.init("/home/thomas/Paella/Code/data/xml/calib.xml");
|
||||
|
||||
ChessboardCameraTracker tracker;
|
||||
cv::Mat pose;
|
||||
|
||||
std::vector<cv::Point2f> pointbuf; // it will contain the detected corners on the chessboard
|
||||
|
||||
// it will contain the size in terms of corners (width X height) of the chessboard
|
||||
cv::Size boardSize;
|
||||
|
||||
// it will contains the filename of the image file
|
||||
std::string inputFilename;
|
||||
|
||||
|
||||
/******************************************************************/
|
||||
/* READ THE INPUT PARAMETERS - DO NOT MODIFY */
|
||||
/******************************************************************/
|
||||
|
||||
if( !parseArgs( argc, argv, boardSize, inputFilename) )
|
||||
{
|
||||
std::cerr << "Aborting..." << std::endl;
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
|
||||
/******************************************************************/
|
||||
/* PART TO DEVELOP */
|
||||
/******************************************************************/
|
||||
|
||||
/******************************************************************/
|
||||
// create a window to display the image --> see namedWindow
|
||||
/******************************************************************/
|
||||
cv::namedWindow(WINDOW_NAME, CV_WINDOW_AUTOSIZE );
|
||||
|
||||
/******************************************************************/
|
||||
// read the input image from file into "view" --> see imread
|
||||
/******************************************************************/
|
||||
view = cv::imread(inputFilename, CV_LOAD_IMAGE_COLOR);
|
||||
|
||||
//Measure the execution time, get time before function call
|
||||
// double t = (double)cv::getTickCount();
|
||||
|
||||
/******************************************************************/
|
||||
// call the function that detects the chessboard on the image
|
||||
// found = detectChessboard...
|
||||
/******************************************************************/
|
||||
// found = detectChessboard(view, pointbuf, boardSize, pattern);
|
||||
|
||||
|
||||
|
||||
|
||||
// // get time after function call and display info
|
||||
// t = ((double)cv::getTickCount() - t)/cv::getTickFrequency();
|
||||
|
||||
// std::cout << ( (!found ) ? ( "No " ) : ("") ) << "chessboard detected!" << std::endl;
|
||||
// std::cout << "Chessboard detection took " << t*1000 << "ms" << std::endl;
|
||||
|
||||
|
||||
// /******************************************************************/
|
||||
// // if the chessboard is found draw the cornerns on top of it
|
||||
// // --> see drawChessboardCorners
|
||||
// /******************************************************************/
|
||||
// cv::drawChessboardCorners(view, boardSize, pointbuf, found);
|
||||
|
||||
if(tracker.process(view, pose, cam, boardSize))
|
||||
{
|
||||
std::cout << "Drawing Reference system"<< std::endl;
|
||||
drawReferenceSystem(view,cam,pose,1,200,false );
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Unable to process" << std::endl;
|
||||
}
|
||||
|
||||
/******************************************************************/
|
||||
// show the image inside the window --> see imshow
|
||||
/******************************************************************/
|
||||
// cv::resize(view, view, cv::Size{view.cols/5, view.rows/5});
|
||||
cv::imshow(WINDOW_NAME, view);
|
||||
|
||||
// wait for user input before ending --> see waitKey
|
||||
cv::waitKey( -1 );
|
||||
|
||||
|
||||
return EXIT_SUCCESS;
|
||||
}
|
||||
|
||||
|
||||
// Display the help for the programm
|
||||
void help( const char* programName )
|
||||
{
|
||||
std::cout << "Detect a chessboard in a given image" << std::endl
|
||||
<< "Usage: " << programName << std::endl
|
||||
<< " -w <board_width> # the number of inner corners per one of board dimension" << std::endl
|
||||
<< " -h <board_height> # the number of inner corners per another board dimension" << std::endl
|
||||
<< " [-pt <pattern=[circles|acircles|chess]>] # the type of pattern: chessboard or circles' grid" << std::endl
|
||||
<< " <image file> " << std::endl
|
||||
<< std::endl;
|
||||
}
|
||||
|
||||
// parse the input command line arguments
|
||||
bool parseArgs( int argc, char**argv, cv::Size &boardSize, std::string &inputFilename )
|
||||
{
|
||||
// check the minimum number of arguments
|
||||
if( argc < 3 )
|
||||
{
|
||||
help( argv[0] );
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
// Read the input arguments
|
||||
for( int i = 1; i < argc; i++ )
|
||||
{
|
||||
const char* s = argv[i];
|
||||
if( strcmp( s, "-w" ) == 0 )
|
||||
{
|
||||
if( sscanf( argv[++i], "%u", &boardSize.width ) != 1 || boardSize.width <= 0 )
|
||||
{
|
||||
std::cerr << "Invalid board width" << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if( strcmp( s, "-h" ) == 0 )
|
||||
{
|
||||
if( sscanf( argv[++i], "%u", &boardSize.height ) != 1 || boardSize.height <= 0 )
|
||||
{
|
||||
std::cerr << "Invalid board height" << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if( s[0] != '-' )
|
||||
{
|
||||
inputFilename.assign(s);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cerr << "Unknown option " << s << std::endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,288 @@
|
|||
#include "utility.hpp"
|
||||
|
||||
#include <opencv2/calib3d/calib3d.hpp>
|
||||
#include <opencv2/imgproc/imgproc.hpp>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
/******************************************************************/
|
||||
/* FUNCTIONS TO DEVELOP */
|
||||
/******************************************************************/
|
||||
/**
|
||||
* Detect a chessboard in a given image
|
||||
*
|
||||
* @param[in] rgbimage The rgb image to process
|
||||
* @param[out] pointbuf the set of 2D image corner detected on the chessboard
|
||||
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
||||
* @return true if the chessboard is detected inside the image, false otherwise
|
||||
*/
|
||||
bool detectChessboard( const cv::Mat &rgbimage, std::vector<cv::Point2f> &pointbuf, const cv::Size &boardSize)
|
||||
{
|
||||
// it contains the value to return
|
||||
bool found = false;
|
||||
|
||||
|
||||
/******************************************************************/
|
||||
// detect the chessboard --> see findChessboardCorners
|
||||
/******************************************************************/
|
||||
found = findChessboardCorners(rgbimage, boardSize, pointbuf);
|
||||
|
||||
// if a chessboard is found refine the position of the points in a window 11x11 pixel
|
||||
// use the default value for the termination criteria --> TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 )
|
||||
|
||||
if ( found )
|
||||
{
|
||||
cv::Mat viewGrey; // it will contain the graylevel version of the image
|
||||
|
||||
/******************************************************************/
|
||||
// convert the image in "rgbimage" to gray level and save it in "viewGrey"
|
||||
// --> cvtColor with CV_BGR2GRAY option
|
||||
/******************************************************************/
|
||||
cv::cvtColor(rgbimage, viewGrey, CV_BGR2GRAY,1);
|
||||
|
||||
/******************************************************************/
|
||||
// refine the corner location in "pointbuf" using "viewGrey"
|
||||
// --> see cornerSubPix
|
||||
/******************************************************************/
|
||||
cv::cornerSubPix(viewGrey, pointbuf, boardSize, cv::Size{5,5}, cv::TermCriteria{CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1});
|
||||
|
||||
}
|
||||
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
*
|
||||
* @param[in,out] rgbimage The image on which to draw the reference system
|
||||
* @param[in] cam The camera
|
||||
* @param[in] projMat The projection matrix of the camera
|
||||
* @param[in] thickness The thickness of the line
|
||||
* @param[in] scale A scale factor for the unit vectors to draw
|
||||
* @param[in] alreadyUndistorted A boolean value that tells if the input image rgbimage is already undistorted or we are working on a distorted image
|
||||
*/
|
||||
void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &projMat, const int &thickness, const double &scale, const bool alreadyUndistorted )
|
||||
{
|
||||
|
||||
// contains the points to project to draw the 3 axis
|
||||
//******************************************************************/
|
||||
// Add the four 3D points (Point3f) that we can use to draw
|
||||
// the reference system to vertex3D. Use <scale> as unit
|
||||
//******************************************************************/
|
||||
float scalef = static_cast<float>(scale);
|
||||
|
||||
std::vector<cv::Point3f> vertex3D{cv::Point3f{0,0,0},
|
||||
cv::Point3f{scalef,0,0},
|
||||
cv::Point3f{0,scalef,0},
|
||||
cv::Point3f{0,0,-scalef}
|
||||
};
|
||||
|
||||
// contains the projected 3D points on the image
|
||||
std::vector<cv::Point2f> imgRefPts;
|
||||
|
||||
//******************************************************************/
|
||||
// Project the 3D points using myProjectPoints. Attention, check the
|
||||
// flag alreadyUndistorted to see if we have to apply the distortion:
|
||||
// if it is true we pass a 1x5 zero vector, otherwise the distortion
|
||||
// parameter of cam
|
||||
//******************************************************************/
|
||||
cv::Mat undist = alreadyUndistorted ? cv::Mat::zeros(1,5,CV_32F) : cam.distCoeff;
|
||||
|
||||
myProjectPoints(vertex3D,projMat,cam.matK,undist,imgRefPts);
|
||||
|
||||
//******************************************************************/
|
||||
// draw the line of the x-axis and put "X" at the end
|
||||
//******************************************************************/
|
||||
line(rgbimage, imgRefPts[0], imgRefPts[1], cv::Scalar{255,0,0}, thickness);
|
||||
putText(rgbimage, "X", imgRefPts[1], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar{255,0,0}, thickness);
|
||||
|
||||
//******************************************************************/
|
||||
// draw the line of the y-axis and put "Y" at the end
|
||||
//******************************************************************/
|
||||
line(rgbimage, imgRefPts[0], imgRefPts[2], cv::Scalar{0,255,0}, thickness);
|
||||
putText(rgbimage, "Y", imgRefPts[2], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar{0,255,0}, thickness);
|
||||
|
||||
//******************************************************************/
|
||||
// draw the line of the z-axis and put "Z" at the end
|
||||
//******************************************************************/
|
||||
line(rgbimage, imgRefPts[0], imgRefPts[3], cv::Scalar{0,0,255}, thickness);
|
||||
putText(rgbimage, "Z", imgRefPts[3], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar{0,0,255}, thickness);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Wrapper around the original opencv's projectPoints
|
||||
*
|
||||
* @param objectPoints the 3D points
|
||||
* @param poseMat the pose matrix
|
||||
* @param cameraMatrix the calibration matrix
|
||||
* @param distCoeffs the distortion coeffi
|
||||
* @param imagePoints
|
||||
*/
|
||||
void myProjectPoints( cv::InputArray objectPoints, const cv::Mat &poseMat, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::OutputArray imagePoints)
|
||||
{
|
||||
cv::Mat rvec;
|
||||
Rodrigues( poseMat.colRange(0,3), rvec );
|
||||
// projectPoints( Mat( vertex3D.t( ) ).reshape( 3, 1 ), rvec, Tvec, K, dist, imgRefPts );
|
||||
projectPoints( objectPoints, rvec, poseMat.col(3), cameraMatrix, distCoeffs, imagePoints );
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Generate the set of 3D points of a chessboard
|
||||
*
|
||||
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
||||
* @param[in] squareSize the size in mm of the each square of the chessboard
|
||||
* @param[out] corners the set of 2D points on the chessboard
|
||||
*/
|
||||
void calcChessboardCorners( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point2f>& corners )
|
||||
{
|
||||
corners.resize(0);
|
||||
corners.reserve( boardSize.height* boardSize.width );
|
||||
|
||||
for( int i = 0; i < boardSize.height; i++ )
|
||||
for( int j = 0; j < boardSize.width; j++ )
|
||||
{
|
||||
/******************************************************************/
|
||||
// create a Point2f(x,y) according to the position j,i and a square
|
||||
// size of squareSize. Add it to corners (using push_back...)
|
||||
/******************************************************************/
|
||||
corners.push_back(cv::Point2f{squareSize*j,squareSize*i});
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Decompose the homography into its components R and t
|
||||
*
|
||||
* @param[in] H The homography H = [r1 r2 t]
|
||||
* @param[in] matK The 3x3 calibration matrix K
|
||||
* @param[out] poseMat the 3x4 pose matrix [R t]
|
||||
*/
|
||||
void decomposeHomography( const cv::Mat &H, const cv::Mat& matK, cv::Mat& poseMat )
|
||||
{
|
||||
|
||||
cv::Mat temp;
|
||||
|
||||
// H = [h1, h2, h3] = lambda * K [r1,r2,t]
|
||||
|
||||
//******************************************************************/
|
||||
//temp contains inv(K)*H
|
||||
//******************************************************************/
|
||||
temp = matK.inv()*H;
|
||||
|
||||
cv::Mat r1, r2, r3, t;
|
||||
|
||||
//******************************************************************/
|
||||
// get r1 and r2 from temp
|
||||
//******************************************************************/
|
||||
r1 = temp.col(0);
|
||||
r2 = temp.col(1);
|
||||
|
||||
//******************************************************************/
|
||||
// compute lambda
|
||||
//******************************************************************/
|
||||
double lambda = 1 / norm(r1);
|
||||
|
||||
//******************************************************************/
|
||||
// normalize r1 and r2
|
||||
//******************************************************************/
|
||||
r1 *= lambda;
|
||||
r2 *= lambda;
|
||||
|
||||
//******************************************************************/
|
||||
// compute r3
|
||||
//******************************************************************/
|
||||
r3 = r1.cross(r2);
|
||||
|
||||
//******************************************************************/
|
||||
// compute t
|
||||
//******************************************************************/
|
||||
t = temp.col(2) * lambda;
|
||||
|
||||
//******************************************************************/
|
||||
// create a 3x4 matrix (float) for poseMat
|
||||
//******************************************************************/
|
||||
poseMat = cv::Mat{3,4, CV_32F};
|
||||
|
||||
//******************************************************************/
|
||||
// fill the columns of poseMat with r1 r2 r3 and t
|
||||
//******************************************************************/
|
||||
r1.copyTo(poseMat.col(0));
|
||||
r2.copyTo(poseMat.col(1));
|
||||
r3.copyTo(poseMat.col(2));
|
||||
t.copyTo(poseMat.col(3));
|
||||
|
||||
// std::cout << "----------------------------" << std::endl;
|
||||
// std::cout << "lambda = " << lambda << std::endl;
|
||||
// std::cout << "----------------------------" << std::endl;
|
||||
// std::cout << r1 << std::endl;
|
||||
// std::cout << "----------------------------" << std::endl;
|
||||
std::cout << poseMat << std::endl;
|
||||
// std::cout << "----------------------------" << std::endl;
|
||||
|
||||
}
|
||||
|
||||
|
||||
/******************************************************************************/
|
||||
//KLT ONLY
|
||||
|
||||
/**
|
||||
* Generate the set of 3D points of a chessboard
|
||||
*
|
||||
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
||||
* @param[in] squareSize the size in mm of the each square of the chessboard
|
||||
* @param[out] corners the set of 3D points on the chessboard
|
||||
*/
|
||||
void calcChessboardCorners3D( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point3f>& corners )
|
||||
{
|
||||
corners.resize(0);
|
||||
corners.reserve( boardSize.height* boardSize.width );
|
||||
for( int i = 0; i < boardSize.height; i++ )
|
||||
for( int j = 0; j < boardSize.width; j++ )
|
||||
{
|
||||
/******************************************************************/
|
||||
// create a Point3f(x,y,0) according to the position j,i and a square
|
||||
// size of squareSize. Add it to corners (using push_back...)
|
||||
/******************************************************************/
|
||||
corners.push_back(cv::Point3f{squareSize*j, squareSize*i, 0});
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Wrapper around the original opencv's solvePnPRansac
|
||||
*
|
||||
* @param[in] objectPoints the 3D points
|
||||
* @param[in] imagePoints the image points
|
||||
* @param[in] cameraMatrix the calibration matrix
|
||||
* @param[in] distCoeffs the distortion coefficients
|
||||
* @param[out] poseMat the pose matrix
|
||||
* @param[out] inliers the list of indices of the inliers points
|
||||
*/
|
||||
void mySolvePnPRansac(cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, cv::OutputArray inliers )
|
||||
{
|
||||
cv::Mat currR, currT;
|
||||
cv::solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs, currR, currT, false, 100, 4, 100, inliers );
|
||||
|
||||
poseMat = cv::Mat(3, 4, CV_32F );
|
||||
|
||||
cv::Mat Rot;
|
||||
Rodrigues( currR, Rot );
|
||||
#if CV_MINOR_VERSION < 4
|
||||
// apparently older versions does not support direct copy
|
||||
cv::Mat temp;
|
||||
Rot.convertTo( temp, CV_32F );
|
||||
cv::Mat a1 = poseMat.colRange(0,3);
|
||||
temp.copyTo( a1 );
|
||||
a1 = poseMat.col(3);
|
||||
currT.convertTo( temp, CV_32F );
|
||||
temp.copyTo( a1 );
|
||||
#else
|
||||
Rot.copyTo( poseMat.colRange(0,3) );
|
||||
currT.copyTo( poseMat.col(3) );
|
||||
#endif
|
||||
}
|
||||
|
|
@ -0,0 +1,112 @@
|
|||
#ifndef _UTILITY_HPP_
|
||||
#define _UTILITY_HPP_
|
||||
|
||||
#include "Camera.hpp"
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#define DEBUGGING 1
|
||||
|
||||
#if DEBUGGING
|
||||
#define PRINTVAR( a ) std::cout << #a << " = " << a << std::endl << std::endl;
|
||||
#else
|
||||
#define PRINTVAR( a )
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Detect a chessboard in a given image
|
||||
*
|
||||
* @param[in] rgbimage The rgb image to process
|
||||
* @param[out] pointbuf the set of 2D image corner detected on the chessboard
|
||||
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
||||
* @return true if the chessboard is detected inside the image, false otherwise
|
||||
*/
|
||||
bool detectChessboard( const cv::Mat &rgbimage, std::vector<cv::Point2f> &pointbuf, const cv::Size &boardSize);
|
||||
|
||||
/**
|
||||
* Decompose the homography into its components R and t
|
||||
*
|
||||
* @param[in] H The homography H = [r1 r2 t]
|
||||
* @param[in] matK The 3x3 calibration matrix K
|
||||
* @param[out] poseMat the 3x4 pose matrix [R t]
|
||||
*/
|
||||
void decomposeHomography( const cv::Mat &H, const cv::Mat& matK, cv::Mat& poseMat );
|
||||
|
||||
/**
|
||||
*
|
||||
* @param[in,out] rgbimage The image on which to draw the reference system
|
||||
* @param[in] cam The camera
|
||||
* @param[in] projMat The projection matrix of the camera
|
||||
* @param[in] thickness The thickness of the line
|
||||
* @param[in] scale A scale factor for the unit vectors to draw
|
||||
* @param[in] alreadyUndistorted A boolean value that tells if the input image rgbimage is already undistorted or we are working on a distorted image
|
||||
*/
|
||||
void drawReferenceSystem( cv::Mat &rgbimage, const Camera& cam, const cv::Mat &projMat, const int &thickness, const double &scale, const bool alreadyUndistorted = true );
|
||||
|
||||
/**
|
||||
* Wrapper around the original opencv's projectPoints
|
||||
*
|
||||
* @param[in] objectPoints the 3D points
|
||||
* @param[in] poseMat the pose matrix
|
||||
* @param[in] cameraMatrix the calibration matrix
|
||||
* @param[in] distCoeffs the distortion coefficients
|
||||
* @param[out] imagePoints the projected points
|
||||
*/
|
||||
void myProjectPoints( cv::InputArray objectPoints, const cv::Mat &poseMat, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::OutputArray imagePoints);
|
||||
|
||||
/**
|
||||
* Wrapper around the original opencv's solvePnPRansac
|
||||
*
|
||||
* @param[in] objectPoints the 3D points
|
||||
* @param[in] imagePoints the image points
|
||||
* @param[in] cameraMatrix the calibration matrix
|
||||
* @param[in] distCoeffs the distortion coefficients
|
||||
* @param[out] poseMat the pose matrix
|
||||
* @param[out] inliers the list of indices of the inliers points
|
||||
*/
|
||||
void mySolvePnPRansac(cv::InputArray objectPoints, cv::InputArray imagePoints, cv::InputArray cameraMatrix, cv::InputArray distCoeffs, cv::Mat &poseMat, cv::OutputArray inliers=cv::noArray());
|
||||
|
||||
/**
|
||||
* Generate the set of 3D points of a chessboard
|
||||
*
|
||||
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
||||
* @param[in] squareSize the size in mm of the each square of the chessboard
|
||||
* @param[out] corners the set of 2D points on the chessboard
|
||||
*/
|
||||
void calcChessboardCorners( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point2f>& corners);
|
||||
|
||||
|
||||
/**
|
||||
* Generate the set of 3D points of a chessboard
|
||||
*
|
||||
* @param[in] boardSize the size of the board in terms of corners (width X height)
|
||||
* @param[in] squareSize the size in mm of the each square of the chessboard
|
||||
* @param[out] corners the set of 3D points on the chessboard
|
||||
*/
|
||||
void calcChessboardCorners3D( const cv::Size &boardSize, const float &squareSize, std::vector<cv::Point3f>& corners );
|
||||
|
||||
/**
|
||||
* Filter a generic vector against a list of index of the elements to be deleted,
|
||||
*
|
||||
* @param[in,out] inout the vector to filter
|
||||
* @param[in] idx list of indices of the element to remove
|
||||
*/
|
||||
template<typename T>
|
||||
void filterVector( std::vector<T> &inout, const std::vector<int> &idx )
|
||||
{
|
||||
std::vector<T> temp;
|
||||
temp.reserve( idx.size() );
|
||||
|
||||
for( size_t i = 0; i < idx.size(); ++i )
|
||||
{
|
||||
assert( idx[i] < inout.size() );
|
||||
temp.push_back( inout[ idx[i] ] );
|
||||
}
|
||||
|
||||
inout.clear(); //necessary??
|
||||
inout = temp;
|
||||
}
|
||||
|
||||
#endif /*_UTILITY_HPP_*/
|
||||
Loading…
Reference in New Issue