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