My Project
Camera.h
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #ifndef CAMERA_H
25 #define CAMERA_H
26 
34 #include "Alvar.h"
35 #include "FileFormat.h"
36 #include "Pose.h"
37 #include "Util.h"
38 
39 #include <opencv2/core.hpp>
40 #include <vector>
41 
42 namespace alvar {
43 
45 struct ALVAR_EXPORT ProjPoints
46 {
47  int width;
48  int height;
49 
51  std::vector<cv::Point3d> object_points;
58  std::vector<cv::Point2d> image_points;
60  std::vector<int> point_counts;
61 
63  void Reset();
64 
66  bool AddPointsUsingChessboard(const cv::Mat &,
67  double etalon_square_size,
68  int etalon_rows,
69  int etalon_columns,
70  bool visualize);
71 
73  bool AddPointsUsingMarkers(std::vector<PointDouble> &marker_corners,
74  std::vector<PointDouble> &marker_corners_img,
75  cv::Mat & image);
76 };
77 
81 class ALVAR_EXPORT Camera
82 {
83 public:
84  cv::Mat calib_K;
85  double calib_K_data[3][3];
86  cv::Mat calib_D;
87  double calib_D_data[4];
88 
89 protected:
90  int calib_x_res;
91  int calib_y_res;
92  int x_res;
93  int y_res;
94 
95 private:
96  bool LoadCalibXML(const char *calibfile);
97  bool LoadCalibOpenCV(const char *calibfile);
98  bool SaveCalibXML(const char *calibfile);
99  bool SaveCalibOpenCV(const char *calibfile);
100 
101 public:
103  std::string
105  {
106  return "camera";
107  };
134  bool
136  {
137  if (!ser->Serialize(calib_x_res, "width"))
138  return false;
139  if (!ser->Serialize(calib_y_res, "height"))
140  return false;
141  if (!ser->Serialize(calib_K, "intrinsic_matrix"))
142  return false;
143  if (!ser->Serialize(calib_D, "distortion"))
144  return false;
145  return true;
146  }
147 
150 
152  double
154  {
155  return (2.0f * atan2(double(x_res) / 2.0f, (double)calib_K_data[0][0]));
156  }
158  double
160  {
161  return (2.0f * atan2(double(y_res) / 2.0f, (double)calib_K_data[1][1]));
162  }
163 
164  void SetSimpleCalib(int _x_res, int _y_res, double f_fac = 1.);
165 
172  bool
173  SetCalib(const char *calibfile, int _x_res, int _y_res, FILE_FORMAT format = FILE_FORMAT_DEFAULT);
174 
179  bool SaveCalib(const char *calibfile, FILE_FORMAT format = FILE_FORMAT_DEFAULT);
180 
183 
185  void SetRes(int _x_res, int _y_res);
186 
206  void GetOpenglProjectionMatrix(double proj_matrix[16],
207  const int width,
208  const int height,
209  const float far_clip = 1000.0f,
210  const float near_clip = 0.1f);
211 
213  void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height);
214 
216  void Undistort(std::vector<PointDouble> &points);
217 
219  void Undistort(PointDouble &point);
220 
222  void Undistort(cv::Point2f &point);
223 
225  void Distort(cv::Point2f &point);
226 
228  void Distort(std::vector<PointDouble> &points);
229 
231  void Distort(PointDouble &point);
232 
235  void CalcExteriorOrientation(const std::vector<cv::Point3d> &pw,
236  const std::vector<PointDouble> &pi,
237  cv::Mat & rodriques,
238  cv::Mat & tra) const;
239 
242  void CalcExteriorOrientation(const std::vector<PointDouble> &pw,
243  const std::vector<PointDouble> &pi,
244  cv::Mat & rodriques,
245  cv::Mat & tra) const;
246 
248  void CalcExteriorOrientation(const std::vector<PointDouble> &pw,
249  const std::vector<PointDouble> &pi,
250  Pose * pose) const;
251 
252  void
253  CalcExteriorOrientation(std::vector<cv::Point3d> &pw, std::vector<cv::Point2d> &pi, Pose *pose);
254 
255  bool CalcExteriorOrientation(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose);
256 
257  bool CalcExteriorOrientation(const cv::Mat &object_points,
258  cv::Mat & image_points,
259  cv::Mat & rodriques,
260  cv::Mat & tra);
262  void ProjectPoint(const cv::Point3d &pw, const Pose *pose, cv::Point2d &pi) const;
263 
265  void ProjectPoint(const cv::Point3f &pw, const Pose *pose, cv::Point2f &pi) const;
266 
268  void ProjectPoints(std::vector<cv::Point3d> &pw, Pose *pose, std::vector<cv::Point2d> &pi) const;
269 
272  void ProjectPoints(const cv::Mat &object_points,
273  const cv::Mat &rotation_vector,
274  const cv::Mat &translation_vector,
275  cv::Mat & image_points) const;
276 
279  void ProjectPoints(const cv::Mat &object_points, double gl[16], cv::Mat &image_points) const;
280 
282  void ProjectPoints(const cv::Mat &object_points, const Pose *pose, cv::Mat &image_points) const;
283 };
284 
288 struct ALVAR_EXPORT Homography
289 {
290  cv::Mat H;
291 
294 
296  void Find(const std::vector<PointDouble> &pw, const std::vector<PointDouble> &pi);
297 
299  void ProjectPoints(const std::vector<PointDouble> &from, std::vector<PointDouble> &to) const;
300 };
301 
302 } // namespace alvar
303 
304 #endif
This file defines library export definitions, version numbers and build information.
This file defines various file formats.
This file implements a pose.
This file implements generic utility functions and a serialization interface.
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
void ProjectPoint(const cv::Point3d &pw, const Pose *pose, cv::Point2d &pi) const
Project one point.
void Undistort(PointDouble &point)
Unapplys the lens distortion for one point on an image plane.
void Undistort(cv::Point2f &point)
Unapplys the lens distortion for one point on an image plane.
void ProjectPoints(const cv::Mat &object_points, double gl[16], cv::Mat &image_points) const
Project points.
void Distort(std::vector< PointDouble > &points)
Applys the lens distortion for points on image plane.
void Distort(PointDouble &point)
Applys the lens distortion for points on image plane.
double GetFovX()
Get x-direction FOV in radians.
Definition: Camera.h:153
void GetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height, const float far_clip=1000.0f, const float near_clip=0.1f)
Get OpenGL matrix Generates the OpenGL projection matrix based on OpenCV intrinsic camera matrix K.
void ProjectPoint(const cv::Point3f &pw, const Pose *pose, cv::Point2f &pi) const
Project one point.
void CalcExteriorOrientation(const std::vector< PointDouble > &pw, const std::vector< PointDouble > &pi, Pose *pose) const
Update existing pose (in rodriques&tra) based on new observations. Use (CV_32FC3 and CV_32FC2) for ma...
bool SetCalib(const char *calibfile, int _x_res, int _y_res, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Set the calibration file and the current resolution for which the calibration is adjusted to.
void SetRes(int _x_res, int _y_res)
If we have no calibration file we can still adjust the default calibration to current resolution
bool Serialize(Serialization *ser)
One of the two methods to make this class serializable by Serialization class.
Definition: Camera.h:135
void Undistort(std::vector< PointDouble > &points)
Unapplys the lens distortion for points on image plane.
void CalcExteriorOrientation(const std::vector< PointDouble > &pw, const std::vector< PointDouble > &pi, cv::Mat &rodriques, cv::Mat &tra) const
Calculate exterior orientation.
void ProjectPoints(std::vector< cv::Point3d > &pw, Pose *pose, std::vector< cv::Point2d > &pi) const
Project points.
Camera()
Constructor.
void Distort(cv::Point2f &point)
Applys the lens distortion for one point on an image plane.
bool SaveCalib(const char *calibfile, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Save the current calibration information to a file.
double GetFovY()
Get y-direction FOV in radians.
Definition: Camera.h:159
void SetOpenglProjectionMatrix(double proj_matrix[16], const int width, const int height)
Invert operation for GetOpenglProjectionMatrix.
void ProjectPoints(const cv::Mat &object_points, const Pose *pose, cv::Mat &image_points) const
Project points
void ProjectPoints(const cv::Mat &object_points, const cv::Mat &rotation_vector, const cv::Mat &translation_vector, cv::Mat &image_points) const
Project points.
void Calibrate(ProjPoints &pp)
Calibrate using the collected ProjPoints.
std::string SerializeId()
One of the two methods to make this class serializable by Serialization class.
Definition: Camera.h:104
void CalcExteriorOrientation(const std::vector< cv::Point3d > &pw, const std::vector< PointDouble > &pi, cv::Mat &rodriques, cv::Mat &tra) const
Calculate exterior orientation.
Pose representation derived from the Rotation class
Definition: Pose.h:53
Class for serializing class content to/from file or std::iostream.
Definition: Util.h:371
bool Serialize(int &data, const std::string &name)
Method for serializing 'int' data element. Used from your serializable class.
Main ALVAR namespace.
Definition: Alvar.h:174
ALVAR_EXPORT Point< cv::Point2d > PointDouble
The default double point type.
Definition: Util.h:110
FILE_FORMAT
Definition: FileFormat.h:39
@ FILE_FORMAT_DEFAULT
Default file format.
Definition: FileFormat.h:45
Simple Homography class for finding and projecting points between two planes.
Definition: Camera.h:289
void ProjectPoints(const std::vector< PointDouble > &from, std::vector< PointDouble > &to) const
Project points using the Homography.
void Find(const std::vector< PointDouble > &pw, const std::vector< PointDouble > &pi)
Find Homography for two point-sets.
Homography()
Constructor
Simple structure for collecting 2D and 3D points e.g. for camera calibration.
Definition: Camera.h:46
bool AddPointsUsingMarkers(std::vector< PointDouble > &marker_corners, std::vector< PointDouble > &marker_corners_img, cv::Mat &image)
Add elements to object_points , image_points and point_counts using detected markers.
void Reset()
Reset object_points , image_points and point_counts.
std::vector< cv::Point2d > image_points
Detected 2D object points If point_counts[0] == 10, then the first 10 points are detected in the firs...
Definition: Camera.h:58
std::vector< int > point_counts
Vector indicating how many points are detected for each frame.
Definition: Camera.h:60
std::vector< cv::Point3d > object_points
3D object points corresponding with the detected 2D image points.
Definition: Camera.h:51
bool AddPointsUsingChessboard(const cv::Mat &, double etalon_square_size, int etalon_rows, int etalon_columns, bool visualize)
Add elements to object_points , image_points and point_counts using Chessboard pattern.