580 lines
21 KiB
C++
580 lines
21 KiB
C++
/*M///////////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
|
|
//
|
|
// By downloading, copying, installing or using the software you agree to this license.
|
|
// If you do not agree to this license, do not download, install,
|
|
// copy or use the software.
|
|
//
|
|
//
|
|
// Intel License Agreement
|
|
// For Open Source Computer Vision Library
|
|
//
|
|
// Copyright (C) 2002, Intel Corporation, all rights reserved.
|
|
// Third party copyrights are property of their respective owners.
|
|
//
|
|
// Redistribution and use in source and binary forms, with or without modification,
|
|
// are permitted provided that the following conditions are met:
|
|
//
|
|
// * Redistributions of source code must retain the above copyright notice,
|
|
// this list of conditions and the following disclaimer.
|
|
//
|
|
// * Redistributions in binary form must reproduce the above copyright notice,
|
|
// this list of conditions and the following disclaimer in the documentation
|
|
// and/or other materials provided with the distribution.
|
|
//
|
|
// * The name of Intel Corporation may not be used to endorse or promote products
|
|
// derived from this software without specific prior written permission.
|
|
//
|
|
// This software is provided by the copyright holders and contributors "as is" and
|
|
// any express or implied warranties, including, but not limited to, the implied
|
|
// warranties of merchantability and fitness for a particular purpose are disclaimed.
|
|
// In no event shall the Intel Corporation or contributors be liable for any direct,
|
|
// indirect, incidental, special, exemplary, or consequential damages
|
|
// (including, but not limited to, procurement of substitute goods or services;
|
|
// loss of use, data, or profits; or business interruption) however caused
|
|
// and on any theory of liability, whether in contract, strict liability,
|
|
// or tort (including negligence or otherwise) arising in any way out of
|
|
// the use of this software, even if advised of the possibility of such damage.
|
|
//
|
|
//M*/
|
|
|
|
#include "precomp.hpp"
|
|
|
|
#include <stdio.h>
|
|
#include <map>
|
|
#include <algorithm>
|
|
|
|
#define ARRAY_SIZEOF(a) (sizeof(a)/sizeof((a)[0]))
|
|
|
|
static void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size);
|
|
static void DrawEtalon(IplImage *img, CvPoint2D32f *corners,
|
|
int corner_count, CvSize etalon_size, int draw_ordered);
|
|
static void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4]);
|
|
static void MultVectorMatrix(float rv[4], const float v[4], const float m[4][4]);
|
|
static CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint2D32f p);
|
|
static bool intersection(CvPoint3D32f o1, CvPoint3D32f p1,
|
|
CvPoint3D32f o2, CvPoint3D32f p2,
|
|
CvPoint3D32f &r1, CvPoint3D32f &r2);
|
|
|
|
/////////////////////////////////
|
|
// cv3dTrackerCalibrateCameras //
|
|
/////////////////////////////////
|
|
CV_IMPL CvBool cv3dTrackerCalibrateCameras(int num_cameras,
|
|
const Cv3dTrackerCameraIntrinsics camera_intrinsics[], // size is num_cameras
|
|
CvSize etalon_size,
|
|
float square_size,
|
|
IplImage *samples[], // size is num_cameras
|
|
Cv3dTrackerCameraInfo camera_info[]) // size is num_cameras
|
|
{
|
|
CV_FUNCNAME("cv3dTrackerCalibrateCameras");
|
|
const int num_points = etalon_size.width * etalon_size.height;
|
|
int cameras_done = 0; // the number of cameras whose positions have been determined
|
|
CvPoint3D32f *object_points = NULL; // real-world coordinates of checkerboard points
|
|
CvPoint2D32f *points = NULL; // 2d coordinates of checkerboard points as seen by a camera
|
|
IplImage *gray_img = NULL; // temporary image for color conversion
|
|
IplImage *tmp_img = NULL; // temporary image used by FindChessboardCornerGuesses
|
|
int c, i, j;
|
|
|
|
if (etalon_size.width < 3 || etalon_size.height < 3)
|
|
CV_ERROR(CV_StsBadArg, "Chess board size is invalid");
|
|
|
|
for (c = 0; c < num_cameras; c++)
|
|
{
|
|
// CV_CHECK_IMAGE is not available in the cvaux library
|
|
// so perform the checks inline.
|
|
|
|
//CV_CALL(CV_CHECK_IMAGE(samples[c]));
|
|
|
|
if( samples[c] == NULL )
|
|
CV_ERROR( CV_HeaderIsNull, "Null image" );
|
|
|
|
if( samples[c]->dataOrder != IPL_DATA_ORDER_PIXEL && samples[c]->nChannels > 1 )
|
|
CV_ERROR( CV_BadOrder, "Unsupported image format" );
|
|
|
|
if( samples[c]->maskROI != 0 || samples[c]->tileInfo != 0 )
|
|
CV_ERROR( CV_StsBadArg, "Unsupported image format" );
|
|
|
|
if( samples[c]->imageData == 0 )
|
|
CV_ERROR( CV_BadDataPtr, "Null image data" );
|
|
|
|
if( samples[c]->roi &&
|
|
((samples[c]->roi->xOffset | samples[c]->roi->yOffset
|
|
| samples[c]->roi->width | samples[c]->roi->height) < 0 ||
|
|
samples[c]->roi->xOffset + samples[c]->roi->width > samples[c]->width ||
|
|
samples[c]->roi->yOffset + samples[c]->roi->height > samples[c]->height ||
|
|
(unsigned) (samples[c]->roi->coi) > (unsigned) (samples[c]->nChannels)))
|
|
CV_ERROR( CV_BadROISize, "Invalid ROI" );
|
|
|
|
// End of CV_CHECK_IMAGE inline expansion
|
|
|
|
if (samples[c]->depth != IPL_DEPTH_8U)
|
|
CV_ERROR(CV_BadDepth, "Channel depth of source image must be 8");
|
|
|
|
if (samples[c]->nChannels != 3 && samples[c]->nChannels != 1)
|
|
CV_ERROR(CV_BadNumChannels, "Source image must have 1 or 3 channels");
|
|
}
|
|
|
|
CV_CALL(object_points = (CvPoint3D32f *)cvAlloc(num_points * sizeof(CvPoint3D32f)));
|
|
CV_CALL(points = (CvPoint2D32f *)cvAlloc(num_points * sizeof(CvPoint2D32f)));
|
|
|
|
// fill in the real-world coordinates of the checkerboard points
|
|
FillObjectPoints(object_points, etalon_size, square_size);
|
|
|
|
for (c = 0; c < num_cameras; c++)
|
|
{
|
|
CvSize image_size = cvSize(samples[c]->width, samples[c]->height);
|
|
IplImage *img;
|
|
|
|
// The input samples are not required to all have the same size or color
|
|
// format. If they have different sizes, the temporary images are
|
|
// reallocated as necessary.
|
|
if (samples[c]->nChannels == 3)
|
|
{
|
|
// convert to gray
|
|
if (gray_img == NULL || gray_img->width != samples[c]->width ||
|
|
gray_img->height != samples[c]->height )
|
|
{
|
|
if (gray_img != NULL)
|
|
cvReleaseImage(&gray_img);
|
|
CV_CALL(gray_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
|
|
}
|
|
|
|
CV_CALL(cvCvtColor(samples[c], gray_img, CV_BGR2GRAY));
|
|
|
|
img = gray_img;
|
|
}
|
|
else
|
|
{
|
|
// no color conversion required
|
|
img = samples[c];
|
|
}
|
|
|
|
if (tmp_img == NULL || tmp_img->width != samples[c]->width ||
|
|
tmp_img->height != samples[c]->height )
|
|
{
|
|
if (tmp_img != NULL)
|
|
cvReleaseImage(&tmp_img);
|
|
CV_CALL(tmp_img = cvCreateImage(image_size, IPL_DEPTH_8U, 1));
|
|
}
|
|
|
|
int count = num_points;
|
|
bool found = cvFindChessBoardCornerGuesses(img, tmp_img, 0,
|
|
etalon_size, points, &count) != 0;
|
|
if (count == 0)
|
|
continue;
|
|
|
|
// If found is true, it means all the points were found (count = num_points).
|
|
// If found is false but count is non-zero, it means that not all points were found.
|
|
|
|
cvFindCornerSubPix(img, points, count, cvSize(5,5), cvSize(-1,-1),
|
|
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 10, 0.01f));
|
|
|
|
// If the image origin is BL (bottom-left), fix the y coordinates
|
|
// so they are relative to the true top of the image.
|
|
if (samples[c]->origin == IPL_ORIGIN_BL)
|
|
{
|
|
for (i = 0; i < count; i++)
|
|
points[i].y = samples[c]->height - 1 - points[i].y;
|
|
}
|
|
|
|
if (found)
|
|
{
|
|
// Make sure x coordinates are increasing and y coordinates are decreasing.
|
|
// (The y coordinate of point (0,0) should be the greatest, because the point
|
|
// on the checkerboard that is the origin is nearest the bottom of the image.)
|
|
// This is done after adjusting the y coordinates according to the image origin.
|
|
if (points[0].x > points[1].x)
|
|
{
|
|
// reverse points in each row
|
|
for (j = 0; j < etalon_size.height; j++)
|
|
{
|
|
CvPoint2D32f *row = &points[j*etalon_size.width];
|
|
for (i = 0; i < etalon_size.width/2; i++)
|
|
std::swap(row[i], row[etalon_size.width-i-1]);
|
|
}
|
|
}
|
|
|
|
if (points[0].y < points[etalon_size.width].y)
|
|
{
|
|
// reverse points in each column
|
|
for (i = 0; i < etalon_size.width; i++)
|
|
{
|
|
for (j = 0; j < etalon_size.height/2; j++)
|
|
std::swap(points[i+j*etalon_size.width],
|
|
points[i+(etalon_size.height-j-1)*etalon_size.width]);
|
|
}
|
|
}
|
|
}
|
|
|
|
DrawEtalon(samples[c], points, count, etalon_size, found);
|
|
|
|
if (!found)
|
|
continue;
|
|
|
|
float rotVect[3];
|
|
float rotMatr[9];
|
|
float transVect[3];
|
|
|
|
cvFindExtrinsicCameraParams(count,
|
|
image_size,
|
|
points,
|
|
object_points,
|
|
const_cast<float *>(camera_intrinsics[c].focal_length),
|
|
camera_intrinsics[c].principal_point,
|
|
const_cast<float *>(camera_intrinsics[c].distortion),
|
|
rotVect,
|
|
transVect);
|
|
|
|
// Check result against an arbitrary limit to eliminate impossible values.
|
|
// (If the chess board were truly that far away, the camera wouldn't be able to
|
|
// see the squares.)
|
|
if (transVect[0] > 1000*square_size
|
|
|| transVect[1] > 1000*square_size
|
|
|| transVect[2] > 1000*square_size)
|
|
{
|
|
// ignore impossible results
|
|
continue;
|
|
}
|
|
|
|
CvMat rotMatrDescr = cvMat(3, 3, CV_32FC1, rotMatr);
|
|
CvMat rotVectDescr = cvMat(3, 1, CV_32FC1, rotVect);
|
|
|
|
/* Calc rotation matrix by Rodrigues Transform */
|
|
cvRodrigues2( &rotVectDescr, &rotMatrDescr );
|
|
|
|
//combine the two transformations into one matrix
|
|
//order is important! rotations are not commutative
|
|
float tmat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
|
|
{ 0.f, 1.f, 0.f, 0.f },
|
|
{ 0.f, 0.f, 1.f, 0.f },
|
|
{ transVect[0], transVect[1], transVect[2], 1.f } };
|
|
|
|
float rmat[4][4] = { { rotMatr[0], rotMatr[1], rotMatr[2], 0.f },
|
|
{ rotMatr[3], rotMatr[4], rotMatr[5], 0.f },
|
|
{ rotMatr[6], rotMatr[7], rotMatr[8], 0.f },
|
|
{ 0.f, 0.f, 0.f, 1.f } };
|
|
|
|
|
|
MultMatrix(camera_info[c].mat, tmat, rmat);
|
|
|
|
// change the transformation of the cameras to put them in the world coordinate
|
|
// system we want to work with.
|
|
|
|
// Start with an identity matrix; then fill in the values to accomplish
|
|
// the desired transformation.
|
|
float smat[4][4] = { { 1.f, 0.f, 0.f, 0.f },
|
|
{ 0.f, 1.f, 0.f, 0.f },
|
|
{ 0.f, 0.f, 1.f, 0.f },
|
|
{ 0.f, 0.f, 0.f, 1.f } };
|
|
|
|
// First, reflect through the origin by inverting all three axes.
|
|
smat[0][0] = -1.f;
|
|
smat[1][1] = -1.f;
|
|
smat[2][2] = -1.f;
|
|
MultMatrix(tmat, camera_info[c].mat, smat);
|
|
|
|
// Scale x and y coordinates by the focal length (allowing for non-square pixels
|
|
// and/or non-symmetrical lenses).
|
|
smat[0][0] = 1.0f / camera_intrinsics[c].focal_length[0];
|
|
smat[1][1] = 1.0f / camera_intrinsics[c].focal_length[1];
|
|
smat[2][2] = 1.0f;
|
|
MultMatrix(camera_info[c].mat, smat, tmat);
|
|
|
|
camera_info[c].principal_point = camera_intrinsics[c].principal_point;
|
|
camera_info[c].valid = true;
|
|
|
|
cameras_done++;
|
|
}
|
|
|
|
exit:
|
|
cvReleaseImage(&gray_img);
|
|
cvReleaseImage(&tmp_img);
|
|
cvFree(&object_points);
|
|
cvFree(&points);
|
|
|
|
return cameras_done == num_cameras;
|
|
}
|
|
|
|
// fill in the real-world coordinates of the checkerboard points
|
|
static void FillObjectPoints(CvPoint3D32f *obj_points, CvSize etalon_size, float square_size)
|
|
{
|
|
int x, y, i;
|
|
|
|
for (y = 0, i = 0; y < etalon_size.height; y++)
|
|
{
|
|
for (x = 0; x < etalon_size.width; x++, i++)
|
|
{
|
|
obj_points[i].x = square_size * x;
|
|
obj_points[i].y = square_size * y;
|
|
obj_points[i].z = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// Mark the points found on the input image
|
|
// The marks are drawn multi-colored if all the points were found.
|
|
static void DrawEtalon(IplImage *img, CvPoint2D32f *corners,
|
|
int corner_count, CvSize etalon_size, int draw_ordered)
|
|
{
|
|
const int r = 4;
|
|
int i;
|
|
int x, y;
|
|
CvPoint prev_pt = { 0, 0 };
|
|
static const CvScalar rgb_colors[] = {
|
|
{{0,0,255}},
|
|
{{0,128,255}},
|
|
{{0,200,200}},
|
|
{{0,255,0}},
|
|
{{200,200,0}},
|
|
{{255,0,0}},
|
|
{{255,0,255}} };
|
|
static const CvScalar gray_colors[] = {
|
|
{{80}}, {{120}}, {{160}}, {{200}}, {{100}}, {{140}}, {{180}}
|
|
};
|
|
const CvScalar* colors = img->nChannels == 3 ? rgb_colors : gray_colors;
|
|
|
|
CvScalar color = colors[0];
|
|
for (y = 0, i = 0; y < etalon_size.height; y++)
|
|
{
|
|
if (draw_ordered)
|
|
color = colors[y % ARRAY_SIZEOF(rgb_colors)];
|
|
|
|
for (x = 0; x < etalon_size.width && i < corner_count; x++, i++)
|
|
{
|
|
CvPoint pt;
|
|
pt.x = cvRound(corners[i].x);
|
|
pt.y = cvRound(corners[i].y);
|
|
if (img->origin == IPL_ORIGIN_BL)
|
|
pt.y = img->height - 1 - pt.y;
|
|
|
|
if (draw_ordered)
|
|
{
|
|
if (i != 0)
|
|
cvLine(img, prev_pt, pt, color, 1, CV_AA);
|
|
prev_pt = pt;
|
|
}
|
|
|
|
cvLine( img, cvPoint(pt.x - r, pt.y - r),
|
|
cvPoint(pt.x + r, pt.y + r), color, 1, CV_AA );
|
|
cvLine( img, cvPoint(pt.x - r, pt.y + r),
|
|
cvPoint(pt.x + r, pt.y - r), color, 1, CV_AA );
|
|
cvCircle( img, pt, r+1, color, 1, CV_AA );
|
|
}
|
|
}
|
|
}
|
|
|
|
// Find the midpoint of the line segment between two points.
|
|
static CvPoint3D32f midpoint(const CvPoint3D32f &p1, const CvPoint3D32f &p2)
|
|
{
|
|
return cvPoint3D32f((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2);
|
|
}
|
|
|
|
static void operator +=(CvPoint3D32f &p1, const CvPoint3D32f &p2)
|
|
{
|
|
p1.x += p2.x;
|
|
p1.y += p2.y;
|
|
p1.z += p2.z;
|
|
}
|
|
|
|
static CvPoint3D32f operator /(const CvPoint3D32f &p, int d)
|
|
{
|
|
return cvPoint3D32f(p.x/d, p.y/d, p.z/d);
|
|
}
|
|
|
|
static const Cv3dTracker2dTrackedObject *find(const Cv3dTracker2dTrackedObject v[], int num_objects, int id)
|
|
{
|
|
for (int i = 0; i < num_objects; i++)
|
|
{
|
|
if (v[i].id == id)
|
|
return &v[i];
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
#define CAMERA_POS(c) (cvPoint3D32f((c).mat[3][0], (c).mat[3][1], (c).mat[3][2]))
|
|
|
|
//////////////////////////////
|
|
// cv3dTrackerLocateObjects //
|
|
//////////////////////////////
|
|
CV_IMPL int cv3dTrackerLocateObjects(int num_cameras, int num_objects,
|
|
const Cv3dTrackerCameraInfo camera_info[], // size is num_cameras
|
|
const Cv3dTracker2dTrackedObject tracking_info[], // size is num_objects*num_cameras
|
|
Cv3dTrackerTrackedObject tracked_objects[]) // size is num_objects
|
|
{
|
|
/*CV_FUNCNAME("cv3dTrackerLocateObjects");*/
|
|
int found_objects = 0;
|
|
|
|
// count how many cameras could see each object
|
|
std::map<int, int> count;
|
|
for (int c = 0; c < num_cameras; c++)
|
|
{
|
|
if (!camera_info[c].valid)
|
|
continue;
|
|
|
|
for (int i = 0; i < num_objects; i++)
|
|
{
|
|
const Cv3dTracker2dTrackedObject *o = &tracking_info[c*num_objects+i];
|
|
if (o->id != -1)
|
|
count[o->id]++;
|
|
}
|
|
}
|
|
|
|
// process each object that was seen by at least two cameras
|
|
for (std::map<int, int>::iterator i = count.begin(); i != count.end(); i++)
|
|
{
|
|
if (i->second < 2)
|
|
continue; // ignore object seen by only one camera
|
|
int id = i->first;
|
|
|
|
// find an approximation of the objects location for each pair of cameras that
|
|
// could see this object, and average them
|
|
CvPoint3D32f total = cvPoint3D32f(0, 0, 0);
|
|
int weight = 0;
|
|
|
|
for (int c1 = 0; c1 < num_cameras-1; c1++)
|
|
{
|
|
if (!camera_info[c1].valid)
|
|
continue;
|
|
|
|
const Cv3dTracker2dTrackedObject *o1 = find(&tracking_info[c1*num_objects],
|
|
num_objects, id);
|
|
if (o1 == NULL)
|
|
continue; // this camera didn't see this object
|
|
|
|
CvPoint3D32f p1a = CAMERA_POS(camera_info[c1]);
|
|
CvPoint3D32f p1b = ImageCStoWorldCS(camera_info[c1], o1->p);
|
|
|
|
for (int c2 = c1 + 1; c2 < num_cameras; c2++)
|
|
{
|
|
if (!camera_info[c2].valid)
|
|
continue;
|
|
|
|
const Cv3dTracker2dTrackedObject *o2 = find(&tracking_info[c2*num_objects],
|
|
num_objects, id);
|
|
if (o2 == NULL)
|
|
continue; // this camera didn't see this object
|
|
|
|
CvPoint3D32f p2a = CAMERA_POS(camera_info[c2]);
|
|
CvPoint3D32f p2b = ImageCStoWorldCS(camera_info[c2], o2->p);
|
|
|
|
// these variables are initialized simply to avoid erroneous error messages
|
|
// from the compiler
|
|
CvPoint3D32f r1 = cvPoint3D32f(0, 0, 0);
|
|
CvPoint3D32f r2 = cvPoint3D32f(0, 0, 0);
|
|
|
|
// find the intersection of the two lines (or the points of closest
|
|
// approach, if they don't intersect)
|
|
if (!intersection(p1a, p1b, p2a, p2b, r1, r2))
|
|
continue;
|
|
|
|
total += midpoint(r1, r2);
|
|
weight++;
|
|
}
|
|
}
|
|
|
|
CvPoint3D32f center = total/weight;
|
|
tracked_objects[found_objects++] = cv3dTrackerTrackedObject(id, center);
|
|
}
|
|
|
|
return found_objects;
|
|
}
|
|
|
|
#define EPS 1e-9
|
|
|
|
// Compute the determinant of the 3x3 matrix represented by 3 row vectors.
|
|
static inline double det(CvPoint3D32f v1, CvPoint3D32f v2, CvPoint3D32f v3)
|
|
{
|
|
return v1.x*v2.y*v3.z + v1.z*v2.x*v3.y + v1.y*v2.z*v3.x
|
|
- v1.z*v2.y*v3.x - v1.x*v2.z*v3.y - v1.y*v2.x*v3.z;
|
|
}
|
|
|
|
static CvPoint3D32f operator +(CvPoint3D32f a, CvPoint3D32f b)
|
|
{
|
|
return cvPoint3D32f(a.x + b.x, a.y + b.y, a.z + b.z);
|
|
}
|
|
|
|
static CvPoint3D32f operator -(CvPoint3D32f a, CvPoint3D32f b)
|
|
{
|
|
return cvPoint3D32f(a.x - b.x, a.y - b.y, a.z - b.z);
|
|
}
|
|
|
|
static CvPoint3D32f operator *(CvPoint3D32f v, double f)
|
|
{
|
|
return cvPoint3D32f(f*v.x, f*v.y, f*v.z);
|
|
}
|
|
|
|
|
|
// Find the intersection of two lines, or if they don't intersect,
|
|
// the points of closest approach.
|
|
// The lines are defined by (o1,p1) and (o2, p2).
|
|
// If they intersect, r1 and r2 will be the same.
|
|
// Returns false on error.
|
|
static bool intersection(CvPoint3D32f o1, CvPoint3D32f p1,
|
|
CvPoint3D32f o2, CvPoint3D32f p2,
|
|
CvPoint3D32f &r1, CvPoint3D32f &r2)
|
|
{
|
|
CvPoint3D32f x = o2 - o1;
|
|
CvPoint3D32f d1 = p1 - o1;
|
|
CvPoint3D32f d2 = p2 - o2;
|
|
|
|
CvPoint3D32f cross = cvPoint3D32f(d1.y*d2.z - d1.z*d2.y,
|
|
d1.z*d2.x - d1.x*d2.z,
|
|
d1.x*d2.y - d1.y*d2.x);
|
|
double den = cross.x*cross.x + cross.y*cross.y + cross.z*cross.z;
|
|
|
|
if (den < EPS)
|
|
return false;
|
|
|
|
double t1 = det(x, d2, cross) / den;
|
|
double t2 = det(x, d1, cross) / den;
|
|
|
|
r1 = o1 + d1 * t1;
|
|
r2 = o2 + d2 * t2;
|
|
|
|
return true;
|
|
}
|
|
|
|
// Convert from image to camera space by transforming point p in
|
|
// the image plane by the camera matrix.
|
|
static CvPoint3D32f ImageCStoWorldCS(const Cv3dTrackerCameraInfo &camera_info, CvPoint2D32f p)
|
|
{
|
|
float tp[4];
|
|
tp[0] = (float)p.x - camera_info.principal_point.x;
|
|
tp[1] = (float)p.y - camera_info.principal_point.y;
|
|
tp[2] = 1.f;
|
|
tp[3] = 1.f;
|
|
|
|
float tr[4];
|
|
//multiply tp by mat to get tr
|
|
MultVectorMatrix(tr, tp, camera_info.mat);
|
|
|
|
return cvPoint3D32f(tr[0]/tr[3], tr[1]/tr[3], tr[2]/tr[3]);
|
|
}
|
|
|
|
// Multiply affine transformation m1 by the affine transformation m2 and
|
|
// return the result in rm.
|
|
static void MultMatrix(float rm[4][4], const float m1[4][4], const float m2[4][4])
|
|
{
|
|
for (int i=0; i<=3; i++)
|
|
for (int j=0; j<=3; j++)
|
|
{
|
|
rm[i][j]= 0.0;
|
|
for (int k=0; k <= 3; k++)
|
|
rm[i][j] += m1[i][k]*m2[k][j];
|
|
}
|
|
}
|
|
|
|
// Multiply the vector v by the affine transformation matrix m and return the
|
|
// result in rv.
|
|
void MultVectorMatrix(float rv[4], const float v[4], const float m[4][4])
|
|
{
|
|
for (int i=0; i<=3; i++)
|
|
{
|
|
rv[i] = 0.f;
|
|
for (int j=0;j<=3;j++)
|
|
rv[i] += v[j] * m[j][i];
|
|
}
|
|
}
|