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); };