aron_conversions.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::ArmarXObjects::armem_images
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2021
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "aron_conversions.h"
 
#include <VisionX/tools/TypeMapping.h>
 
 
namespace visionx::armem::camera
{
    visionx::StereoCalibration 
    fromAron(visionx::StereoCalibration& bo, visionx::arondto::StereoCameraCalibration& dto){
        
        //Parameters for the left calibration:
        visionx::MonocularCalibration left;
        visionx::CameraParameters leftParams;
        leftParams.width = dto.calibrationLeft.cameraParam.width;
        leftParams.height = dto.calibrationLeft.cameraParam.height;
        Eigen::Vector2f pp = dto.calibrationLeft.cameraParam.principalPoint;
        leftParams.principalPoint = visionx::tools::convertEigenVecToVisionX(pp);
 
        Eigen::Vector2f fl = dto.calibrationLeft.cameraParam.focalLength;
        leftParams.focalLength = visionx::tools::convertEigenVecToVisionX(fl);
        //Note: there is a mismatch between distortion parameters in visionx and aron
        //aron does differentiate between radial and tangential distortion, visionx does not
        //for now the distortion is simply set to zero, as this is done when using it anyways
        Eigen::Vector4f rd;
        rd.setZero();
        leftParams.distortion = visionx::tools::convertEigenVecToVisionX(rd);
        Eigen::Matrix3f rot = dto.calibrationLeft.cameraParam.rotation;
        leftParams.rotation = visionx::tools::convertEigenMatToVisionX(rot);
        Eigen::Vector3f trans = dto.calibrationLeft.cameraParam.translation;
        leftParams.translation = visionx::tools::convertEigenVecToVisionX(trans);
        left.cameraParam = leftParams;
 
        //Parameters for the right calibration:
        visionx::MonocularCalibration right;
        visionx::CameraParameters rightParams;
        rightParams.width = dto.calibrationRight.cameraParam.width;
        rightParams.height = dto.calibrationRight.cameraParam.height;
 
        Eigen::Vector2f ppr = dto.calibrationRight.cameraParam.principalPoint;
        rightParams.principalPoint = visionx::tools::convertEigenVecToVisionX(ppr);
 
        Eigen::Vector2f flr = dto.calibrationRight.cameraParam.focalLength;
        rightParams.focalLength = visionx::tools::convertEigenVecToVisionX(flr);
        Eigen::Vector4f rdr;
        rdr.setZero();
        rightParams.distortion = visionx::tools::convertEigenVecToVisionX(rdr);
        Eigen::Matrix3f rotr = dto.calibrationRight.cameraParam.rotation;
        rightParams.rotation = visionx::tools::convertEigenMatToVisionX(rotr);
        Eigen::Vector3f transr = dto.calibrationRight.cameraParam.translation;
        rightParams.translation = visionx::tools::convertEigenVecToVisionX(transr);
        right.cameraParam = rightParams;
 
        //Setting rectification of homograpy:
        Eigen::Matrix3f rHL = dto.rectificationHomographyLeft;
        bo.rectificationHomographyLeft = visionx::tools::convertEigenMatToVisionX(rHL);
        Eigen::Matrix3f rHR = dto.rectificationHomographyRight;
        bo.rectificationHomographyRight = visionx::tools::convertEigenMatToVisionX(rHR);
        bo.calibrationLeft = left;
        bo.calibrationRight = right;
 
        return bo;
 
    }
 
    visionx::arondto::StereoCameraCalibration 
    toAron(visionx::arondto::StereoCameraCalibration& dto, visionx::StereoCalibration& bo){
 
        visionx::arondto::MonocularCameraCalibration left;
        visionx::arondto::CameraParameters leftParams;
 
        //set all parameters for left camera calibration:
        leftParams.height = bo.calibrationLeft.cameraParam.height;
        leftParams.width = bo.calibrationLeft.cameraParam.width;
 
        leftParams.focalLength = visionx::tools::convertVisionXVecToEigen(bo.calibrationLeft.cameraParam.focalLength);
        leftParams.principalPoint = visionx::tools::convertVisionXVecToEigen(bo.calibrationLeft.cameraParam.principalPoint);
        leftParams.radialDistortion = visionx::types::Vec(); //due to mismatch in types and non-usage of this anyways
        //leftParams.tangentialDistortion; due to mismatch in types and non-usage of this anyways
        leftParams.rotation = visionx::tools::convertVisionXMatToEigen(bo.calibrationLeft.cameraParam.rotation);
        leftParams.translation = visionx::tools::convertVisionXVecToEigen(bo.calibrationLeft.cameraParam.translation);
 
        //left camera just needs the parameters assigned:
        left.cameraParam = leftParams;
 
        visionx::arondto::MonocularCameraCalibration right;
        visionx::arondto::CameraParameters rightParams;
 
        //set all parameters for right camera calibration:
        rightParams.height = bo.calibrationRight.cameraParam.height;
        rightParams.width = bo.calibrationRight.cameraParam.width;
 
        rightParams.focalLength = visionx::tools::convertVisionXVecToEigen(bo.calibrationRight.cameraParam.focalLength);
        rightParams.principalPoint = visionx::tools::convertVisionXVecToEigen(bo.calibrationRight.cameraParam.principalPoint);
        rightParams.radialDistortion = visionx::types::Vec(); //due to mismatch in types and non-usage of this anyways
        //rightParams.tangentialDistortion; due to mismatch in types and non-usage of this anyways
        rightParams.rotation = visionx::tools::convertVisionXMatToEigen(bo.calibrationRight.cameraParam.rotation);
        rightParams.translation = visionx::tools::convertVisionXVecToEigen(bo.calibrationRight.cameraParam.translation);
 
        //right camera just needs the parameters assigned:
        right.cameraParam = rightParams;
 
        auto rectificationHomographyLeft = bo.rectificationHomographyLeft;
        auto rectificationHomographyRight = bo.rectificationHomographyRight;
 
        dto.rectificationHomographyLeft = visionx::tools::convertVisionXMatToEigen(rectificationHomographyLeft);
        dto.rectificationHomographyRight = visionx::tools::convertVisionXMatToEigen(rectificationHomographyRight);
        dto.calibrationLeft = left;
        dto.calibrationRight = right;
 
        return dto;
    }
}