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())