Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) {
// number of cameras
size_t m = projection_matrices.size();
// Allocate DLT matrix
Matrix A = Matrix::Zero(m * 2, 4);
for (size_t i = 0; i < m; i++) {
size_t row = i * 2;
const Matrix34& projection = projection_matrices.at(i);
const Point2& p = measurements.at(i);
// build system of equations
A.row(row) = p.x() * projection.row(2) - projection.row(0);
A.row(row + 1) = p.y() * projection.row(2) - projection.row(1);
}
int rank;
double error;
Vector v;
boost::tie(rank, error, v) = DLT(A, rank_tol);
if (rank < 3)
throw(TriangulationUnderconstrainedException());
return v;
}
void MMVRDump(IntX level, LongN start)
{
IntX i;
DL(1, (OUTPUTBUFF, "### [MMVR] (%08lx)\n", start));
DLV(2, "Version =", MMVR->Version);
DLx(2, "Flags =", MMVR->Flags);
DLu(2, "AxisCount=", MMVR->AxisCount);
for (i = 0; i < MMVR->AxisCount; i++)
{
Axis *axis = &MMVR->axis[i];
DL(2, (OUTPUTBUFF, "--- axis[%d]\n", i));
DLT(2, "Tag =", axis->Tag);
DLu(2, "Default=", axis->Default);
DLu(2, "Scale =", axis->Scale);
}
}
