TestProjectionParametersBase.cpp

/* This file is part of the BIAS library (Basic ImageAlgorithmS).
   
   Copyright (C) 2003-2009    (see file CONTACT for details)
   Multimediale Systeme der Informationsverarbeitung
   Institut fuer Informatik
   Christian-Albrechts-Universitaet Kiel
   
   BIAS is free software; you can redistribute it and/or modify
   it under the terms of the GNU Lesser General Public License as published by
   the Free Software Foundation; either version 2.1 of the License, or
   (at your option) any later version.
   
   BIAS is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public License
   along with BIAS; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */


/**
   @file TestProjectionParametersBase.cpp
   @brief Unit test for basic functionality of projection parameters.
   @relates ProjectionParametersBase
   @ingroup g_tests
   @author MIP
*/


#include <Base/Common/CompareFloatingPoint.hh>
#include <Geometry/CoordinateTransform3D.hh>
#include <Geometry/ProjectionParametersBase.hh>
#include <Geometry/ProjectionParametersBufferedRay.hh>
#include <Geometry/ProjectionParametersCylindric.hh>
#include <Geometry/ProjectionParametersFactory.hh>
#include <Geometry/ProjectionParametersGreatCircles.hh>
#include <Geometry/ProjectionParametersIO.hh>
#include <Geometry/ProjectionParametersPerspective.hh>
#include <Geometry/ProjectionParametersPerspectiveDepth.hh>
#include <Geometry/ProjectionParametersSpherical.hh>
#include <Geometry/ProjectionParametersSphericalSimple.hh>
#include <Geometry/ProjectionParametersZoom.hh>
#include <Base/Image/Image.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Math/Random.hh>
#include <string>
#include <vector>

#ifdef BIAS_HAVE_XML2
#include <Base/Common/XMLIO.hh>
#endif

using namespace BIAS;
using namespace std;


#define MAX_C         10.0
#define MAX_COORDS_3D 100.0
#define EPSILON       1e-9

bool verbose = false;

int main(int argc, char *argv[])
{
  // Default values
  const unsigned int width = 1000;
  const unsigned int height = 1000;
  const unsigned int radius = min(width,height);
  const double phi = M_PI;
  const double theta = M_PI;

  // Create instances of different projection parameters
  ProjectionParametersPerspective pp_persp(width, height);
  pp_persp.SetSimplePerspective(0.5*phi, width, height);
  ProjectionParametersSpherical pp_sph(radius, theta, width, height);
  ProjectionParametersSphericalSimple
    pp_sph_smp(-0.5*phi, 0, phi/(double)radius,
               theta/(double)radius, width, height);
  ProjectionParametersCylindric
    pp_cyl(-10.0, 10.0, -0.5*phi, 0.5*phi, width, height);
  ProjectionParametersBufferedRay pp_buf_ray(&pp_persp);
  ProjectionParametersGreatCircles pp_gr_circ(width, height);
  pp_gr_circ.SetInternals(-0.5*phi, 0.5*phi, 0, theta, width, height);
  ProjectionParametersZoom pp_zoom(width, height);
  std::vector<ProjectionParametersBase*> projections;
  std::vector<std::string> names;
  projections.push_back(&pp_persp);
  names.push_back("ProjectionParametersPerspective");
  projections.push_back(&pp_sph);
  names.push_back("ProjectionParametersSpherical");
  projections.push_back(&pp_sph_smp);
  names.push_back("ProjectionParametersSphericalSimple");
  projections.push_back(&pp_cyl);
  names.push_back("ProjectionParametersCylindric");
  projections.push_back(&pp_buf_ray);
  names.push_back("ProjectionParametersBufferedRay");
  projections.push_back(&pp_gr_circ);
  names.push_back("ProjectionParametersGreatCircles");
  projections.push_back(&pp_zoom);
  names.push_back("ProjectionParametersZoom");
  const unsigned int projs_num = projections.size();
  BIASASSERT(projections.size() == names.size());

  // Create randomizer instance
  Random random;

  // Test pose interface
  cout << "Start pose interface tests...\n";
  for (unsigned int proj_id = 0; proj_id < projs_num; proj_id++)
  {
    cout << "- Testing " << names[proj_id] << "... ";
    if (verbose) cout << endl;

    ProjectionParametersBase *ppb = projections[proj_id];
    Vector3<double> C(0,0,0);
    Vector3<double> rad(0,0,0);
    Quaternion<double> q(0,0,0,1);
    RMatrix R(MatrixIdentity);

    // create random pose
    for (unsigned int k = 0; k < 3; k++)
      C[k] = random.GetUniformDistributed (-MAX_C, MAX_C);
    for (unsigned int k = 0; k < 3; k++)
      rad[k] = random.GetUniformDistributed (-M_PI, M_PI);
    q.SetXYZ(rad[0], rad[1], rad[2]);
    R.SetFromQuaternion(q);
    if (verbose) cout << "  - created random pose C = " << C << ", q = "
                      << q << ", R = " << R << endl; 

    // test initial validity
    if (ppb->PoseValid() || ppb->CValid() || ppb->QValid()) {
      cout << names[proj_id] << " pose should be initially invalid!\n";
      BIASABORT;
    }

    // set position and test validity
    ppb->SetC(C);
    if (!ppb->CValid()) {
      cout << names[proj_id] << " CValid() failed after setting position!\n";
      BIASABORT;
    }
    if (ppb->PoseValid()) {
      cout << names[proj_id] << " pose should stay invalid after setting "
           << "position only, but is valid!\n";
      BIASABORT;
    }

    // set orientation and test validity
    ppb->SetQ(q);
    if (!ppb->QValid()) {
      cout << names[proj_id] << " QValid() failed after setting orientation!\n";
      BIASABORT;
    }
    if (!ppb->PoseValid()) {
      cout << names[proj_id] << " pose should be valid after setting position "
           << "and orientation, but is invalid!\n";
      BIASABORT;
    }

    // test invalidate/validate
    ppb->InvalidatePose();
    if (ppb->PoseValid() || ppb->CValid() || ppb->QValid()) {
      cout << names[proj_id] << " pose should be invalid after Invalidate()!\n";
      BIASABORT;
    }
    ppb->ValidatePose();
    if (!ppb->PoseValid() || !ppb->CValid() || !ppb->QValid()) {
      cout << names[proj_id] << " pose should be valid after Validate()!\n";
      BIASABORT;
    }

    // test position and orientation
    Vector3<double> test_C = ppb->GetC();
    Quaternion<double> test_q = ppb->GetQ();
    Quaternion<double> test_q_neg;
    Matrix3x3<double> test_R = Matrix3x3<double>(ppb->GetR());
    test_q.Multiply(-1.0, test_q_neg);
    if (verbose) cout << " GetC() : " << C << ", GetQ() : " << q
                      << ", GetR() : " << R << endl;
    if (!BIAS::Equal(C.Dist(test_C), 0.0, EPSILON)) {
      cout << names[proj_id] << " position differs after Set()/Get()!\n";
      cout << "expected C = " << C << "\tfound C = " << test_C << endl;
      BIASABORT;
    }
    if (!BIAS::Equal(q.Dist(test_q), 0.0, EPSILON) &&
        !BIAS::Equal(q.Dist(test_q_neg), 0.0, EPSILON)) {
      cout << names[proj_id] << " orientation differs after Set()/Get()!\n";
      cout << "expected q = +-" << q << "\tfound q = " << test_q << endl;
      BIASABORT;
    }
    if (!BIAS::Equal((R-test_R).NormL2(), 0.0, EPSILON)) {
      cout << names[proj_id] << " rotation matrix differs after Set()/Get()!\n";
      cout << "expected R = " << R << "\tfound R = " << test_R << endl;
      BIASABORT;
    }

    // test pose parametrization
    PoseParametrization pose = ppb->GetPoseParametrization();
    if (verbose) cout << "  - GetPoseParametrization() : " << pose << endl;
    test_C = pose.GetPosition();
    test_q = pose.GetOrientation();
    test_q.Multiply(-1.0, test_q_neg);
    if (!BIAS::Equal(C.Dist(test_C), 0.0, EPSILON)) {
      cout << names[proj_id] << " position differs from pose parametrization!\n";
      cout << "expected C = " << C << "\tfound C = " << test_C << endl;
      BIASABORT;
    }
    if (!BIAS::Equal(q.Dist(test_q), 0.0, EPSILON) &&
        !BIAS::Equal(q.Dist(test_q_neg), 0.0, EPSILON)) {
      cout << names[proj_id] << " orientation differs from pose parametrization!\n";
      cout << "expected q = +-" << q << "\tfound q = " << test_q << endl;
      BIASABORT;
    }
    
    // test external parameters
    Matrix3x4<double> T = ppb->GetExternals();
    if (verbose) cout << "  - GetExternals() : " << T << endl;
    test_R.SetFromRowVectors(T.GetCol(0), T.GetCol(1), T.GetCol(2));
    test_R.Mult(T.GetCol(3), test_C);
    test_C.MultIP(-1.0);
    if (!BIAS::Equal(C.Dist(test_C), 0.0, EPSILON)) {
      cout << names[proj_id] << " position differs from externals!\n";
      cout << "expected C = " << C << "\tfound C = " << test_C << endl;
      BIASABORT;
    }
    if (!BIAS::Equal((R-test_R).NormL2(), 0.0, EPSILON)) {
      cout << names[proj_id] << " rotation matrix differs from externals!\n";
      cout << "expected R = " << R << "\tfound R = " << test_R << endl;
      BIASABORT;
    }

    // test intrinsics/extrinsics comparison
    if (ppb->ProjectionParametersBase::DoIntrinsicsDiffer(ppb)) {
      cout << names[proj_id] << " intrinsics differ from themselves!\n";
      BIASABORT;
    }
    if (ppb->DoExtrinsicsDiffer(ppb)) {
      cout << names[proj_id] << " extrinsics differ from themselves!\n";
      BIASABORT;
    }
    
    // test cloning
    ProjectionParametersBase *ppb_copy = ppb->Clone();
    if (ppb->ProjectionParametersBase::DoIntrinsicsDiffer(ppb_copy)) {
      cout << names[proj_id] << " intrinsics differ after Clone()!\n";
      BIASABORT;
    }
    if (ppb->DoExtrinsicsDiffer(ppb_copy)) {
      cout << names[proj_id] << " extrinsics differ after Clone()!\n";
      BIASABORT;
    }
    delete ppb_copy;

    if (verbose) cout << "  -> "; cout << "OK\n";
  }

#ifdef BIAS_HAVE_XML2
  // Test XMLIO interface
  cout << "Start XML input/output tests...\n";

  for (unsigned int proj_id = 0; proj_id < projs_num; proj_id++)
  {
    cout << "- Testing " << names[proj_id] << "... ";
    if (verbose) cout << endl;

    ProjectionParametersBase *ppb = projections[proj_id];
    ProjectionParametersBase *ppb_copy = ppb->Clone();

    // write to xml file
    std::string filename = "Test";
    filename.append(names[proj_id]);
    filename.append(".xml");
    XMLIO xmlIO;
    std::string xmlName;
    double xmlVersion;
    ppb->XMLGetClassName(xmlName, xmlVersion);
    xmlNodePtr xmlNode = xmlIO.create(xmlName);
    xmlIO.addAttribute(xmlNode, "Version", xmlVersion);
    if (ppb->XMLOut(xmlNode, xmlIO) != 0) {
      cout << names[proj_id] << " failed to export XML node with XMLOut()!\n";
      BIASABORT;
    }
    if (xmlIO.write(filename) != 0) {
      cout << names[proj_id] << " failed to write XML to " <<  filename << "!\n";
      BIASABORT;
    }

    // read from xml file
    xmlNode = xmlIO.read(filename);
    if (xmlNode == NULL) {
      cout << names[proj_id] << " failed to read XML from " <<  filename << "!\n";
      BIASABORT;
    }
    if (ppb_copy->XMLIn(xmlNode, xmlIO) != 0) {
      cout << names[proj_id] << " failed to import XML node with XMLIn()!\n";
      BIASABORT;
    }

    // compare original and loaded projection parameters
    if (ppb->ProjectionParametersBase::DoIntrinsicsDiffer(ppb_copy)) {
      cout << names[proj_id] << " intrinsics differ after importing XML!\n";
      BIASABORT;
    }
    if (ppb->DoExtrinsicsDiffer(ppb_copy)) {
      cout << names[proj_id] << " extrinsics differ after importing XML!\n";
      BIASABORT;
    }
    delete ppb_copy;

    if (verbose) cout << "  -> "; cout << "OK\n";
  }
#endif

  // Test projection interface
  cout << "Start projection tests...\n";

  const unsigned int points_num = 100;
  for (unsigned int proj_id = 0; proj_id < projs_num; proj_id++)
  {
    cout << "- Testing " << names[proj_id] << "... ";
    if (verbose) cout << endl;

    ProjectionParametersBase *ppb = projections[proj_id];

    // skip for buffered ray projection parameters...
    if (ppb == &pp_buf_ray || ppb == &pp_zoom) {
      if (verbose) cout << "  -> "; cout << "Skipped\n";
      continue;
    }

/*
    // obtain spherical viewing range for visibility check
    CoordinateTransform3D sphericalReferenceFrame;
    double minPhi, maxPhi, centerPhi, minTheta, maxTheta;
    ppb->GetSphericalViewingRange(sphericalReferenceFrame,
                                  minPhi, maxPhi, centerPhi, minTheta, maxTheta);
    if (verbose) {                                  
      cout << "- min. phi   = " << minPhi*180.0/M_PI << " degree" << endl
           << "- max. phi   = " << maxPhi*180.0/M_PI << " degree" << endl
           << "- center phi = " << centerPhi*180.0/M_PI << " degree" << endl
           << "- min. theta = " << minTheta*180.0/M_PI << " degree" << endl
           << "- max. theta = " << maxTheta*180.0/M_PI << " degree" << endl;
    }
*/

    // create 2d/3d correspondences
    std::vector<HomgPoint2D> points2D;
    std::vector<HomgPoint3D> points3D;
    HomgPoint2D point2D(0,0,1);
    HomgPoint3D point3D(0,0,0,1);
    Vector3<double> C(ppb->GetC());
    
    if (verbose) cout << "C = " << C << endl;
    for (unsigned int i = 0; i < points_num; i++)
    {
      for (unsigned int k = 0; k < 3; k++)
        point3D[k] =
        random.GetUniformDistributed(C[k]-MAX_COORDS_3D, C[k]+MAX_COORDS_3D);
      points3D.push_back(point3D);
      point2D = ppb->Project(point3D);
      if (BIAS_ISNAN(point2D[0]) || BIAS_ISNAN(point2D[1]) || BIAS_ISNAN(point2D[2]))
        point2D.Set(0,0,0); // invalidate 2d point if it yields NaN
      points2D.push_back(point2D);
      if (verbose)
        cout << "[" << i << "] " << points2D[i] << " <-> " << points3D[i] << endl;
    }

    // test unprojection/reprojection
    double depth, diff;
    Vector3<double> ray3D, eucl3D, pos;
    unsigned int invisible_num = 0;
    for (unsigned int i = 0; i < points_num; i++)
    {
      // skip if point is not visible in image
      point2D = points2D[i];
      if (BIAS::Equal(point2D.NormL2(), 0.0)) {
        invisible_num++;
        continue;
      }

      // compare unprojected 3D point
      eucl3D = points3D[i].GetEuclidean();
      depth = C.Dist(eucl3D);
      ppb->UnProjectToRay(point2D, pos, ray3D);
      point3D = ppb->UnProjectToPoint(point2D, depth);
      diff = point3D.Dist(points3D[i]);
      if (!BIAS::Equal(diff, 0.0, 1e-10)) {
        cout << names[proj_id] << " unprojection yield inconsistent results!\n";
        cout << "3D point " << points3D[i] << " -> 2D point " << point2D
             << " -> 3D point " << point3D << " (difference = "
             << diff << ")" << endl;
        BIASABORT;
      }

      // compare unprojected ray
      diff = ray3D.Dist((eucl3D-C).GetNormalized());
      if (!BIAS::Equal(diff, 0.0, 1e-12)) {
        cout << names[proj_id] << " unprojection yield inconsistent results!\n";
        cout << "3D ray " << (eucl3D-C).GetNormalized() << " -> 2D point "
             << point2D << " -> 3D ray " << ray3D << " (difference = "
             << diff << ")" << endl;
        BIASABORT;
      }

      // compare reprojected 2D point
/*
      point2D = ppb->Project(BIAS::HomgPoint3D(ray3D+C));
      diff = point2D.Dist(points2D[i]);
      if (!BIAS::Equal(diff, 0.0, 1e-12)) {
        cout << names[proj_id] << " reprojection yield inconsistent results!\n";
        cout << "2D point " << points2D[i] << " -> 3D ray " << ray3D
             << " -> 2D point " << point2D << " (difference = "
             << diff << ")" << endl;
        BIASABORT;
      }
*/
    }

    if (verbose) cout << "  -> ";
    cout << "OK (" << invisible_num << " not visible)\n";
  }

  // Create debug image for each projection parameters
  cout << "Create debug images...\n";

  for (unsigned int proj_id = 0; proj_id < projs_num; proj_id++)
  {
    cout << "- Save debug image for " << names[proj_id] << "... ";

    ProjectionParametersBase *ppb = projections[proj_id];

    // run over image and save color-coded unprojected rays
    HomgPoint2D point2D(0,0,1);
    Vector3<double> ray3D;
    Image<unsigned char> image(width, height, 3);
    unsigned char *ptr = image.GetImageData();
    for (unsigned int y = 0; y < height; y++) {
      point2D[1] = y;
      for (unsigned int x = 0; x < width; x++) {
        point2D[0] = x;
        ray3D = ppb->UnProjectToPointLocal(point2D, 1.0);
        if (!BIAS::Equal(ray3D.NormL2(), 0.0, 1e-8))
          ray3D.Normalize();
        for (unsigned int k = 0; k < 3; k++)
          *(ptr++) = (unsigned char)rint(255.0*fabs(ray3D[k]));
      }
    }

    // write debug image to file
    std::string filename = "Test";
    filename.append(names[proj_id]);
    filename.append(".png");

    if (ImageIO::Save(filename, image) != 0)
      cout << "failed!\n";
    else
      cout << "OK\n";
  }
  
  return 0;
}