hesuicong
/
openMVS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
34 Commits
1 Branch
0 Tags
13 MiB
 
 
 
 
 
 
Tree: d4b269d095
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'd4b269d095'
${ noResults }
openMVS/apps/InterfaceVisualSFM/DataInterface.h

383 lines
14 KiB
Raw Blame History

////////////////////////////////////////////////////////////////////////////
// File: DataInterface.h
// Author: Changchang Wu (ccwu@cs.washington.edu)
// Description : data interface, the data format been uploaded to GPU
//
// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu)
// and the University of Washington at Seattle
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public
// License as published by the Free Software Foundation; either
// Version 3 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
// General Public License for more details.
//
////////////////////////////////////////////////////////////////////////////////
#ifndef DATA_INTERFACE_GPU_H
#define DATA_INTERFACE_GPU_H
namespace PBA {
// ----------------------------WARNING------------------------------
// -----------------------------------------------------------------
// ROTATION CONVERSION:
// The internal rotation representation is 3x3 float matrix. Reading
// back the rotations as quaternion or Rodrigues's representation will
// cause inaccuracy, IF you have wrongly reconstructed cameras with
// a very very large focal length (typically also very far away).
// In this case, any small change in the rotation matrix, will cause
// a large reprojection error.
//
// ---------------------------------------------------------------------
// RADIAL distortion is NOT enabled by default, use parameter "-md", -pd"
// or set ConfigBA::__use_radial_distortion to 1 or -1 to enable it.
// ---------------------------------------------------------------------------
//transfer data type with 4-float alignment
#define CameraT CameraT_
#define Point3D Point3D_
template<class FT>
struct CameraT_
{
typedef FT float_t;
//////////////////////////////////////////////////////
float_t f; // single focal length, K = [f, 0, 0; 0 f 0; 0 0 1]
float_t t[3]; // T in P = K[R T], T = - RC
float_t m[3][3]; // R in P = K[R T].
float_t radial; // WARNING: BE careful with the radial distortion model.
float_t distortion_type;
float_t constant_camera;
//////////////////////////////////////////////////////////
CameraT_() { radial = 0; distortion_type = 0; constant_camera = 0; }
//////////////////////////////////////////////
template <class CameraX> void SetCameraT(const CameraX & cam)
{
f = (float_t)cam.f;
t[0] = (float_t)cam.t[0]; t[1] = (float_t)cam.t[1]; t[2] = (float_t)cam.t[2];
for(int i = 0; i < 3; ++i) for(int j = 0; j < 3; ++j) m[i][j] = (float_t)cam.m[i][j];
radial = (float_t)cam.radial;
distortion_type = (float_t)cam.distortion_type;
constant_camera = (float_t)cam.constant_camera;
}
//////////////////////////////////////////
enum {
CAMERA_VARIABLE = 0,
CAMERA_FIXEDINTRINSIC = (1<<0),
CAMERA_FIXEDEXTRINSIC = (1<<1),
};
void SetVariableCamera() {(int&)constant_camera = CAMERA_VARIABLE;}
void SetFixedIntrinsic() {(int&)constant_camera = CAMERA_FIXEDINTRINSIC;}
void SetFixedExtrinsic() {(int&)constant_camera = CAMERA_FIXEDEXTRINSIC;}
void SetConstantCamera() {(int&)constant_camera = CAMERA_FIXEDINTRINSIC|CAMERA_FIXEDEXTRINSIC;}
//////////////////////////////////////
template <class Float> void SetFocalLength(Float F){ f = (float_t) F; }
float_t GetFocalLength() const{return f;}
template <class Float> void SetMeasurementDistortion(Float r) {radial = (float_t) r; distortion_type = -1;}
float_t GetMeasurementDistortion() const {return distortion_type == -1 ? radial : 0; }
//normalize radial distortion that applies to angle will be (radial * f * f);
template <class Float> void SetNormalizedMeasurementDistortion(Float r) {SetMeasurementDistortion(r / (f * f)); }
float_t GetNormalizedMeasurementDistortion() const{return GetMeasurementDistortion() * (f * f); }
//use projection distortion
template <class Float> void SetProjectionDistortion(Float r) {radial = float_t(r); distortion_type = 1; }
template <class Float> void SetProjectionDistortion(const Float* r) {SetProjectionDistortion(r[0]); }
float_t GetProjectionDistortion() {return distortion_type == 1 ? radial : 0; }
template <class Float> void SetRodriguesRotation(const Float r[3])
{
double a = sqrt(r[0]*r[0]+r[1]*r[1]+r[2]*r[2]);
double ct = a==0.0?0.5:(1.0-cos(a))/a/a;
double st = a==0.0?1:sin(a)/a;
m[0][0]=float_t(1.0 - (r[1]*r[1] + r[2]*r[2])*ct);
m[0][1]=float_t(r[0]*r[1]*ct - r[2]*st);
m[0][2]=float_t(r[2]*r[0]*ct + r[1]*st);
m[1][0]=float_t(r[0]*r[1]*ct + r[2]*st);
m[1][1]=float_t(1.0 - (r[2]*r[2] + r[0]*r[0])*ct);
m[1][2]=float_t(r[1]*r[2]*ct - r[0]*st);
m[2][0]=float_t(r[2]*r[0]*ct - r[1]*st);
m[2][1]=float_t(r[1]*r[2]*ct + r[0]*st);
m[2][2]=float_t(1.0 - (r[0]*r[0] + r[1]*r[1])*ct );
}
template <class Float> void GetRodriguesRotation(Float r[3]) const
{
double a = (m[0][0]+m[1][1]+m[2][2]-1.0)/2.0;
const double epsilon = 0.01;
if( fabs(m[0][1] - m[1][0]) < epsilon &&
fabs(m[1][2] - m[2][1]) < epsilon &&
fabs(m[0][2] - m[2][0]) < epsilon )
{
if( fabs(m[0][1] + m[1][0]) < 0.1 &&
fabs(m[1][2] + m[2][1]) < 0.1 &&
fabs(m[0][2] + m[2][0]) < 0.1 && a > 0.9)
{
r[0] = 0;
r[1] = 0;
r[2] = 0;
}
else
{
const Float ha = Float(sqrt(0.5) * 3.14159265358979323846);
double xx = (m[0][0]+1.0)/2.0;
double yy = (m[1][1]+1.0)/2.0;
double zz = (m[2][2]+1.0)/2.0;
double xy = (m[0][1]+m[1][0])/4.0;
double xz = (m[0][2]+m[2][0])/4.0;
double yz = (m[1][2]+m[2][1])/4.0;
if ((xx > yy) && (xx > zz))
{
if (xx< epsilon)
{
r[0] = 0; r[1] = r[2] = ha;
} else
{
double t = sqrt(xx) ;
r[0] = Float(t * 3.14159265358979323846);
r[1] = Float(xy/t * 3.14159265358979323846);
r[2] = Float(xz/t * 3.14159265358979323846);
}
} else if (yy > zz)
{
if (yy< epsilon)
{
r[0] = r[2] = ha; r[1] = 0;
} else
{
double t = sqrt(yy);
r[0] = Float(xy/t* 3.14159265358979323846);
r[1] = Float( t * 3.14159265358979323846);
r[2] = Float(yz/t* 3.14159265358979323846);
}
} else
{
if (zz< epsilon)
{
r[0] = r[1] = ha; r[2] = 0;
} else
{
double t = sqrt(zz);
r[0] = Float(xz/ t* 3.14159265358979323846);
r[1] = Float(yz/ t* 3.14159265358979323846);
r[2] = Float( t * 3.14159265358979323846);
}
}
}
}
else
{
a = acos(a);
double b = 0.5*a/sin(a);
r[0] = Float(b*(m[2][1]-m[1][2]));
r[1] = Float(b*(m[0][2]-m[2][0]));
r[2] = Float(b*(m[1][0]-m[0][1]));
}
}
////////////////////////
template <class Float> void SetQuaternionRotation(const Float q[4])
{
double qq = sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
double qw, qx, qy, qz;
if(qq>0)
{
qw=q[0]/qq;
qx=q[1]/qq;
qy=q[2]/qq;
qz=q[3]/qq;
}else
{
qw = 1;
qx = qy = qz = 0;
}
m[0][0]=float_t(qw*qw + qx*qx- qz*qz- qy*qy );
m[0][1]=float_t(2*qx*qy -2*qz*qw );
m[0][2]=float_t(2*qy*qw + 2*qz*qx);
m[1][0]=float_t(2*qx*qy+ 2*qw*qz);
m[1][1]=float_t(qy*qy+ qw*qw - qz*qz- qx*qx);
m[1][2]=float_t(2*qz*qy- 2*qx*qw);
m[2][0]=float_t(2*qx*qz- 2*qy*qw);
m[2][1]=float_t(2*qy*qz + 2*qw*qx );
m[2][2]=float_t(qz*qz+ qw*qw- qy*qy- qx*qx);
}
template <class Float> void GetQuaternionRotation(Float q[4]) const
{
q[0]= 1 + m[0][0] + m[1][1] + m[2][2];
if(q[0]>0.000000001)
{
q[0] = sqrt(q[0])/2.0;
q[1]= (m[2][1] - m[1][2])/( 4.0 *q[0]);
q[2]= (m[0][2] - m[2][0])/( 4.0 *q[0]);
q[3]= (m[1][0] - m[0][1])/( 4.0 *q[0]);
}else
{
double s;
if ( m[0][0] > m[1][1] && m[0][0] > m[2][2] )
{
s = 2.0 * sqrt( 1.0 + m[0][0] - m[1][1] - m[2][2]);
q[1] = 0.25 * s;
q[2] = (m[0][1] + m[1][0] ) / s;
q[3] = (m[0][2] + m[2][0] ) / s;
q[0] = (m[1][2] - m[2][1] ) / s;
} else if (m[1][1] > m[2][2])
{
s = 2.0 * sqrt( 1.0 + m[1][1] - m[0][0] - m[2][2]);
q[1] = (m[0][1] + m[1][0] ) / s;
q[2] = 0.25 * s;
q[3] = (m[1][2] + m[2][1] ) / s;
q[0] = (m[0][2] - m[2][0] ) / s;
} else
{
s = 2.0 * sqrt( 1.0 + m[2][2] - m[0][0] - m[1][1]);
q[1] = (m[0][2] + m[2][0] ) / s;
q[2] = (m[1][2] + m[2][1] ) / s;
q[3] = 0.25f * s;
q[0] = (m[0][1] - m[1][0] ) / s;
}
}
}
////////////////////////////////////////////////
template <class Float> void SetMatrixRotation(const Float * r)
{
for(int i = 0; i < 9; ++i) m[0][i] = float_t(r[i]);
}
template <class Float> void GetMatrixRotation(Float * r) const
{
for(int i = 0; i < 9; ++i) r[i] = Float(m[0][i]);
}
float GetRotationMatrixDeterminant()const
{
return m[0][0]*m[1][1]*m[2][2] +
m[0][1]*m[1][2]*m[2][0] +
m[0][2]*m[1][0]*m[2][1] -
m[0][2]*m[1][1]*m[2][0] -
m[0][1]*m[1][0]*m[2][2] -
m[0][0]*m[1][2]*m[2][1];
}
///////////////////////////////////////
template <class Float> void SetTranslation(const Float T[3])
{
t[0] = (float_t)T[0];
t[1] = (float_t)T[1];
t[2] = (float_t)T[2];
}
template <class Float> void GetTranslation(Float T[3]) const
{
T[0] = (Float)t[0];
T[1] = (Float)t[1];
T[2] = (Float)t[2];
}
/////////////////////////////////////////////
template <class Float> void SetCameraCenterAfterRotation(const Float c[3])
{
//t = - R * C
for(int j = 0; j < 3; ++j) t[j] = -float_t(double(m[j][0])*double(c[0]) + double(m[j][1])*double(c[1]) + double(m[j][2])*double(c[2]));
}
template <class Float> void GetCameraCenter(Float c[3]) const
{
//C = - R' * t
for(int j = 0; j < 3; ++j) c[j] = -Float(double(m[0][j])*double(t[0]) + double(m[1][j])*double(t[1]) + double(m[2][j])*double(t[2]));
}
////////////////////////////////////////////
template <class Float> void SetInvertedRT(const Float e[3], const Float T[3])
{
SetRodriguesRotation(e);
for(int i = 3; i < 9; ++i) m[0][i] = - m[0][i];
SetTranslation(T); t[1] = - t[1]; t[2] = -t[2];
}
template <class Float> void GetInvertedRT (Float e[3], Float T[3]) const
{
CameraT ci; ci.SetMatrixRotation(m[0]);
for(int i = 3; i < 9; ++i) ci.m[0][i] = - ci.m[0][i];
//for(int i = 1; i < 3; ++i) for(int j = 0; j < 3; ++j) ci.m[i][j] = - ci.m[i][j];
ci.GetRodriguesRotation(e);
GetTranslation(T); T[1] = - T[1]; T[2] = -T[2];
}
template <class Float> void SetInvertedR9T(const Float e[9], const Float T[3])
{
//for(int i = 0; i < 9; ++i) m[0][i] = (i < 3 ? e[i] : - e[i]);
//SetTranslation(T); t[1] = - t[1]; t[2] = -t[2];
m[0][0] = e[0]; m[0][1] = e[1]; m[0][2] = e[2];
m[1][0] = -e[3]; m[1][1] = -e[4]; m[1][2] = -e[5];
m[2][0] = -e[6]; m[2][1] = -e[7]; m[2][2] = -e[8];
t[0] = T[0]; t[1] = -T[1]; t[2] = -T[2];
}
template<class Float> void GetInvertedR9T(Float e[9], Float T[3]) const
{
e[0] = m[0][0]; e[1] = m[0][1]; e[2] = m[0][2];
e[3] = - m[1][0]; e[4] = -m[1][1]; e[5] = -m[1][2];
e[6] = -m[2][0]; e[7] = -m[2][1]; e[8] = -m[2][2] ;
T[0] = t[0]; T[1] = -t[1]; T[2] = -t[2];
}
};
template<class FT>
struct Point3D
{
typedef FT float_t;
float_t xyz[3]; //3D point location
float_t reserved; //alignment
////////////////////////////////
template <class Float> void SetPoint(Float x, Float y, Float z)
{
xyz[0] = (float_t) x;
xyz[1] = (float_t) y;
xyz[2] = (float_t) z;
reserved = 0;
}
template <class Float> void SetPoint(const Float * p)
{
xyz[0] = (float_t) p[0];
xyz[1] = (float_t) p[1];
xyz[2] = (float_t) p[2];
reserved = 0;
}
template <class Float> void GetPoint(Float* p) const
{
p[0] = (Float) xyz[0];
p[1] = (Float) xyz[1];
p[2] = (Float) xyz[2];
}
template <class Float> void GetPoint(Float&x, Float&y, Float&z) const
{
x = (Float) xyz[0];
y = (Float) xyz[1];
z = (Float) xyz[2];
}
};
#undef CameraT
#undef Point3D
typedef CameraT_<float> CameraT;
typedef CameraT_<double> CameraD;
typedef Point3D_<float> Point3D;
typedef Point3D_<double> Point3B;
struct Point2D
{
float x, y;
////////////////////////////////////////////////////////
Point2D(){}
template <class Float> Point2D(Float X, Float Y) {SetPoint2D(X, Y); }
template <class Float> void SetPoint2D(Float X, Float Y) { x = (float) X; y = (float) Y; }
template <class Float> void GetPoint2D(Float&X, Float&Y) const { X = (Float) x; Y = (Float) y; }
};
} // namespace PBA
#endif