ExtrinsicParameterCalculatorCV.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 // ****************************************************************************
36 // Filename: ExtrinsicParameterCalculatorCV.cpp
37 // Author: Pedram Azad
38 // Date: 27.03.2007
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
50 #include "Image/PrimitivesDrawer.h"
51 #include "Image/ImageProcessor.h"
52 #include "Image/IplImageAdaptor.h"
53 #include "Image/ByteImage.h"
55 #include "Math/Math3d.h"
56 #include "Math/Math2d.h"
57 #include "Helpers/helpers.h"
58 
59 #include <stdio.h>
60 #include <opencv2/legacy/compat.hpp>
61 
62 
63 // ****************************************************************************
64 // Functions
65 // ****************************************************************************
66 
68  const CByteImage *pImage, int nColumns, int nRows, float fSquareSize, // input
69  Vec2d *pPoints, Mat3d &rotation, Vec3d &translation) // output
70 {
71  CByteImage gray_image(pImage->width, pImage->height, CByteImage::eGrayScale);
72  CByteImage temp_image(&gray_image);
73 
74  ImageProcessor::ConvertImage(pImage, &gray_image);
75 
76  IplImage *pIplImage = IplImageAdaptor::Adapt(&gray_image);
77  IplImage *pIplTempImage = IplImageAdaptor::Adapt(&temp_image);
78  CvMemStorage *pStorage = cvCreateMemStorage();
79 
80  CvPoint2D32f *pIplPoints = new CvPoint2D32f[nColumns * nRows];
81 
82  int nPoints = 0;
83 
84  if (cvFindChessBoardCornerGuesses(pIplImage, pIplTempImage, pStorage,
85  cvSize(nColumns - 1, nRows - 1), pIplPoints, &nPoints) == 0 ||
86  nPoints != (nRows - 1) * (nColumns - 1))
87  {
88  // free memory
89  cvReleaseImageHeader(&pIplImage);
90  cvReleaseImageHeader(&pIplTempImage);
91  cvReleaseMemStorage(&pStorage);
92  delete [] pIplPoints;
93  return false;
94  }
95 
96  cvFindCornerSubPix(pIplImage, pIplPoints, nPoints,
97  cvSize(5, 5), cvSize(-1, -1),
98  cvTermCriteria(CV_TERMCRIT_ITER |CV_TERMCRIT_EPS, 10, 0.1));
99 
100  const CCalibration::CCameraParameters &cameraParameters = pCalibration->GetCameraParameters();
101 
102  float distortion[] = { (float) cameraParameters.distortion[0], (float) cameraParameters.distortion[1], (float) cameraParameters.distortion[2], (float) cameraParameters.distortion[3] };
103  float focalLength[] = { (float) cameraParameters.focalLength.x, (float) cameraParameters.focalLength.y };
104  CvPoint2D32f principalPoint;
105  float rotationVector[3];
106  float translationVector[3];
107 
108  principalPoint.x = (float) cameraParameters.principalPoint.x;
109  principalPoint.y = (float) cameraParameters.principalPoint.y;
110 
111  CvPoint3D32f *pIplObjectPoints = (CvPoint3D32f *) malloc(nPoints * sizeof(CvPoint3D32f));
112 
113  int i = 0;
114  for (int y = 0; y < nRows - 1; y++)
115  for (int x = 0; x < nColumns - 1; x++, i++)
116  {
117  pIplObjectPoints[i].x = fSquareSize * x;
118  pIplObjectPoints[i].y = fSquareSize * y;
119  pIplObjectPoints[i].z = 0;
120  }
121 
122  cvFindExtrinsicCameraParams(nPoints,
123  cvSize(pImage->width, pImage->height),
124  pIplPoints,
125  pIplObjectPoints,
126  focalLength,
127  principalPoint,
128  distortion,
129  rotationVector,
130  translationVector);
131 
132  float rotationMatrix[9];
133 
134  CvMat rmat = cvMat(3, 3, CV_32FC1, rotationMatrix);
135  CvMat rvec = cvMat(3, 1, CV_32FC1, rotationVector);
136  cvRodrigues(&rmat, &rvec, 0, CV_RODRIGUES_V2M);
137 
138  // result:
139 
140  // translation
141  Math3d::SetVec(translation, translationVector[0], translationVector[1], translationVector[2]);
142 
143  // rotation
144  rotation.r1 = rotationMatrix[0];
145  rotation.r2 = rotationMatrix[1];
146  rotation.r3 = rotationMatrix[2];
147  rotation.r4 = rotationMatrix[3];
148  rotation.r5 = rotationMatrix[4];
149  rotation.r6 = rotationMatrix[5];
150  rotation.r7 = rotationMatrix[6];
151  rotation.r8 = rotationMatrix[7];
152  rotation.r9 = rotationMatrix[8];
153 
154  // corner points
155  for (i = 0; i < nPoints; i++)
156  Math2d::SetVec(pPoints[i], pIplPoints[i].x, pIplPoints[i].y);
157 
158  // free memory
159  cvReleaseImageHeader(&pIplImage);
160  cvReleaseImageHeader(&pIplTempImage);
161  cvReleaseMemStorage(&pStorage);
162  free(pIplObjectPoints);
163  delete [] pIplPoints;
164 
165  return true;
166 }
167 
168 
169 void ExtrinsicParameterCalculatorCV::DrawExtrinsic(CByteImage *pResultImage, const CCalibration *pCalibration, const Vec2d *pPoints, int nPoints, float fSquareSize)
170 {
171  Vec3d worldPoint0 = { 0, 0, 0 };
172  Vec3d worldPointX = { 2 * fSquareSize, 0, 0 };
173  Vec3d worldPointY = { 0, 2 * fSquareSize, 0 };
174  Vec2d imagePoint0, imagePointX, imagePointY;
175  pCalibration->WorldToImageCoordinates(worldPoint0, imagePoint0);
176  pCalibration->WorldToImageCoordinates(worldPointX, imagePointX);
177  pCalibration->WorldToImageCoordinates(worldPointY, imagePointY);
178 
179  PrimitivesDrawer::DrawLine(pResultImage, imagePoint0, imagePointX, 0, 0, 0, 2);
180  PrimitivesDrawer::DrawLine(pResultImage, imagePoint0, imagePointY, 0, 0, 0, 2);
181  PrimitivesDrawer::DrawCircle(pResultImage, imagePoint0, 5, 0, 0, 0, -1);
182  PrimitivesDrawerCV::PutText(pResultImage, "x", imagePointX.x, imagePointX.y, 0.75, 0.75, 0, 0, 0, 2);
183  PrimitivesDrawerCV::PutText(pResultImage, "y", imagePointY.x, imagePointY.y, 0.75, 0.75, 0, 0, 0, 2);
184 
185  if (pPoints && nPoints > 0)
186  {
187  Vec2d last_point;
188  Math2d::SetVec(last_point, pPoints[0]);
189 
190  for (int i = 0; i < nPoints; i++)
191  {
192  int r, g, b;
193  hsv2rgb(int(float(i) / (nPoints - 1) * 240), 255, 255, r, g, b);
194  PrimitivesDrawer::DrawCircle(pResultImage, pPoints[i], 3, r, g, b, -1);
195  PrimitivesDrawer::DrawLine(pResultImage, last_point, pPoints[i], r, g, b, 1);
196  Math2d::SetVec(last_point, pPoints[i]);
197  }
198  }
199 }
float r4
Definition: Math3d.h:95
Struct containing all parameters of the camera model.
Definition: Calibration.h:133
GLubyte g
Definition: glext.h:5166
float r7
Definition: Math3d.h:95
float y
Definition: Math2d.h:84
float r1
Definition: Math3d.h:95
int width
The width of the image in pixels.
Definition: ByteImage.h:257
void hsv2rgb(int h, int s, int v, int &r, int &g, int &b)
Definition: helpers.cpp:348
float r2
Definition: Math3d.h:95
float r3
Definition: Math3d.h:95
void PutText(CByteImage *pImage, const char *pText, double x, double y, double scale_x, double scale_y, int r=255, int g=255, int b=255, int thickness=1)
Draws text into a CByteImage.
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
Data structure for the representation of 8-bit grayscale images and 24-bit RGB (or HSV) color images ...
Definition: ByteImage.h:80
bool GetPointsAndTranslationAndRotation(const CCalibration *pCalibration, const CByteImage *pImage, int nColumns, int nRows, float fSquareSize, Vec2d *pPoints, Mat3d &rotation, Vec3d &translation)
bool ConvertImage(const CByteImage *pInputImage, CByteImage *pOutputImage, bool bFast=false, const MyRegion *pROI=0)
Converts a grayscale CByteImage to an RGB CByteImage image and vice versa.
GLenum GLint x
Definition: glext.h:3125
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
Definition: Calibration.h:268
float x
Definition: Math2d.h:84
void SetVec(Vec3d &vec, float x, float y, float z)
Definition: Math3d.cpp:243
void DrawCircle(CByteImage *pImage, float mx, float my, float radius, int r=255, int g=255, int b=255, int thickness=1, bool bAntiAlias=false)
Draws a circle into a CByteImage.
float r6
Definition: Math3d.h:95
void DrawExtrinsic(CByteImage *pResultImage, const CCalibration *pCalibration, const Vec2d *pPoints, int nPoints, float fSquareSize)
float r8
Definition: Math3d.h:95
float r9
Definition: Math3d.h:95
int height
The height of the image in pixels.
Definition: ByteImage.h:264
void DrawLine(CByteImage *pImage, const PointPair2d &line, int r=255, int g=255, int b=255, int thickness=1)
Draws a line segment into a CByteImage, given its two end points.
GLubyte GLubyte b
Definition: glext.h:5166
GLdouble GLdouble GLdouble r
Definition: glext.h:3227
float r5
Definition: Math3d.h:95
GLenum GLint GLint y
Definition: glext.h:3125
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
IplImage * Adapt(const CByteImage *pImage, bool bAllocateMemory=false)
Converts a CByteImage to an IplImage.
void SetVec(Vec2d &vec, float x, float y)
Definition: Math2d.cpp:68
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
Camera model parameters and functions for a single camera.
Definition: Calibration.h:125
void WorldToImageCoordinates(const Vec3d &worldPoint, Vec2d &imagePoint, bool bUseDistortionParameters=true) const
Transforms 3D world coordinates to 2D image coordinates.


asr_ivt
Author(s): Allgeyer Tobias, Hutmacher Robin, Kleinert Daniel, Meißner Pascal, Scholz Jonas, Stöckle Patrick
autogenerated on Mon Dec 2 2019 03:47:27