matf Triangulate(const matf & P1, const matf & P2, const TrackedPoint & p, bool full) {
matf A;
if (full)
A = matf(6, 4);
else
A = matf(4,4);
for (int i = 0; i < 4; ++i) {
A(0, i) = p.x1 * P1(2, i) - P1(0, i);
A(1, i) = p.y1 * P1(2, i) - P1(1, i);
A(2, i) = p.x2 * P2(2, i) - P2(0, i);
A(3, i) = p.y2 * P2(2, i) - P2(1, i);
// the two following equations could be removed in most of the cases.
// however, sometimes it induces errors if some (which ones?) coefficients are
// too close to 0
if (full) {
A(4, i) = p.x1 * P1(1, i) - p.y1 * P1(0, i);
A(5, i) = p.x2 * P2(1, i) - p.y2 * P2(0, i);
}
}
SVD svd(A, SVD::MODIFY_A);
matf p3d(3,1);
for (int i = 0; i < 3; ++i)
p3d(i,0) = svd.vt.at<float>(3,i) / svd.vt.at<float>(3,3);
return p3d;
}
void GetCameraMatricesFromFundMat(const matf & fundmat, matf & P1, matf & P2) {
P1 = matf(3,4, 0.0f);
P1(0,0) = P1(1,1) = P1(2,2) = 1.0f;
matf e1, e2;
GetEpipolesFromFundMat(fundmat, e1, e2);
matf M = epscov(e2) * fundmat;
P2 = matf(3,4);
for (int i = 0; i < 3; ++i)
copyCol(M, P2, i, i);
copyCol(e2, P2, 0, 3);
}
void GetEpipolesFromFundMat(const matf & fundmat, matf & e1, matf & e2) {
SVD svd(fundmat);
//e1 = matf(3,1);
if ((e1.size().height != 3) or (e1.size().width != 3))
e1 = matf(3, 1);
copyCol(svd.vt.t(), e1, 2, 0);
svd(fundmat.t());
//e2 = matf(3,1);
if ((e2.size().height != 3) or (e2.size().width != 3))
e2 = matf(3, 1);
copyCol(svd.vt.t(), e2, 2, 0);
}
void World::move(float xrel, float yrel, float zrel)
{
osg::Matrixf matf(osg::Matrixf::rotate(
0.0f, osg::Vec3f(0.0f, 0.0f, 1.0f),
mCameraRot.y()*3.14159f/180.0f, osg::Vec3f(0.0f, 1.0f, 0.0f),
-mCameraRot.x()*3.14159f/180.0f, osg::Vec3f(1.0f, 0.0f, 0.0f)
));
mCameraPos += matf*osg::Vec3f(xrel, yrel, zrel);
}
//HZ p394 Algo 16.1
//the H's are homographies to "denormalize" (unlike HZ)
void NormalizePoints2(const vector<TrackedPoint> & points2d,
vector<TrackedPoint> & points2d_out,
vector<matf> & H_out) {
int n = points2d.size();
matf means[2];
means[0] = matf(3,1,0.0f);
means[1] = matf(3,1,0.0f);
for (int iView = 0; iView < 2; ++iView) {
for (int i = 0; i < n; ++i)
means[iView] += points2d[i].getP(iView);
means[iView] = means[iView] / (float)n;
}
for (int i = 0; i < n; ++i)
points2d_out.push_back(TrackedPoint(points2d[i].x1 - means[0](0,0),
points2d[i].y1 - means[0](1,0),
points2d[i].x2 - means[1](0,0),
points2d[i].y2 - means[1](1,0)));
float meandist[2];
meandist[0] = meandist[1] = 0.0f;
float x, y;
for (int iView = 0; iView < 2; ++iView) {
for (int i = 0; i < n; ++i) {
x = points2d_out[i].getX(iView);
y = points2d_out[i].getY(iView);
meandist[iView] += sqrt(x*x + y*y);
}
meandist[iView] = meandist[iView] / ((float)n) * SQRT2;
//meandist[iView] = 1.0f;
}
for (int i = 0; i < n; ++i) {
points2d_out[i] = TrackedPoint(points2d_out[i].x1 / meandist[0],
points2d_out[i].y1 / meandist[0],
points2d_out[i].x2 / meandist[1],
points2d_out[i].y2 / meandist[1]);
}
H_out.clear();
for (int i = 0; i < 2; ++i) {
H_out.push_back(matf(3,3,0.0f));
H_out.back()(0,0) = H_out.back()(1,1) = meandist[i];
H_out.back()(0,2) = means[i](0,0);
H_out.back()(1,2) = means[i](1,0);
H_out.back()(2,2) = 1.0f;
}
}
void World::update(float timediff)
{
GuiIface::Mode guimode = GuiIface::get().getMode();
if(guimode <= GuiIface::Mode_Cursor)
{
Linker::get().update();
Mover::get().update(timediff);
Door::get().update(timediff);
}
Renderer::get().update();
osg::Matrixf matf(osg::Matrixf::rotate(
0.0f, osg::Vec3f(0.0f, 0.0f, 1.0f),
mCameraRot.y()*3.14159f/180.0f, osg::Vec3f(0.0f, 1.0f, 0.0f),
-mCameraRot.x()*3.14159f/180.0f, osg::Vec3f(1.0f, 0.0f, 0.0f)
));
matf.preMultTranslate(mCameraPos);
mViewer->getCamera()->setViewMatrix(matf);
if(guimode == GuiIface::Mode_Game)
mCurrentSelection = castCameraToViewportRay(0.5f, 0.5f, 1024.0f, false);
else
{
float x, y;
GuiIface::get().getMousePosition(x, y);
mCurrentSelection = castCameraToViewportRay(x, y, 1024.0f, false);
}
if(mCurrentSelection == InvalidHandle || !*g_introspect)
GuiIface::get().updateStatus(std::string());
else
{
std::stringstream sstr;
if(!mExterior.empty())
{
MBlockHeader *block = mExterior.at(mCurrentSelection>>24).get();
const MObjectBase *obj = block->getObject(mCurrentSelection);
if(!obj)
sstr<< "Failed to lookup object 0x"<<std::hex<<std::setfill('0')<<std::setw(8)<<mCurrentSelection;
else
{
sstr<<std::setfill('0');
obj->print(sstr);
}
}
else
{
matf GetExtrinsicsFromEssential(const matf & essMat_, const TrackedPoint & one_point,
bool correct_essMat, int c) {
matf essMat;
if (correct_essMat) {
SVD svd2(essMat_);
matf D = matf(3,3,0.0f);
D(0,0) = D(1,1) = (svd2.w.at<float>(0,0) + svd2.w.at<float>(1,0)) * 0.5f;
essMat = svd2.u * D * svd2.vt;
} else {
matf essMat = essMat_;
}
SVD svd(essMat);
//assert(epsEqual(D(0,0) / D(1,0), 1.0, 0.1) && epsEqual(D(2,0), 0.0f));
matf W(3,3,0.0f); //TODO do not recreate at each call
W(0,1) = -1.0f;
W(1,0) = W(2,2) = 1.0f;
matf extr(3,4);
matf tmp;
//case 1
tmp = svd.u*W*svd.vt;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
extr(i,j) = tmp(i,j);
for (int i = 0; i < 3; ++i)
extr(i, 3) = svd.u.at<float>(i, 2);
//cout << extr << endl;
if ((c == 1) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 1" << endl;
return extr;
}
//case 2
for (int i = 0; i < 3; ++i)
extr(i, 3) = -svd.u.at<float>(i, 2);
//cout << extr << endl;
if ((c == 2) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 2" << endl;
return extr;
}
//case 3
tmp = svd.u*W.t()*svd.vt;
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
extr(i,j) = tmp(i,j);
//cout << extr << endl;
if ((c == 3) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 3" << endl;
return extr;
}
//case 4
for (int i = 0; i < 3; ++i)
extr(i, 3) = svd.u.at<float>(i, 2);
//cout << extr << endl;
if ((c == 4) || ((c == -1) && IsExtrinsicsPossible(extr, one_point))) {
//cout << "case 4" << endl;
return extr;
}
//assert(false); //this should not happen. If it does, sth is wrong
// (the point might be the on the principal plane, or there is a bug)
return matf(0,0); // remove warning
}
TEST(FloatPrecision, Snapshots) {
// Initialise the data path with en empty modulename,
// to get the data from the root of the test data path
cvtest::TS::ptr()->init("");
chilitags::Chilitags3Df chilitagsf;
chilitagsf.getChilitags().setPerformance(chilitags::Chilitags::ROBUST);
chilitagsf.enableFilter(false);
chilitags::Chilitags3Dd chilitagsd;
chilitagsd.getChilitags().setPerformance(chilitags::Chilitags::ROBUST);
chilitagsd.enableFilter(false);
for (auto testCase : TestMetadata::all) {
std::string path = std::string(cvtest::TS::ptr()->get_data_path())+testCase.filename;
cv::Mat image = cv::imread(path);
if(image.data) {
auto tagsd = chilitagsd.estimate(image, chilitags::Chilitags::DETECT_ONLY);
auto tagsf = chilitagsf.estimate(image, chilitags::Chilitags::DETECT_ONLY);
ASSERT_EQ(tagsf.size(), tagsd.size()) <<
"Single precision found " << tagsf.size() <<
" tags while double precision found " << tagsd.size() << " tags in " << testCase.filename;
double diff;
for(const auto& tag : tagsf) {
const auto& matf = tag.second;
const auto& matd = tagsd[tag.first];
//Test rotation component
double failThreshold = 1e-5; //TODO: What does this threshold actually mean?
for(int i=0; i<3; i++)
for(int j=0; j<3; j++) {
diff = (double)matf(i,j) - matd(i,j);
if(!std::isnan(diff))
ASSERT_LT(diff, failThreshold) <<
tag.first << "'s single and double precision transforms differ more than " << failThreshold << "%% in ("
<< i << "," << j << ");" << std::endl << " Single precision transform: " << matf << std::endl
<< "Double precision transform:" << matd;
}
//Test translation component
failThreshold = 1e-1; //Tenth of a millimeter
for(int i=0; i<3; i++) {
diff = (double)matf(i,3) - matd(i,3);
if(!std::isnan(diff))
ASSERT_LT(diff, failThreshold) <<
tag.first << "'s single and double precision transforms differ more than " << failThreshold << "%% in ("
<< i << "," << "3);" << std::endl << " Single precision transform: " << matf << std::endl
<< "Double precision transform:" << matd;
}
//Test last row
ASSERT_EQ(matf(3,0), 0.0f); ASSERT_EQ(matf(3,1), 0.0f); ASSERT_EQ(matf(3,2), 0.0f); ASSERT_EQ(matf(3,3), 1.0f);
ASSERT_EQ(matd(3,0), 0.0); ASSERT_EQ(matd(3,1), 0.0); ASSERT_EQ(matd(3,2), 0.0); ASSERT_EQ(matd(3,3), 1.0);
}
}
else {
ADD_FAILURE()
<< "Unable to read: " << path << "\n"
<< "Did you correctly set the OPENCV_TEST_DATA_PATH environment variable?\n"
<< "CMake takes care of this if you set its TEST_DATA variable.\n"
<< "You can download the test data from\n"
<< "https://github.com/chili-epfl/chilitags-testdata";
}
}
}
inline const matf getP(int i) const {
return matf(3, 1, const_cast<float*>((&x1)+3*i));
}
inline matf getP(int i) {
return matf(3, 1, ((&x1)+3*i));
}
inline matf getP2() {
return matf(3, 1, &x2);
};
inline matf getP1() {
//ugly hack that could induce segfaults if the object
// is destroyed before the returned matf
return matf(3, 1, &x1);
};
