void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedInputPoints,
cv::OutputArray _Ma, cv::OutputArray _Mb)
{
//argument checking:
size_t n = _objectPoints.rows() * _objectPoints.cols(); //number of points
int objType = _objectPoints.type();
int type_input = _normalizedInputPoints.type();
assert((objType == CV_32FC3) | (objType == CV_64FC3));
assert((type_input == CV_32FC2) | (type_input == CV_64FC2));
assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1));
assert((_objectPoints.rows() >= 4) | (_objectPoints.cols() >= 4));
assert((_normalizedInputPoints.rows() == 1) | (_normalizedInputPoints.cols() == 1));
assert(static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols()) == n);
cv::Mat normalizedInputPoints;
if (type_input == CV_32FC2) {
_normalizedInputPoints.getMat().convertTo(normalizedInputPoints, CV_64FC2);
}
else {
normalizedInputPoints = _normalizedInputPoints.getMat();
}
cv::Mat objectInputPoints;
if (type_input == CV_32FC3) {
_objectPoints.getMat().convertTo(objectInputPoints, CV_64FC3);
}
else {
objectInputPoints = _objectPoints.getMat();
}
cv::Mat canonicalObjPoints;
cv::Mat MmodelPoints2Canonical;
//transform object points to the canonical position (zero centred and on the plane z=0):
makeCanonicalObjectPoints(objectInputPoints, canonicalObjPoints, MmodelPoints2Canonical);
//compute the homography mapping the model's points to normalizedInputPoints
cv::Mat H;
HomographyHO::homographyHO(canonicalObjPoints, _normalizedInputPoints, H);
//now solve
cv::Mat MaCanon, MbCanon;
solveCanonicalForm(canonicalObjPoints, normalizedInputPoints, H, MaCanon, MbCanon);
//transform computed poses to account for canonical transform:
cv::Mat Ma = MaCanon * MmodelPoints2Canonical;
cv::Mat Mb = MbCanon * MmodelPoints2Canonical;
//output poses:
Ma.copyTo(_Ma);
Mb.copyTo(_Mb);
}
FeatureValue FeatureShiCorner::calculate( cv::InputArray image )
{
Mat image_gray;
cvtColor( image, image_gray, CV_BGR2GRAY );
Mat corner( image.rows(), image.cols(), CV_32SC1, Scalar( 0 ) );
computeRawCornerMat( image_gray, corner );
auto points = genPoints( corner );
// return genDescriptor;
return FeatureValue();
}
inline static void to_nv12(cv::InputArray bgr, cv::Mat & Y, cv::Mat & UV)
{
const int height = bgr.rows();
const int width = bgr.cols();
Mat yuv;
cvtColor(bgr, yuv, CV_BGR2YUV_I420);
CV_Assert(yuv.isContinuous());
Mat Y_(Y, Rect(0, 0, width, height));
yuv.rowRange(0, height).copyTo(Y_);
Mat UV_planar(height, width / 2, CV_8UC1, yuv.ptr(height));
Mat u_and_v[2] = {
UV_planar.rowRange(0, height / 2),
UV_planar.rowRange(height / 2, height),
};
Mat uv;
cv::merge(u_and_v, 2, uv);
Mat UV_(UV, Rect(0, 0, width, height / 2));
uv.reshape(1).copyTo(UV_);
}
void IPPE::PoseSolver::evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _M, float& err)
{
cv::Mat projectedPoints;
cv::Mat imagePoints = _imagePoints.getMat();
cv::Mat r;
rot2vec(_M.getMat().colRange(0, 3).rowRange(0, 3), r);
if (_cameraMatrix.empty()) {
//there is no camera matrix and image points are in normalized pixel coordinates
cv::Mat K(3, 3, CV_64FC1);
K.setTo(0);
K.at<double>(0, 0) = 1;
K.at<double>(1, 1) = 1;
K.at<double>(2, 2) = 1;
cv::Mat kc;
cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), K, kc, projectedPoints);
}
else {
cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), _cameraMatrix, _distCoeffs, projectedPoints);
}
err = 0;
size_t n = _objectPoints.rows() * _objectPoints.cols();
float dx, dy;
for (size_t i = 0; i < n; i++) {
if (projectedPoints.depth() == CV_32FC1) {
dx = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0];
dy = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0];
}
else {
dx = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0];
dy = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0];
}
err += dx * dx + dy * dy;
}
err = sqrt(err / (2.0f * n));
}
// *
FeatureValue FeatureAverageColor::calculate( cv::InputArray _image )
{
Mat image_gray;
Mat shiImg( _image.rows(), _image.cols(), CV_32SC1, Scalar( 0 ) );
cvtColor( _image, image_gray, CV_BGR2GRAY );
FeatureShiCorner shi;
shi.computeRawCornerMat( image_gray, shiImg );
auto points = shi.genPoints( shiImg );
if( points.empty() )
{
return {
0, 0, 0
};
}
// ---------------------------------------------------------------------
auto image = _image.getMat();
auto ret = FeatureValue();
uint64 r, g, b, n;
const int radius = 15;
r = g = b = 0;
n = 0;
const auto width = _image.cols();
const auto height = _image.rows();
for( auto p : points )
{
int x = p.first, y = p.second;
for( int dy = -radius; dy <= radius; dy++ )
{
for( int dx = -radius; dx <= radius; dx++ )
{
int fx = x + dx;
if( (fx < 0) || (fx >= width) ) { continue; }
int fy = y + dy;
if( (fy < 0) || (fy >= height) ) { continue; }
Pixel ptr = image.at< Pixel >( fy, fx );
b += ptr.x;
g += ptr.y;
r += ptr.z;
n++;
}
}
}
ret.push_back( r / (n * 1.0) );
ret.push_back( g / (n * 1.0) );
ret.push_back( b / (n * 1.0) );
return ret;
}
