TriangulationMidPoint.cpp

#include "TriangulationMidPoint.hh"
#include <Base/Math/Matrix.hh>
#include <MathAlgo/LeastSquares.hh>
#include <MathAlgo/SVD.hh>

using namespace BIAS;
using namespace std;


TriangulationMidPoint::TriangulationMidPoint() : BIAS::Debug()
{
}


TriangulationMidPoint::
~TriangulationMidPoint()
{
}


int TriangulationMidPoint::
Intersect_(const std::vector< BIAS::Vector3<double> >& pos,
           const std::vector< BIAS::Vector3<double> >& dir,
           BIAS::Vector3<double>& res, double& dist,
           const bool computeDist)
{ 
  const unsigned int num = pos.size();
  BIASASSERT(dir.size() == pos.size());

/*
  // Solve least squares problem directly via SVD
  BIAS::Matrix<double> ATA(num+3, num+3, BIAS::MatrixIdentity);
  BIAS::Matrix<double> AT(num+3, 3*num, BIAS::MatrixZero);
  BIAS::Vector<double> b(3*num);
  for (unsigned int k = 0; k < 3; k++)
    ATA[k][k] = num;
  for (unsigned int i = 0; i < num; i++) {
    for (unsigned int k = 0; k < 3; k++) {
      ATA[k][i+3] = -dir[i][k];
      ATA[i+3][k] = -dir[i][k];
    }
  }
  unsigned int j = 0;
  for (unsigned int i = 0; i < num; i++) {
    for (unsigned int k = 0; k < 3; k++, j++) {
      AT[k][j] = 1.0;
      AT[i+3][j] = -dir[i][k];
      b[j] = pos[i][k];
    }
  }
  BIAS::SVD solver;
  if (solver.Compute(ATA) != 0) return -1;
  BIAS::Matrix<double> ATAinv = solver.Invert();
  BIAS::Vector<double> x = ATAinv * (AT * b);
 */

  // Solve linear least squares problem
  BIAS::Matrix<double> A(3*num, num+3, BIAS::MatrixZero);
  BIAS::Vector<double> b(3*num), x(num+3);
  unsigned int j = 0;
  for (unsigned int i = 0; i < num; i++) {
    for (unsigned int k = 0; k < 3; k++, j++) {
      A[j][k] = 1.0;
      A[j][i+3] = -dir[i][k];
      b[j] = pos[i][k];
    }
  }
  BIAS::LeastSquaresLapack solver;
  if (solver.Solve(A, b, x) != 0) return -1;

  // Check if resulting 3d point is valid
  for (unsigned int k = 0; k < 3; k++) {
    res[k] = x[k];
    if (BIAS_ISNAN(x[k])) return -1;
  }

  // Compute average distance
  if (computeDist) {
    dist = 0.0;
    BIAS::Vector3<double> d;
    for (unsigned int i = 0; i < num; i++) {
      d = pos[i] + x[i+3]*dir[i] - res;
      dist += d.NormL2();
    }
    dist /= (double)num;
  }

  // Check if result is in front of all cameras
  for (unsigned int i = 0; i < num; i++) {
    if (x[i+3] < 0) return i+1;
  }
  return 0;
}


int TriangulationMidPoint::
Triangulate_(const std::vector< BIAS::ProjectionParametersBase* >& cams,
             const std::vector< BIAS::HomgPoint2D >& points2d,
             BIAS::Vector3<double>& point3d, double& dist,
             const bool computeDist)
{
  const unsigned int num = cams.size();
  BIASASSERT(points2d.size() == num);
  std::vector< BIAS::Vector3<double> > pos;
  std::vector< BIAS::Vector3<double> > dir;
  pos.reserve(num);
  dir.reserve(num);

  BIAS::Vector3<double> d, p;
  for (unsigned int i = 0; i < num; i++) {
    BIASASSERT(points2d[i].IsHomogenized());
    cams[i]->UnProjectToRay(points2d[i], p, d);
#ifdef BIAS_DEBUG
    BIASASSERT(BIAS::Equal(d.NormL2(), 1.0));
#endif
    dir.push_back(d);
    pos.push_back(p);
  }
  return Intersect_(pos, dir, point3d, dist, computeDist);
}


int TriangulationMidPoint::
Triangulate_(const std::vector< BIAS::PoseParametrization >& poses,
             const std::vector< BIAS::HomgPoint2D >& rays,
             BIAS::Vector3<double>& point3d, double& dist,
             const bool computeDist)
{
  const unsigned int num = poses.size();
  BIASASSERT(rays.size() == num);
  std::vector< BIAS::Vector3<double> > pos;
  std::vector< BIAS::Vector3<double> > dir;
  pos.reserve(num);
  dir.reserve(num);

  for (unsigned int i = 0; i < num; i++) {
    pos.push_back(poses[i].GetPosition());
    BIASASSERT(BIAS::Equal(rays[i].NormL2(), 1.0));
    dir.push_back(poses[i].GetOrientation().MultVec(BIAS::Vector3<double>(rays[i])));
  }
  return Intersect_(pos, dir, point3d, dist, computeDist);
}