Camera.h

Go to the documentation of this file.

/*
 * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
 *
 * Copyright 2007-2012 VTT Technical Research Centre of Finland
 *
 * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
 *          <http://www.vtt.fi/multimedia/alvar.html>
 *
 * ALVAR 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.
 *
 * This library 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 ALVAR; if not, see
 * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
 */

#ifndef CAMERA_H
#define CAMERA_H

#include "Alvar.h"
#include "Pose.h"
#include "Util.h"
#include "FileFormat.h"
#include <vector>

#include <ros/ros.h>
#include <ros/package.h>
#include <ros/console.h>
#include <geometry_msgs/TransformStamped.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/CameraInfo.h>
#include <visualization_msgs/Marker.h>
#include <resource_retriever/retriever.h>

namespace alvar {

struct ALVAR_EXPORT ProjPoints {
        int width;
        int height;

        std::vector<CvPoint3D64f> object_points;
        std::vector<CvPoint2D64f> image_points;
        std::vector<int> point_counts;

        void Reset();

        bool AddPointsUsingChessboard(IplImage *image, double etalon_square_size, int etalon_rows, int etalon_columns, bool visualize);

        bool AddPointsUsingMarkers(std::vector<PointDouble> &marker_corners, std::vector<PointDouble> &marker_corners_img, IplImage* image);
};


class ALVAR_EXPORT Camera {

public:

        CvMat calib_K; double calib_K_data[3][3];
        CvMat calib_D; double calib_D_data[4];
        int calib_x_res;
        int calib_y_res;
        int x_res;
        int y_res;
        bool getCamInfo_;

protected:
        std::string cameraInfoTopic_;
        sensor_msgs::CameraInfo cam_info_;
        void camInfoCallback (const sensor_msgs::CameraInfoConstPtr &);
        ros::Subscriber sub_;
        ros::NodeHandle n_;

private:
        bool LoadCalibXML(const char *calibfile);
        bool LoadCalibOpenCV(const char *calibfile);
        bool SaveCalibXML(const char *calibfile);
        bool SaveCalibOpenCV(const char *calibfile);

public:
        std::string SerializeId() { return "camera"; };
        bool Serialize(Serialization *ser) {
                if (!ser->Serialize(calib_x_res, "width")) return false;
                if (!ser->Serialize(calib_y_res, "height")) return false;
                if (!ser->Serialize(calib_K, "intrinsic_matrix")) return false;
                if (!ser->Serialize(calib_D, "distortion")) return false;
                return true;
        }

        Camera();
        Camera(ros::NodeHandle & n, std::string cam_info_topic);

        void SetCameraInfo(const sensor_msgs::CameraInfo& camInfo);

        double GetFovX() {
                return (2.0f * atan2(double(x_res) / 2.0f, (double)calib_K_data[0][0]));        
        }
        double GetFovY() {
                return (2.0f * atan2(double(y_res) / 2.0f, (double)calib_K_data[1][1]));
        }
        
        void SetSimpleCalib(int _x_res, int _y_res, double f_fac=1.);

        bool SetCalib(const char *calibfile, int _x_res, int _y_res, 
                      FILE_FORMAT format = FILE_FORMAT_DEFAULT);

        bool SaveCalib(const char *calibfile, FILE_FORMAT format = FILE_FORMAT_DEFAULT);

        void Calibrate(ProjPoints &pp);

        void SetRes(int _x_res, int _y_res);

        void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height, const float far_clip = 1000.0f, const float near_clip = 0.1f);

        void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height);

        void Undistort(std::vector<PointDouble >& points);

        void Undistort(PointDouble &point);

        void Undistort(CvPoint2D32f& point);

        void Distort(CvPoint2D32f& point);

        void Distort(std::vector<PointDouble>& points);

        void Distort(PointDouble &point);

        void CalcExteriorOrientation(std::vector<CvPoint3D64f>& pw, std::vector<CvPoint2D64f>& pi, Pose *pose);

        void CalcExteriorOrientation(std::vector<CvPoint3D64f>& pw, std::vector<PointDouble >& pi,
                                                CvMat *rodriques, CvMat *tra);

        void CalcExteriorOrientation(std::vector<PointDouble >& pw, std::vector<PointDouble >& pi,
                                                CvMat *rodriques, CvMat *tra);

        void CalcExteriorOrientation(std::vector<PointDouble>& pw, std::vector<PointDouble >& pi, Pose *pose);

        bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, Pose *pose);

        bool CalcExteriorOrientation(const CvMat* object_points, CvMat* image_points, CvMat *rodriques, CvMat *tra);

        void ProjectPoint(const CvPoint3D64f pw, const Pose *pose, CvPoint2D64f &pi) const;

        void ProjectPoint(const CvPoint3D32f pw, const Pose *pose, CvPoint2D32f &pi) const;

        void ProjectPoints(std::vector<CvPoint3D64f>& pw, Pose *pose, std::vector<CvPoint2D64f>& pi) const;

        void ProjectPoints(const CvMat* object_points, const CvMat* rotation_vector,
                       const CvMat* translation_vector, CvMat* image_points) const;
        
        void ProjectPoints(const CvMat* object_points, double gl[16], CvMat* image_points) const;

        void ProjectPoints(const CvMat* object_points, const Pose* pose, CvMat* image_points) const;
};

struct ALVAR_EXPORT Homography {
        double H_data[3][3];
        CvMat H;
        
        Homography();
        
        void Find(const std::vector<PointDouble>& pw, const std::vector<PointDouble>& pi);
        
        void ProjectPoints(const std::vector<PointDouble>& from, std::vector<PointDouble>& to);
};

} // namespace alvar

#endif