Skip to content

Commit 57a8c18

Browse files
philip-lambphilip-lamb
committed
Refactor calibration-related functionality into new Calibration class.
1 parent 34966d5 commit 57a8c18

File tree

7 files changed

+410
-333
lines changed

7 files changed

+410
-333
lines changed

‎Calibration.cpp‎

Lines changed: 230 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,230 @@
1+
//
2+
// calib.c
3+
// ARToolKit6 Camera Calibration Utility
4+
//
5+
// Created by Philip Lamb on 20/03/17.
6+
// Copyright © 2017 artoolkit.org. All rights reserved.
7+
//
8+
9+
#include "Calibration.hpp"
10+
#include <opencv2/calib3d/calib3d.hpp>
11+
#include <opencv2/imgproc/imgproc_c.h>
12+
#include "calc.h"
13+
14+
15+
bool Calibration::init(const int calibImageCountMax, const int chessboardCornerNumX, const int chessboardCornerNumY, const int chessboardSquareWidth, const int videoWidth, const int videoHeight)
16+
{
17+
// Calibration inputs.
18+
m_calibImageCount = 0;
19+
m_calibImageCountMax = calibImageCountMax;
20+
m_chessboardCornerNumX = chessboardCornerNumX;
21+
m_chessboardCornerNumY = chessboardCornerNumY;
22+
arMalloc(gCorners, CvPoint2D32f, chessboardCornerNumX*chessboardCornerNumY*calibImageCountMax);
23+
m_chessboardSquareWidth = chessboardSquareWidth;
24+
m_videoWidth = videoWidth;
25+
m_videoHeight = videoHeight;
26+
27+
//
28+
// Corner finder inputs and outputs.
29+
//
30+
CORNER_FINDER_DATA_T* cornerFinderDataPtr;
31+
arMallocClear(cornerFinderDataPtr, CORNER_FINDER_DATA_T, 1);
32+
cornerFinderDataPtr->chessboardCornerNumX = chessboardCornerNumX;
33+
cornerFinderDataPtr->chessboardCornerNumY = chessboardCornerNumY;
34+
arMalloc(cornerFinderDataPtr->corners, CvPoint2D32f, cornerFinderDataPtr->chessboardCornerNumX * cornerFinderDataPtr->chessboardCornerNumY);
35+
arMalloc(cornerFinderDataPtr->videoFrame, ARUint8, videoWidth*videoHeight);
36+
cornerFinderDataPtr->calibImage = cvCreateImageHeader(cvSize(videoWidth, videoHeight), IPL_DEPTH_8U, 1);
37+
cvSetData(cornerFinderDataPtr->calibImage, cornerFinderDataPtr->videoFrame, videoWidth); // Last parameter is rowBytes.
38+
39+
// Spawn the corner finder worker thread.
40+
gCornerFinderThread = threadInit(0, (void *)(cornerFinderDataPtr), cornerFinder);
41+
42+
// Corner finder results copy, for display to user.
43+
arMalloc(gCornerFinderOutputCorners, CvPoint2D32f, chessboardCornerNumX*chessboardCornerNumY);
44+
arMalloc(gCornerFinderOutputImage, ARUint8, videoWidth*videoHeight);
45+
pthread_mutex_init(&gCornerFinderResultLock, NULL);
46+
47+
return true;
48+
}
49+
50+
bool Calibration::frame(ARVideoSource *vs)
51+
{
52+
//
53+
// Start of main calibration-related cycle.
54+
//
55+
56+
// First, see if an image has been completely processed.
57+
if (threadGetStatus(gCornerFinderThread)) {
58+
threadEndWait(gCornerFinderThread); // We know from status above that worker has already finished, so this just resets it.
59+
ARLOGd("processFrame: corner find DONE.\n");
60+
61+
// Copy the results.
62+
pthread_mutex_lock(&gCornerFinderResultLock); // Results are also read by GL thread, so need to lock before modifying.
63+
CORNER_FINDER_DATA_T *cornerFinderData = (CORNER_FINDER_DATA_T *)threadGetArg(gCornerFinderThread);
64+
gCornerFinderOutputFoundAllFlag = cornerFinderData->cornerFoundAllFlag;
65+
gCornerFinderOutputFoundCount = cornerFinderData->cornerCount;
66+
for (int i = 0; i < cornerFinderData->chessboardCornerNumX*cornerFinderData->chessboardCornerNumY; i++) gCornerFinderOutputCorners[i] = cornerFinderData->corners[i];
67+
memcpy(gCornerFinderOutputImage, cornerFinderData->videoFrame, vs->getVideoWidth()*vs->getVideoHeight()); // For the visual overlay of corner locations to be accurate, we need a copy of the image in which they were found.
68+
pthread_mutex_unlock(&gCornerFinderResultLock);
69+
}
70+
71+
// If corner finder worker thread is ready and waiting, submit the new image.
72+
if (!threadGetBusyStatus(gCornerFinderThread)) {
73+
// As corner finding takes longer than a single frame capture, we need to copy the incoming image
74+
// so that OpenCV has exclusive use of it. We copy into cornerFinderData->videoFrame which provides
75+
// the backing for calibImage.
76+
AR2VideoBufferT *buff = vs->checkoutFrameIfNewerThan({0,0});
77+
if (buff) {
78+
CORNER_FINDER_DATA_T *cornerFinderData = (CORNER_FINDER_DATA_T *)threadGetArg(gCornerFinderThread);
79+
memcpy(cornerFinderData->videoFrame, buff->buffLuma, vs->getVideoWidth()*vs->getVideoHeight());
80+
vs->checkinFrame();
81+
82+
// Kick off a new cycle of the cornerFinder. The results will be collected on a subsequent cycle.
83+
ARLOGd("processFrame: corner find GO\n");
84+
threadStartSignal(gCornerFinderThread);
85+
}
86+
}
87+
88+
//
89+
// End of main calibration-related cycle.
90+
//
91+
return true;
92+
}
93+
94+
bool Calibration::cornerFinderResultsLockAndFetch(int *cornerFoundAllFlag, int *cornerCount, CvPoint2D32f **corners, ARUint8** videoFrame)
95+
{
96+
pthread_mutex_lock(&gCornerFinderResultLock);
97+
*cornerFoundAllFlag = gCornerFinderOutputFoundAllFlag;
98+
*cornerCount = gCornerFinderOutputFoundCount;
99+
*corners = gCornerFinderOutputCorners;
100+
*videoFrame = gCornerFinderOutputImage;
101+
return true;
102+
}
103+
104+
bool Calibration::cornerFinderResultsUnlock(void)
105+
{
106+
pthread_mutex_unlock(&gCornerFinderResultLock);
107+
return true;
108+
}
109+
110+
// Worker thread.
111+
void *Calibration::cornerFinder(THREAD_HANDLE_T *threadHandle)
112+
{
113+
#ifdef DEBUG
114+
ARLOGi("Start cornerFinder thread.\n");
115+
#endif
116+
117+
CORNER_FINDER_DATA_T *cornerFinderDataPtr = (CORNER_FINDER_DATA_T *)threadGetArg(threadHandle);
118+
119+
while (threadStartWait(threadHandle) == 0) {
120+
121+
cornerFinderDataPtr->cornerFoundAllFlag = cvFindChessboardCorners(cornerFinderDataPtr->calibImage,
122+
cvSize(cornerFinderDataPtr->chessboardCornerNumY, cornerFinderDataPtr->chessboardCornerNumX),
123+
cornerFinderDataPtr->corners,
124+
&(cornerFinderDataPtr->cornerCount),
125+
CV_CALIB_CB_FAST_CHECK|CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS);
126+
threadEndSignal(threadHandle);
127+
}
128+
129+
#ifdef DEBUG
130+
ARLOGi("End cornerFinder thread.\n");
131+
#endif
132+
return (NULL);
133+
}
134+
135+
bool Calibration::capture()
136+
{
137+
CORNER_FINDER_DATA_T *cornerFinderDataPtr;
138+
CvPoint2D32f *p1, *p2;
139+
int i;
140+
141+
if (m_calibImageCount >= m_calibImageCountMax) return false;
142+
143+
bool saved = false;
144+
145+
pthread_mutex_lock(&gCornerFinderResultLock);
146+
if (gCornerFinderOutputFoundAllFlag) {
147+
// Refine the corner positions.
148+
cvFindCornerSubPix(gCornerFinderOutputImage,
149+
gCornerFinderOutputCorners,
150+
m_chessboardCornerNumX*m_chessboardCornerNumY,
151+
cvSize(5,5),
152+
cvSize(-1,-1),
153+
cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1) );
154+
155+
// Save the corners.
156+
p1 = gCornerFinderOutputCorners;
157+
p2 = &gCorners[m_calibImageCount * m_chessboardCornerNumX * m_chessboardCornerNumY];
158+
for (i = 0; i < m_chessboardCornerNumX * m_chessboardCornerNumY; i++) {
159+
*(p2++) = *(p1++);
160+
}
161+
saved = true;
162+
}
163+
pthread_mutex_unlock(&gCornerFinderResultLock);
164+
165+
if (saved) {
166+
ARLOG("---------- %2d/%2d -----------\n", m_calibImageCount + 1, m_calibImageCountMax);
167+
for (i = 0; i < cornerFinderDataPtr->chessboardCornerNumX*cornerFinderDataPtr->chessboardCornerNumY; i++) {
168+
ARLOG(" %f, %f\n", gCorners[m_calibImageCount*cornerFinderDataPtr->chessboardCornerNumX*cornerFinderDataPtr->chessboardCornerNumY + i].x, gCorners[m_calibImageCount*cornerFinderDataPtr->chessboardCornerNumX*cornerFinderDataPtr->chessboardCornerNumY + i].y);
169+
}
170+
ARLOG("---------- %2d/%2d -----------\n", m_calibImageCount + 1, m_calibImageCountMax);
171+
172+
m_calibImageCount++;
173+
}
174+
175+
return (saved);
176+
}
177+
178+
bool Calibration::uncapture(void)
179+
{
180+
if (m_calibImageCount < 0) return false;
181+
m_calibImageCount--;
182+
return true;
183+
}
184+
185+
void Calibration::calib(ARParam *param_out, ARdouble *err_min_out, ARdouble *err_avg_out, ARdouble *err_max_out)
186+
{
187+
calc(m_calibImageCount, m_chessboardCornerNumX, m_chessboardCornerNumY, m_chessboardSquareWidth, gCorners, m_videoWidth, m_videoHeight, param_out, err_min_out, err_avg_out, err_max_out);
188+
}
189+
190+
Calibration::~Calibration()
191+
{
192+
// Clean up results copy.
193+
// Free space for results.
194+
if (gCornerFinderOutputCorners) {
195+
free(gCornerFinderOutputCorners);
196+
gCornerFinderOutputCorners = NULL;
197+
}
198+
gCornerFinderOutputFoundCount = 0;
199+
gCornerFinderOutputFoundAllFlag = 0;
200+
if (gCornerFinderOutputImage) {
201+
free(gCornerFinderOutputImage);
202+
gCornerFinderOutputImage = NULL;
203+
}
204+
pthread_mutex_destroy(&gCornerFinderResultLock);
205+
206+
// Clean up the corner finder.
207+
if (gCornerFinderThread) {
208+
209+
threadWaitQuit(gCornerFinderThread);
210+
CORNER_FINDER_DATA_T *cornerFinderData = (CORNER_FINDER_DATA_T *)threadGetArg(gCornerFinderThread);
211+
212+
if (cornerFinderData->calibImage) cvReleaseImageHeader(&(cornerFinderData->calibImage));
213+
free(cornerFinderData->videoFrame);
214+
215+
// Free space for results.
216+
if (cornerFinderData->corners) {
217+
free(cornerFinderData->corners);
218+
cornerFinderData->corners = NULL;
219+
}
220+
cornerFinderData->cornerCount = 0;
221+
cornerFinderData->cornerFoundAllFlag = 0;
222+
threadFree(&gCornerFinderThread);
223+
}
224+
225+
// Calibration input cleanup.
226+
free(gCorners);
227+
gCorners = NULL;
228+
}
229+
230+

‎Calibration.hpp‎

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
/*
2+
* Calibration.hpp
3+
* ARToolKit6
4+
*
5+
* Camera calibration utility.
6+
*
7+
* Run with "--help" parameter to see usage.
8+
*
9+
* This file is part of ARToolKit.
10+
*
11+
* ARToolKit is free software: you can redistribute it and/or modify
12+
* it under the terms of the GNU Lesser General Public License as published by
13+
* the Free Software Foundation, either version 3 of the License, or
14+
* (at your option) any later version.
15+
*
16+
* ARToolKit is distributed in the hope that it will be useful,
17+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
18+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19+
* GNU Lesser General Public License for more details.
20+
*
21+
* You should have received a copy of the GNU Lesser General Public License
22+
* along with ARToolKit. If not, see <http://www.gnu.org/licenses/>.
23+
*
24+
* As a special exception, the copyright holders of this library give you
25+
* permission to link this library with independent modules to produce an
26+
* executable, regardless of the license terms of these independent modules, and to
27+
* copy and distribute the resulting executable under terms of your choice,
28+
* provided that you also meet, for each linked independent module, the terms and
29+
* conditions of the license of that module. An independent module is a module
30+
* which is neither derived from nor based on this library. If you modify this
31+
* library, you may extend this exception to your version of the library, but you
32+
* are not obligated to do so. If you do not wish to do so, delete this exception
33+
* statement from your version.
34+
*
35+
* Copyright 2015-2016 Daqri, LLC.
36+
* Copyright 2002-2015 ARToolworks, Inc.
37+
*
38+
* Author(s): Hirokazu Kato, Philip Lamb
39+
*
40+
*/
41+
42+
#pragma once
43+
44+
#include <AR6/AR/ar.h>
45+
#include <opencv2/core/core.hpp>
46+
#include <AR6/ARVideoSource.h>
47+
48+
#include <AR6/ARUtil/thread_sub.h>
49+
50+
class Calibration
51+
{
52+
public:
53+
//Calibration();
54+
bool init(const int calibImageCountMax, const int chessboardCornerNumX, const int chessboardCornerNumY, const int chessboardSquareWidth, const int videoWidth, const int videoHeight);
55+
int calibImageCount() const {return m_calibImageCount; }
56+
int calibImageCountMax() const {return m_calibImageCountMax; }
57+
bool frame(ARVideoSource *vs);
58+
bool cornerFinderResultsLockAndFetch(int *cornerFoundAllFlag, int *cornerCount, CvPoint2D32f **corners, ARUint8** videoFrame);
59+
bool cornerFinderResultsUnlock(void);
60+
bool capture();
61+
bool uncapture();
62+
void calib(ARParam *param_out, ARdouble *err_min_out, ARdouble *err_avg_out, ARdouble *err_max_out);
63+
~Calibration();
64+
65+
private:
66+
67+
static void *cornerFinder(THREAD_HANDLE_T *threadHandle);
68+
69+
typedef struct {
70+
ARUint8* videoFrame;
71+
IplImage *calibImage;
72+
int chessboardCornerNumX;
73+
int chessboardCornerNumY;
74+
int cornerFoundAllFlag;
75+
int cornerCount;
76+
CvPoint2D32f *corners;
77+
} CORNER_FINDER_DATA_T;
78+
79+
// Corner finder.
80+
THREAD_HANDLE_T *gCornerFinderThread = NULL;
81+
pthread_mutex_t gCornerFinderResultLock;
82+
int gCornerFinderOutputFoundAllFlag = 0;
83+
int gCornerFinderOutputFoundCount = 0;
84+
CvPoint2D32f *gCornerFinderOutputCorners = NULL;
85+
ARUint8* gCornerFinderOutputImage = NULL; // The image to which gCornerFinderOutputCorners apply.
86+
87+
// Calibration inputs.
88+
CvPoint2D32f *gCorners = NULL;
89+
int m_calibImageCount;
90+
int m_calibImageCountMax;
91+
int m_chessboardCornerNumX;
92+
int m_chessboardCornerNumY;
93+
int m_chessboardSquareWidth;
94+
int m_videoWidth;
95+
int m_videoHeight;
96+
};

0 commit comments

Comments
 (0)