ExampleXB3Rectification.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
*/

/**
 * @example ExampleXB3Rectification.cpp
 * @relates PlanarRectification, RectificationBase
 * @brief shows rectification for PtGrey Bumblebee
 * @author MIP
 */

#include <iostream>
#include <Base/Image/ImageIO.hh>
#include <Image/Camera.hh>
#include <Image/PlanarRectification.hh>
#include <Image/RectificationBase.hh>
#include <Utils/Param.hh>
#include <Utils/IOUtils.hh>

using namespace std;
using namespace BIAS;


int main(int argc, char* argv[])
{
  Param params(true);

  int IOGrpID = params.GetFreeGroupID();
  string* leftCamName = params.AddParamString("leftCam", "", 
                                            "", 0, IOGrpID);
  string* centerCamName = params.AddParamString("centerCam", "", 
                                              "", 0, IOGrpID);
  string* rightCamName = params.AddParamString("rightCam", "", 
                                             "", 0, IOGrpID);  
  params.SetGroupName(IOGrpID, "I/O");

  if(!IOUtils::ParseCommandLineEvalHelp(params, argc, argv))
    return 0;

  Camera<unsigned char> camLeft, camCenter, camRight;
  Projection projLeft, projRight, projCenter;
  if(!IOUtils::LoadCamera<unsigned char>(leftCamName, camLeft, projLeft))
    return -1;
  if(!IOUtils::LoadCamera<unsigned char>(centerCamName, camCenter, projCenter))
    return -1;
  if(!IOUtils::LoadCamera<unsigned char>(rightCamName, camRight, projRight))
    return -1;

  
  
  Pose poseLeft = projLeft.GetPose();
  Pose poseRight = projRight.GetPose();
  Pose poseCenter = projCenter.GetPose();
  Quaternion<double> orientation;

  ProjectionParametersPerspective rectPPPLeft, rectPPPRight, rectPPPCenter;
  RectificationBase<unsigned char, unsigned char>::
    CalculateMeanOrientation(poseLeft,
                             poseCenter,
                             poseRight,
                             orientation);

  CoordinateTransform3D sphericalBase; // will contain the canonical 
  //  sphericalBase.Set(poseCenter.GetQ(), poseCenter.GetC());
  sphericalBase.Set(orientation, poseCenter.GetC());
   // (cartesian) base
  Quaternion<double> rot(0, -sin(M_PI/4.0), 0, cos(M_PI/4.0));
    sphericalBase.RotateLocalFrame(rot);
   double minPhi, maxPhi, centerPhi, minTheta,  maxTheta;
   cerr<<"sphericalBase "<<sphericalBase.GetR()<<endl;
   cerr<<"CameraBase "<<projCenter.GetR()<<endl;
   projCenter.GetParameters()->GetSphericalViewingRange(sphericalBase,
                                                        minPhi, maxPhi, 
                                                        centerPhi,
                                                        minTheta, maxTheta );
  cerr<<"minPhi "<<minPhi<<", maxPhi "<<maxPhi<<endl;
  cerr<<"centerPhi "<<centerPhi<<endl;
  cerr<<"minTheta "<<minTheta<<", maxTheta "<<maxTheta<<endl;

  double minSamplingStep;
  projCenter.GetParameters()->GetMinimalAngularSamplingStep(minSamplingStep);

  //controlls cut out !
  minPhi+= 2.0*0.06;
  maxPhi-= 5.0*0.06;
  minTheta+= 0.06;
  maxTheta-= 5.0*0.06;
  
  rectPPPCenter.SetIntrinsics(minPhi, maxPhi, 
                              minTheta, maxTheta,
                              2.0*minSamplingStep);


  rectPPPLeft=rectPPPRight=rectPPPCenter;

  rectPPPLeft.SetC(poseLeft.GetC());
  rectPPPLeft.SetQ(orientation);
  rectPPPCenter.SetC(poseCenter.GetC());
  rectPPPCenter.SetQ(orientation);
  rectPPPRight.SetC(poseRight.GetC());
  rectPPPRight.SetQ(orientation);
  cerr<<"rectPPPC = "<<rectPPPCenter<<endl;
  cerr<<"rectPPPLeft = "<<rectPPPLeft<<endl;
  cerr<<"rectPPPRight = "<<rectPPPRight<<endl;

  ProjectionMapping<unsigned char, unsigned char> rectifierLeft, 
    rectifierCenter, rectifierRight;

  unsigned int targetWidth, targetHeight;
  rectPPPCenter.GetImageSize(targetWidth, targetHeight);
  cerr<<targetWidth<<"x"<< targetHeight<<endl;

  Camera<unsigned char> rectCamLeft, rectCamRight, rectCamCenter;
  rectCamLeft.Init(targetWidth, targetHeight,3);
  rectCamRight.Init(targetWidth, targetHeight,3);
  rectCamCenter.Init(targetWidth, targetHeight,3);

  
  rectifierCenter.SetSourceCam(projCenter);
  rectifierCenter.SetSinkCam(Projection(rectPPPCenter));
  rectifierCenter.PrepareLookupTableMapping(camCenter, rectCamCenter, 
                                            MapBilinear);

  rectifierLeft.SetSourceCam(projLeft);
  rectifierLeft.SetSinkCam(Projection(rectPPPLeft));
  rectifierLeft.PrepareLookupTableMapping(camLeft, rectCamLeft, 
                                          MapBilinear);

  rectifierLeft.SetSourceCam(projRight);
  rectifierLeft.SetSinkCam(Projection(rectPPPRight));
  rectifierLeft.PrepareLookupTableMapping(camRight, rectCamRight, 
                                          MapBilinear);

  rectifierCenter.Map(camCenter, rectCamCenter);
  IOUtils::SaveCamera<unsigned char>("centerRect.mip", 
                                     rectCamCenter, 
                                     Projection(rectPPPCenter));
  rectifierCenter.Map(camLeft, rectCamLeft);
  IOUtils::SaveCamera<unsigned char>("leftRect.mip", 
                                     rectCamLeft, 
                                     Projection(rectPPPLeft));
  
  rectifierCenter.Map(camRight, rectCamRight);
  IOUtils::SaveCamera<unsigned char>("righRect.mip", 
                                     rectCamRight, 
                                     Projection(rectPPPRight));
  

  return 0;
}