ArCameraParam.cpp

Go to the documentation of this file.

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com
 
Copyright (c) 2012 I-maginer
 
This program 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 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 Lesser General Public License for more details.
 
You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt
 
-----------------------------------------------------------------------------
*/
 
#include "ArCameraParam.h"
#include "Prerequisites.h"
 
ArCameraParam::ArCameraParam(int width, int height, float nearclip, float farclip, std::string yamlfile)
{
        SetBitmapSize(width, height);
        SetClipping(nearclip, farclip);
 
        cv::Mat Distorsion=cv::Mat::zeros(5,1,CV_32FC1);
  cv::Mat CameraMatrix=cv::Mat::eye(3,3,CV_32FC1);
 
  //set default camera values
  float f = std::max(bitmapSize.width, bitmapSize.height);
 
  CameraMatrix.at<float>(0, 0) = f;
  CameraMatrix.at<float>(0, 2) = static_cast<float>(bitmapSize.width) * 0.5f;
  CameraMatrix.at<float>(1, 1) = f;
  CameraMatrix.at<float>(1, 2) = static_cast<float>(bitmapSize.height) * 0.5f;
 
  Distorsion.at<float>(0, 0) = 0.0f;
  Distorsion.at<float>(1, 0) = 0.0f;
  Distorsion.at<float>(2, 0) = 0.0f;
  Distorsion.at<float>(3, 0) = 0.0f;
  Distorsion.at<float>(4, 0) = 0.0f;
 
        camParam = aruco::CameraParameters(CameraMatrix, Distorsion, bitmapSize);
        SetCameraParameter(yamlfile);
}
 
ArCameraParam::ArCameraParam(int width, int height, float nearclip, float farclip, Vector2 &flength, Vector2 &center)
{
  SetBitmapSize(width, height);
  SetClipping(nearclip, farclip);
 
  cv::Mat Distorsion = cv::Mat::zeros(5, 1, CV_32FC1);
  cv::Mat CameraMatrix = cv::Mat::eye(3, 3, CV_32FC1);
  
  CameraMatrix.at<float>(0, 0) = flength.x;
  CameraMatrix.at<float>(0, 2) = center.x;
  CameraMatrix.at<float>(1, 1) = flength.y;
  CameraMatrix.at<float>(1, 2) = center.y;
 
  Distorsion.at<float>(0, 0) = 0.0f;
  Distorsion.at<float>(1, 0) = 0.0f;
  Distorsion.at<float>(2, 0) = 0.0f;
  Distorsion.at<float>(3, 0) = 0.0f;
  Distorsion.at<float>(4, 0) = 0.0f;
 
  camParam = aruco::CameraParameters(CameraMatrix, Distorsion, bitmapSize);
}
 
ArCameraParam::~ArCameraParam()
{
}
 
ArCameraParam::ArCameraParam(const ArCameraParam& CI)
{
  bitmapSize = CI.bitmapSize;
  camParam = aruco::CameraParameters(CI.camParam);
  clipping = CI.clipping;
  mCamQuat = CI.mCamQuat;
  mCamOffset = CI.mCamOffset;
 
  std::vector<ArImuData> cimu = CI.GetCameraImu();
  std::vector<ArImuData> imuCpy;
  for (unsigned int i = 0; i < cimu.size(); i++)
    imuCpy.push_back(ArImuData(Vector3(cimu[i].m_gry), Vector3(cimu[i].m_acc), cimu[i].m_delta));
  mImuData = imuCpy;
}
 
aruco::CameraParameters ArCameraParam::GetCameraParameter()
{
        return camParam;
}
 
void ArCameraParam::SetCameraParameter(std::string yamlfile)
{
  // load yaml calibration file or default values
  if (!yamlfile.empty())
  {
    try
    {
            camParam.readFromXMLFile(yamlfile.c_str());
    }
    catch (cv::Exception e)
    {
      MMechostr(MSKDEBUG, "ArCameraParam::SetCameraParameter cloud not read file : %s", yamlfile.c_str());
    }
  }
}
 
cv::Size ArCameraParam::GetBitmapSize()
{
        return bitmapSize;
}
 
void ArCameraParam::SetBitmapSize(int width, int height)
{
        bitmapSize.height = height;
        bitmapSize.width = width;
}
 
cv::Point2f ArCameraParam::GetClipping()
{
        return clipping;
}
 
void ArCameraParam::SetClipping(float nearclip, float farclip)
{
        clipping.x = std::max(0.001f, nearclip);
        clipping.y = std::max(0.002f, std::min(100000.0f, farclip));
}
 
void ArCameraParam::GetProjectionMatrix(double m_proj[16], cv::Size screensize)
{
        try
        {
                camParam.glGetProjectionMatrix(bitmapSize, screensize, m_proj, clipping.x, clipping.y, true);
        }
        catch(cv::Exception e)
        {
                MMechostr(MSKDEBUG,"error : %s \n", e.what());
        }
}
 
void ArCameraParam::SetCameraQuat(BtQuaternion quat)
{
  mCamQuat = quat;
}
 
BtQuaternion ArCameraParam::GetCameraQuat()
{
  return mCamQuat;
}
 
void ArCameraParam::AddCameraImu(Vector3 gyro, Vector3 accel, double delta)
{
  mImuData.push_back(ArImuData(gyro, accel, delta));
}
 
std::vector<ArImuData> ArCameraParam::GetCameraImu() const
{
  return mImuData;
}
 
void ArCameraParam::ResetCameraImu()
{
  mImuData.clear();
}
 
Vector3 ArCameraParam::GetCameraOffset()
{
  return mCamOffset;
}
 
void ArCameraParam::SetCameraOffset(Vector3 offset)
{
  mCamOffset = offset;
}