coord_transform.h

Go to the documentation of this file.

#ifndef VR_COORD_TRANSFORM_H
#define VR_COORD_TRANSFORM_H

namespace vr
{

class CoordinateTransformation
{
    // camera matrix
    cv::Matx33d mat3dTo2d_;
    // inverse der camera matrix
    cv::Matx33d mat2dTo3d_;
    cv::Mat distortionCoeffs_;
    // Rotations- und Translationsvektor aus cv::solvePnP();
    // Argumente für cv::projectPoints().
    cv::Vec3d rvec_, tvec_;
    cv::Matx33d R_;
    cv::Matx33d R_inv_;
    const cv::Matx33d& scaledCameraMatrix(const cv::Matx33d& cameraMatrix, double scaleFactor);
public:
    CoordinateTransformation() {};
    CoordinateTransformation(const cv::Matx33d& cameraMatrix,
        const cv::Mat& distortionCoeffs,
        double scaleFactor = 1.0);
    ~CoordinateTransformation();
    void init(cv::InputArray objPoints, cv::InputArray imgPoints,
        const cv::Vec3d& rvec, const cv::Vec3d& tvec);
    inline void copyExtrinsicParamsFrom(const CoordinateTransformation& src)
    {
        rvec_   = src.rvec_;
        tvec_   = src.tvec_;
        R_      = src.R_;
        R_inv_  = src.R_inv_;
    }
    inline const cv::Vec3d& getRotationVector() const {return rvec_; };
    inline const cv::Vec3d& getTranslationVector() const {return tvec_; };
    cv::Vec3d& fromScreen(const cv::Point2f& imgPoint, cv::Vec3d& imgPlanePoint) const;
    cv::Point2f& toScreen(const cv::Vec3d& camPoint, cv::Point2f& imgPoint) const;
    void toScreen(cv::InputArray camPoints, cv::OutputArray imgPoints) const
    {
        cv::Vec3d rvec(0, 0, 0);
        cv::Vec3d tvec(0, 0, 0);
        cv::projectPoints(camPoints, rvec, tvec, mat3dTo2d_, distortionCoeffs_, imgPoints);
    }

    cv::Vec3d& fromWorld(const cv::Vec3d& objPoint, cv::Vec3d& camPoint) const;
    cv::Point3f& fromWorld(const cv::Point3f& objPoint, cv::Point3f& camPoint) const;
    cv::Vec3d& toWorld(const cv::Vec3d& camPoint, cv::Vec3d& objPoint) const;
    void worldToScreen(cv::InputArray objPoints, cv::OutputArray imgPoints) const
    {
        cv::projectPoints(objPoints, rvec_, tvec_, mat3dTo2d_, distortionCoeffs_, imgPoints);
    }
    cv::Point2f& worldToScreen(const cv::Vec3d& objPoint, cv::Point2f& imgPoint) const;
    Line screenToWorld(const cv::Point2f& imgPoint) const;
};

} // namespace vr

#endif // VR_COORD_TRANSFORM_H