shot.h

Go to the documentation of this file.

/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004                                                \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* This program is free software; you can redistribute it and/or modify      *
* it under the terms of the GNU General Public License as published by      *
* the Free Software Foundation; either version 2 of the License, or         *
* (at your option) any later version.                                       *
*                                                                           *
* This program 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 General Public License (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/


#ifndef __VCGLIB_SHOT
#define __VCGLIB_SHOT

#include <vcg/space/point2.h>
#include <vcg/space/point3.h>
#include <vcg/math/similarity.h>
#include <vcg/math/camera.h>

namespace vcg{

template <class S, class RotationType = Matrix44<S> >
class Shot {
public:
  typedef Camera<S> CameraType;
  typedef S ScalarType;

  template <class ScalarType, class RotoType >
  class ReferenceFrame {
    friend class Shot<ScalarType, RotoType>;
      RotoType rot;      // rotation
      Point3<S> tra; // viewpoint
  public:
      ReferenceFrame(){}

      void SetIdentity(){ rot.SetIdentity(); tra = Point3<S>(0.0,0.0,0.0);}
      void SetTra(const Point3<S> & tr) {tra = tr;}
      void SetRot(const  RotoType & rt) {rot = rt;}
      Point3<ScalarType>   Tra() const  { return tra;}
      RotoType Rot() const  { return rot;}
  };

  Camera<S>                            Intrinsics;              // the camera that made the shot
  ReferenceFrame<S,RotationType>   Extrinsics;          // the position and orientation of the camera
  Shot(const Camera<S> &i, const ReferenceFrame<S,RotationType> &e)
  {
    Intrinsics = i;
    Extrinsics = e;
  }

  Shot(const Camera<S> &c)
  {
    Intrinsics = c;
    Extrinsics.SetIdentity();
  }

  Shot()
  {
    Extrinsics.SetIdentity();
  }

   template <class Q>
   static inline Shot Construct( const Shot<Q> & b )
   {
     ReferenceFrame<S,RotationType> r;
     r.SetRot(Matrix44<S>::Construct(b.Extrinsics.Rot()));
     r.SetTra(Point3<S>::Construct(b.Extrinsics.Tra()));
     return Shot(Camera<S>::Construct(b.Intrinsics), r);
   }


  vcg::Point3<S> Axis(const int & i)const;

  const vcg::Point3<S> GetViewDir()const;
  const vcg::Point3<S> GetViewPoint()const;
  void SetViewPoint(const vcg::Point3<S> & viewpoint);

  float GetFovFromFocal();

  void LookAt(const vcg::Point3<S> & point,const vcg::Point3<S> & up);

  void LookAt(const S & eye_x,const S & eye_y,const S & eye_z,
        const S & at_x,const S & at_y,const S & at_z,
        const S & up_x,const S & up_y,const S & up_z);

  void LookTowards(const vcg::Point3<S> & z_dir,const vcg::Point3<S> & up);

    /* Sometimes the focal is given in pixels. In this case, this function can be used to convert it in millimiters
     * given the CCD width (in mm). This method should be moved in vcg::Camera().
     * Equivalent focal length is obtained by setting the ccd width to 35 mm.
   */
  void ConvertFocalToMM(S ccdwidth);

    /* Sometimes the 3D World coordinates are known up to a scale factor. This method adjust the camera/shot parameters
     * to account for the re-scaling of the World. If the intrisic parameters are just reasonable values
     * the cameras need only a re-positioning.
     */
  void RescalingWorld(S scalefactor, bool adjustIntrinsics);

  void ApplyRigidTransformation(const Matrix44<S> & M);

  void ApplySimilarity(  Matrix44<S>   M);

  void ApplySimilarity(const Similarity<S> & Sim);

  vcg::Point3<S> ConvertWorldToCameraCoordinates(const vcg::Point3<S> & p) const;

  vcg::Point3<S> ConvertCameraToWorldCoordinates(const vcg::Point3<S> & p) const;

  /* convert a 3d point from camera (do not confuse with the Shot reference frame) to world coordinates
   * it uses inverse instead of transpose for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia)
   */
  vcg::Point3<S> ConvertCameraToWorldCoordinates_Substitute(const vcg::Point3<S> & p) const;

  vcg::Point2<S> Project(const vcg::Point3<S> & p) const;

  vcg::Point3<S> UnProject(const vcg::Point2<S> & p, const S & d) const;

  /* inverse projection from 2d camera viewport (in pixels) to 3d world coordinates (it requires the original depth of the projected point)
   * uses inverse instead of trranspose for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia)
   */
  vcg::Point3<S> UnProject_Substitute(const vcg::Point2<S> & p, const S & d) const;

  S Depth(const vcg::Point3<S> & p)const;


// accessors
public:

    Matrix44<S> GetExtrinsicsToWorldMatrix() const
    {
        Matrix44<S> rotM;
        Extrinsics.rot.ToMatrix(rotM);
        return Matrix44<S>().SetTranslate(Extrinsics.tra) * rotM.transpose();
    }

    Matrix44<S> GetWorldToExtrinsicsMatrix() const
    {
        Matrix44<S> rotM;
        Extrinsics.rot.ToMatrix(rotM);
        return rotM * Matrix44<S>().SetTranslate(-Extrinsics.tra) ;
    }

  /*  multiply the current reference frame for the matrix passed
   note: it is up to the caller to check the the matrix passed is a pure rototraslation
   */
  void MultMatrix(    vcg::Matrix44<S>    m44)
  {
    Extrinsics.tra = m44 * Extrinsics.tra;
    m44[0][3] = m44[1][3] = m44[2][3] =  0.0; //set no translation
    const S k = m44.GetRow3(0).Norm(); //compute scaling (assumed uniform)
    Extrinsics.rot = Extrinsics.rot * m44.transpose() * (1/k);
  }

  /*  multiply the current reference frame for the similarity passed
   note: it is up to the caller to check the the matrix passed is a pure rototraslation
   */
  void MultSimilarity( const Similarity<S> & s){ MultMatrix(s.Matrix());}

  bool IsValid() const
  {
    return Intrinsics.PixelSizeMm[0]>0 && Intrinsics.PixelSizeMm[1]>0;
  }

}; // end class definition



template <class S, class RotationType>
const vcg::Point3<S> Shot<S,RotationType>::GetViewDir() const
{
  return  Extrinsics.Rot().GetRow3(2);
}



//---
template <class S, class RotationType>
const vcg::Point3<S> Shot<S,RotationType>::GetViewPoint() const
{
  return  Extrinsics.tra;
}
template <class S, class RotationType>
void Shot<S,RotationType>::SetViewPoint(const vcg::Point3<S> & viewpoint)
{
  Extrinsics.SetTra( viewpoint );
}
//---

template <class S, class RotationType>
float Shot<S,RotationType>::GetFovFromFocal()
{
  double viewportYMm= Intrinsics.PixelSizeMm[1]* Intrinsics.ViewportPx[1];
  return 2*(vcg::math::ToDeg(atanf(viewportYMm/(2*Intrinsics.FocalMm))));
}

//---

template <class S, class RotationType>
vcg::Point3<S>  Shot<S,RotationType>::Axis(const int & i) const
{
  vcg::Matrix44<S> m;
  Extrinsics.rot.ToMatrix(m);
  vcg::Point3<S> aa = m.GetRow3(i);
  return aa;
}

template <class S, class RotationType>
void Shot<S,RotationType>::LookAt(const vcg::Point3<S> & z_dir,const vcg::Point3<S> & up)
{
    LookTowards(z_dir-GetViewPoint(),up);
}

template <class S, class RotationType>
void Shot<S,RotationType>::LookAt(const S & eye_x, const S & eye_y, const S & eye_z,
           const S & at_x, const S & at_y, const S & at_z,
           const S & up_x,const S & up_y,const S & up_z)
{
  SetViewPoint(Point3<S>(eye_x,eye_y,eye_z));
  LookAt(Point3<S>(at_x,at_y,at_z),Point3<S>(up_x,up_y,up_z));
}

template <class S, class RotationType>
void Shot<S,RotationType>::LookTowards(const vcg::Point3<S> & z_dir,const vcg::Point3<S> & up)
{
  vcg::Point3<S> x_dir = up ^-z_dir;
  vcg::Point3<S> y_dir = -z_dir ^x_dir;

  Matrix44<S> m;
  m.SetIdentity();
  *(vcg::Point3<S> *)&m[0][0] =  x_dir/x_dir.Norm();
  *(vcg::Point3<S> *)&m[1][0] =  y_dir/y_dir.Norm();
  *(vcg::Point3<S> *)&m[2][0] = -z_dir/z_dir.Norm();

  Extrinsics.rot.FromMatrix(m);
}

//--- Space transformation methods

template <class S, class RotationType>
vcg::Point3<S> Shot<S,RotationType>::ConvertWorldToCameraCoordinates(const vcg::Point3<S> & p) const
{
  Matrix44<S> rotM;
  Extrinsics.rot.ToMatrix(rotM);
  vcg::Point3<S> cp = rotM * (p - GetViewPoint() );
  cp[2]=-cp[2];
  return cp;
  }

template <class S, class RotationType>
vcg::Point3<S> Shot<S,RotationType>::ConvertCameraToWorldCoordinates(const vcg::Point3<S> & p) const
{
  Matrix44<S> rotM;
  vcg::Point3<S> cp = p;
  cp[2]=-cp[2];
  Extrinsics.rot.ToMatrix(rotM);
  cp = rotM.transpose() * cp + GetViewPoint();
  return cp;
}

template <class S, class RotationType>
vcg::Point3<S> Shot<S,RotationType>::ConvertCameraToWorldCoordinates_Substitute(const vcg::Point3<S> & p) const
{
  Matrix44<S> rotM;
  vcg::Point3<S> cp = p;
  cp[2]=-cp[2];
  Extrinsics.rot.ToMatrix(rotM);
  cp = Inverse(rotM) * cp + GetViewPoint();
  return cp;
}

template <class S, class RotationType>
vcg::Point2<S> Shot<S,RotationType>::Project(const vcg::Point3<S> & p) const
{
  Point3<S> cp = ConvertWorldToCameraCoordinates(p);
  Point2<S> pp = Intrinsics.Project(cp);
  Point2<S> vp = Intrinsics.LocalToViewportPx(pp);
  return vp;
}

template <class S, class RotationType>
vcg::Point3<S> Shot<S,RotationType>::UnProject(const vcg::Point2<S> & p, const S & d) const
{
  Point2<S> lp = Intrinsics.ViewportPxToLocal(p);
  Point3<S> cp = Intrinsics.UnProject(lp,d);
  Point3<S> wp = ConvertCameraToWorldCoordinates(cp);
  return wp;
}

/* inverse projection from 2d camera viewport (in pixels) to 3d world coordinates (it requires the original depth of the projected point)
 * uses inverse instead of trranspose for non-exactly-rigid rotation matrices (such as calculated by tsai and garcia)
 */
template <class S, class RotationType>
vcg::Point3<S> Shot<S,RotationType>::UnProject_Substitute(const vcg::Point2<S> & p, const S & d) const
{
  Point2<S> lp = Intrinsics.ViewportPxToLocal(p);
  Point3<S> cp = Intrinsics.UnProject(lp,d);
  Point3<S> wp = ConvertCameraToWorldCoordinates_Substitute(cp);
  return wp;
}

template <class S, class RotationType>
S Shot<S,RotationType>::Depth(const vcg::Point3<S> & p)const
{
  return ConvertWorldToCameraCoordinates(p).Z();
}

/* Sometimes the focal is given in pixels. In this case, this function can be used to convert it in millimiters
 * given the CCD width (in mm). This method should be moved in vcg::Camera().
 * Equivalent focal length is obtained by setting the ccd width to 35 mm.
 */
template <class S, class RotationType>
void Shot<S, RotationType>::ConvertFocalToMM(S ccdwidth)
{
  double ccd_width = ccdwidth; // ccd is assumed conventionally to be 35mm
  double ccd_height = (ccd_width * Intrinsics.ViewportPx[1]) / Intrinsics.ViewportPx[0];
  Intrinsics.PixelSizeMm[0] = (ccd_width / Intrinsics.ViewportPx[0]);
  Intrinsics.PixelSizeMm[1] = (ccd_height / Intrinsics.ViewportPx[1]);
  Intrinsics.FocalMm = (ccd_width * Intrinsics.FocalMm) / Intrinsics.ViewportPx[0];  // NOW FOCAL IS IN MM
}

/* Sometimes the 3D World coordinates are known up to a scale factor. This method adjust the camera/shot parameters
 * to account for the re-scaling of the World. If the intrisic parameters are just reasonable values
 * the cameras need only a re-positioning.
 */
template <class S, class RotationType>
void Shot<S, RotationType>::RescalingWorld(S scalefactor, bool adjustIntrinsics)
{
    // adjust INTRINSICS (if required)

        if (adjustIntrinsics)
        {
            Intrinsics.FocalMm = Intrinsics.FocalMm * scalefactor;
            double ccdwidth = static_cast<double>(Intrinsics.ViewportPx[0] * Intrinsics.PixelSizeMm[0]);
            double ccdheight = static_cast<double>(Intrinsics.ViewportPx[1] * Intrinsics.PixelSizeMm[1]);

            Intrinsics.PixelSizeMm[0] = (ccdwidth * scalefactor) / Intrinsics.ViewportPx[0];
            Intrinsics.PixelSizeMm[1] = (ccdheight * scalefactor) / Intrinsics.ViewportPx[1];
        }

    // adjust EXTRINSICS

    // rotation remains the same (!)
    // nothing to do..

    // the viewpoint should be modified according to the scale factor
    Extrinsics.tra *= scalefactor;
}

template <class S, class RotationType>
void Shot<S, RotationType>::ApplyRigidTransformation(const Matrix44<S> & M)
{
  Matrix44<S> rotM;
  Extrinsics.rot.ToMatrix(rotM);
  // roto-translate the viewpoint
  Extrinsics.tra = M * Extrinsics.tra;
  Matrix44<S> newRot = rotM * M.transpose();
  newRot[3][0] =  newRot[3][1] =  newRot[3][2] = 0.0;

  Extrinsics.SetRot(newRot);
}

template <class S, class RotationType>
void Shot<S, RotationType>::ApplySimilarity( Matrix44<S>   M)
{
  Matrix44<S> rotM;
  Extrinsics.rot.ToMatrix(rotM);

  // normalize
  M = M * (1/M.ElementAt(3,3));
  M[3][3] = 1; // just for numeric precision

  // compute scale factor
  ScalarType scalefactor = 1.0 / pow(ScalarType(M.Determinant()),1/ScalarType(3.0));

  // roto-translate the viewpoint
  Extrinsics.tra = M * Extrinsics.tra;

  vcg::Matrix44<S> M2 = M;

  M2 = M2 * scalefactor;                                // remove the scaling
  M2[3][3] = 1.0;
  M2[0][3] = M2[1][3] = M2[2][3] = 0;   // remove the translation

  rotM = rotM * M2.transpose();
  Extrinsics.SetRot(rotM);
}

template <class S, class RotationType>
void Shot<S, RotationType>::ApplySimilarity(const Similarity<S> & Sm)
{
  Matrix44<S> rotM;
  Extrinsics.rot.ToMatrix(rotM);

  // similarity decomposition
  vcg::Matrix44<S> R;
  Sm.rot.ToMatrix(R);
  vcg::Matrix44<S> T;
  T.SetIdentity();
  T.ElementAt(0,3) = Sm.tra[0];
  T.ElementAt(1,3) = Sm.tra[1];
  T.ElementAt(2,3) = Sm.tra[2];
  vcg::Matrix44d S44;
  S44.SetIdentity();
  S44 *= Sm.sca;
  S44.ElementAt(3,3) = 1.0;

  vcg::Matrix44<S> M = T * R * S44;

  // roto-translate the viewpoint
  Extrinsics.tra = M * Extrinsics.tra;

  vcg::Matrix44<S> M2 = M;

  M2 = M2 * (1.0 / Sm.sca);

  Extrinsics.rot = rotM * M2.transpose();

  Extrinsics.rot.ElementAt(3,0) = 0;
  Extrinsics.rot.ElementAt(3,1) = 0;
  Extrinsics.rot.ElementAt(3,2) = 0;
  Extrinsics.rot.ElementAt(3,3) = 1;

}


//--------------------------------


//--- utility definitions
typedef  Shot<float> Shotf;
typedef  Shot<double> Shotd;
//-----------------------

} // end name space

#endif