void MarkerRenderer::_render( const Marker& marker )
{
if ( !_texture.isValid() && !_generateTexture( ))
return;
const QPointF pos = marker.getPosition();
glPushAttrib( GL_ENABLE_BIT | GL_TEXTURE_BIT );
glDisable( GL_DEPTH_TEST );
glEnable( GL_BLEND );
glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
glPushMatrix();
glTranslatef( pos.x(), pos.y(), 0.f );
glScalef( MARKER_SIZE_PIXELS, MARKER_SIZE_PIXELS, 1.f );
glTranslatef( -0.5f, -0.5f, 0.f ); // Center unit quad
_quad.setTexture( _texture.getTextureId( ));
_quad.render();
glPopMatrix();
glPopAttrib();
}
float Manager::getEstimatedCameraPose(cv::Size image_size,
cv::Mat& camera_matrix, cv::Mat& rvec,
cv::Mat& tvec, float force_fov, ofVec2f lens_offset_pix)
{
if (markers.size() <= 6) return -1;
markUpdated();
using namespace ofxCv;
using namespace cv;
vector<cv::Point3f> object_points;
vector<cv::Point2f> image_points;
for (int i = 0; i < markers.size(); i++)
{
Marker* o = markers[i].get();
object_points.push_back(toCv(o->object_pos));
image_points.push_back(toCv((ofVec2f)o->getPosition()));
}
cv::Mat dist_coeffs = cv::Mat::zeros(8, 1, CV_64F);
float fov = 60;
if (force_fov != 0) fov = force_fov;
float f = (image_size.height / 2) * tan(ofDegToRad(fov / 2.0));
camera_matrix = (cv::Mat_<double>(3, 3) << f, 0, image_size.width / 2 + lens_offset_pix.x, 0, f,
image_size.height / 2 + lens_offset_pix.y, 0, 0, 1);
float rms = 0;
if (force_fov == 0)
{
vector<cv::Mat> rvecs;
vector<cv::Mat> tvecs;
vector<vector<cv::Point3f> > object_points_arr(1);
vector<vector<cv::Point2f> > image_points_arr(1);
object_points_arr[0] = object_points;
image_points_arr[0] = image_points;
int flags = 0;
flags |= CV_CALIB_USE_INTRINSIC_GUESS;
flags |= CV_CALIB_FIX_ASPECT_RATIO;
flags |= CV_CALIB_ZERO_TANGENT_DIST;
flags |= (CV_CALIB_FIX_K1 | CV_CALIB_FIX_K2 | CV_CALIB_FIX_K3 |
CV_CALIB_FIX_K4 | CV_CALIB_FIX_K5 | CV_CALIB_FIX_K6 |
CV_CALIB_RATIONAL_MODEL);
if (lens_offset_pix.lengthSquared() > FLT_EPSILON) {
flags |= CV_CALIB_FIX_PRINCIPAL_POINT;
}
rms = cv::calibrateCamera(object_points_arr, image_points_arr, image_size,
camera_matrix, dist_coeffs, rvecs, tvecs,
flags);
rvec = rvecs[0];
tvec = tvecs[0];
}
else
{
cv::Mat rvecs;
cv::Mat tvecs;
cv::solvePnP(object_points, image_points, camera_matrix, dist_coeffs, rvecs, tvecs);
rms = 1;
rvec = rvecs;
tvec = tvecs;
}
return rms;
}
