// Camera triangulation using the iterated linear method Vec3 Triangulation::compute(int iter) const { const int nviews = int(views.size()); assert(nviews>=2); // Iterative weighted linear least squares Mat3 AtA; Vec3 Atb, X; Vec weights = Vec::Constant(nviews,1.0); for (int it=0;it<iter;++it) { AtA.fill(0.0); Atb.fill(0.0); for (int i=0;i<nviews;++i) { const Mat34& PMat = views[i].first; const Vec2 & p = views[i].second; const double w = weights[i]; Vec3 v1, v2; for (int j=0;j<3;j++) { v1[j] = w * ( PMat(0,j) - p(0) * PMat(2,j) ); v2[j] = w * ( PMat(1,j) - p(1) * PMat(2,j) ); Atb[j] += w * ( v1[j] * ( p(0) * PMat(2,3) - PMat(0,3) ) + v2[j] * ( p(1) * PMat(2,3) - PMat(1,3) ) ); } for (int k=0;k<3;k++) { for (int j=0;j<=k;j++) { const double v = v1[j] * v1[k] + v2[j] * v2[k]; AtA(j,k) += v; if (j<k) AtA(k,j) += v; } } } X = AtA.inverse() * Atb; // Compute reprojection error, min and max depth, and update weights zmin = std::numeric_limits<double>::max(); zmax = - std::numeric_limits<double>::max(); err = 0.0; for (int i=0;i<nviews;++i) { const Mat34& PMat = views[i].first; const Vec2 & p = views[i].second; const Vec3 xProj = PMat * X.homogeneous(); const double z = xProj(2); if (z < zmin) zmin = z; else if (z > zmax) zmax = z; err += (p - xProj.hnormalized()).norm(); // residual error weights[i] = 1.0 / z; } } return X; }
TEST(Image, Convolution_MeanBoxFilter) { Image<unsigned char> in(40,40); in.block(10,10,20,20).fill(255.f); Mat3 meanBoxFilterKernel; meanBoxFilterKernel.fill(1.f/9.f); Image<unsigned char> out; ImageConvolution(in, meanBoxFilterKernel, out); }