void Regression::write(cv::InputArray array)
{
write() << "kind" << array.kind();
write() << "type" << array.type();
if (isVector(array))
{
int total = (int)array.total();
int idx = regRNG.uniform(0, total);
write() << "len" << total;
write() << "idx" << idx;
cv::Mat m = array.getMat(idx);
if (m.total() * m.channels() < 26) //5x5 or smaller
write() << "val" << m;
else
write(m);
}
else
{
if (array.total() * array.channels() < 26) //5x5 or smaller
write() << "val" << array.getMat();
else
write(array.getMat());
}
}
void unprojectPointsFisheye( cv::InputArray distorted, cv::OutputArray undistorted, cv::InputArray K, cv::InputArray D, cv::InputArray R, cv::InputArray P)
{
// will support only 2-channel data now for points
CV_Assert(distorted.type() == CV_32FC2 || distorted.type() == CV_64FC2);
undistorted.create(distorted.size(), CV_MAKETYPE(distorted.depth(), 3));
CV_Assert(P.empty() || P.size() == cv::Size(3, 3) || P.size() == cv::Size(4, 3));
CV_Assert(R.empty() || R.size() == cv::Size(3, 3) || R.total() * R.channels() == 3);
CV_Assert(D.total() == 4 && K.size() == cv::Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F));
cv::Vec2d f, c;
if (K.depth() == CV_32F)
{
cv::Matx33f camMat = K.getMat();
f = cv::Vec2f(camMat(0, 0), camMat(1, 1));
c = cv::Vec2f(camMat(0, 2), camMat(1, 2));
}
else
{
cv::Matx33d camMat = K.getMat();
f = cv::Vec2d(camMat(0, 0), camMat(1, 1));
c = cv::Vec2d(camMat(0, 2), camMat(1, 2));
}
cv::Vec4d k = D.depth() == CV_32F ? (cv::Vec4d)*D.getMat().ptr<cv::Vec4f>(): *D.getMat().ptr<cv::Vec4d>();
cv::Matx33d RR = cv::Matx33d::eye();
if (!R.empty() && R.total() * R.channels() == 3)
{
cv::Vec3d rvec;
R.getMat().convertTo(rvec, CV_64F);
RR = cv::Affine3d(rvec).rotation();
}
else if (!R.empty() && R.size() == cv::Size(3, 3))
R.getMat().convertTo(RR, CV_64F);
if(!P.empty())
{
cv::Matx33d PP;
P.getMat().colRange(0, 3).convertTo(PP, CV_64F);
RR = PP * RR;
}
// start undistorting
const cv::Vec2f* srcf = distorted.getMat().ptr<cv::Vec2f>();
const cv::Vec2d* srcd = distorted.getMat().ptr<cv::Vec2d>();
cv::Vec3f* dstf = undistorted.getMat().ptr<cv::Vec3f>();
cv::Vec3d* dstd = undistorted.getMat().ptr<cv::Vec3d>();
size_t n = distorted.total();
int sdepth = distorted.depth();
for(size_t i = 0; i < n; i++ )
{
cv::Vec2d pi = sdepth == CV_32F ? (cv::Vec2d)srcf[i] : srcd[i]; // image point
cv::Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point
double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]);
double theta = theta_d;
if (theta_d > 1e-8)
{
// compensate distortion iteratively
for(int j = 0; j < 10; j++ )
{
double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2;
theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8);
}
}
double z = std::cos(theta);
double r = std::sin(theta);
cv::Vec3d pu = cv::Vec3d(r*pw[0], r*pw[1], z); //undistorted point
// reproject
cv::Vec3d pr = RR * pu; // rotated point optionally multiplied by new camera matrix
cv::Vec3d fi; // final
normalize(pr, fi);
if( sdepth == CV_32F )
dstf[i] = fi;
else
dstd[i] = fi;
}
}
void Regression::verify(cv::FileNode node, cv::InputArray array, double eps, ERROR_TYPE err)
{
int expected_kind = (int)node["kind"];
int expected_type = (int)node["type"];
ASSERT_EQ(expected_kind, array.kind()) << " Argument \"" << node.name() << "\" has unexpected kind";
ASSERT_EQ(expected_type, array.type()) << " Argument \"" << node.name() << "\" has unexpected type";
cv::FileNode valnode = node["val"];
if (isVector(array))
{
int expected_length = (int)node["len"];
ASSERT_EQ(expected_length, (int)array.total()) << " Vector \"" << node.name() << "\" has unexpected length";
int idx = node["idx"];
cv::Mat actual = array.getMat(idx);
if (valnode.isNone())
{
ASSERT_LE((size_t)26, actual.total() * (size_t)actual.channels())
<< " \"" << node.name() << "[" << idx << "]\" has unexpected number of elements";
verify(node, actual, eps, cv::format("%s[%d]", node.name().c_str(), idx), err);
}
else
{
cv::Mat expected;
valnode >> expected;
if(expected.empty())
{
ASSERT_TRUE(actual.empty())
<< " expected empty " << node.name() << "[" << idx<< "]";
}
else
{
ASSERT_EQ(expected.size(), actual.size())
<< " " << node.name() << "[" << idx<< "] has unexpected size";
cv::Mat diff;
cv::absdiff(expected, actual, diff);
if (err == ERROR_ABSOLUTE)
{
if (!cv::checkRange(diff, true, 0, 0, eps))
{
if(expected.total() * expected.channels() < 12)
std::cout << " Expected: " << std::endl << expected << std::endl << " Actual:" << std::endl << actual << std::endl;
double max;
cv::minMaxIdx(diff.reshape(1), 0, &max);
FAIL() << " Absolute difference (=" << max << ") between argument \""
<< node.name() << "[" << idx << "]\" and expected value is greater than " << eps;
}
}
else if (err == ERROR_RELATIVE)
{
double maxv, maxa;
int violations = countViolations(expected, actual, diff, eps, &maxv, &maxa);
if (violations > 0)
{
FAIL() << " Relative difference (" << maxv << " of " << maxa << " allowed) between argument \""
<< node.name() << "[" << idx << "]\" and expected value is greater than " << eps << " in " << violations << " points";
}
}
}
}
}
else
{
if (valnode.isNone())