void Mat3::RotateZ(const float angle)
{
Mat3 tmp;
tmp.BuildRotateZ(angle);
(*this) *= tmp;
}
void Sprite::Transform(const Mat3 &rhs)
{
Vector3 topLeft(0 - m_anchor.x, 1 - m_anchor.y);
Vector3 topRight(1 - m_anchor.x, 1 - m_anchor.y);
Vector3 bottomLeft(0 - m_anchor.x, 0 - m_anchor.y);
Vector3 bottomRight(1 - m_anchor.x, 0 - m_anchor.y);
Mat3 scale = (scale.CreateScale( Vector3( (float)m_width, (float)m_height)));
topLeft = rhs * (scale * topLeft);
topRight = rhs * (scale * topRight);
bottomLeft = rhs * (scale * bottomLeft);
bottomRight = rhs * (scale * bottomRight);
topLeft.x = ((2.0f / m_game->GetScreenWidth()) * topLeft.x) - 1.0f;
topLeft.y = ((2.0f / m_game->GetScreenHeight()) * topLeft.y) - 1.0f;
topRight.x = ((2.0f / m_game->GetScreenWidth()) * topRight.x) - 1.0f;
topRight.y = ((2.0f / m_game->GetScreenHeight()) * topRight.y) - 1.0f;
bottomLeft.x = ((2.0f / m_game->GetScreenWidth()) * bottomLeft.x) - 1.0f;
bottomLeft.y = ((2.0f / m_game->GetScreenHeight()) * bottomLeft.y) - 1.0f;
bottomRight.x = ((2.0f / m_game->GetScreenWidth()) * bottomRight.x) - 1.0f;
bottomRight.y = ((2.0f / m_game->GetScreenHeight()) * bottomRight.y) - 1.0f;
m_vertices[0].Positions[0] = topLeft.x;
m_vertices[0].Positions[1] = topLeft.y;
m_vertices[1].Positions[0] = topRight.x;
m_vertices[1].Positions[1] = topRight.y;
m_vertices[2].Positions[0] = bottomLeft.x;
m_vertices[2].Positions[1] = bottomLeft.y;
m_vertices[3].Positions[0] = bottomRight.x;
m_vertices[3].Positions[1] = bottomRight.y;
}
void calc_projective (const std::vector<double>& frame_ts,
const std::vector<Vec4>& gyro_quat,
const std::vector<Vec3>& acc_trans,
const std::vector<double>& gyro_ts,
CalibrationParams calib,
std::vector<Mat3>& projective)
{
int index0 = 0;
int index1 = 0;
size_t frame_count = frame_ts.size();
for (int fid = 0; fid < frame_count; fid++) {
const double ts0 = frame_ts[fid] + calib.gyro_delay;
Quatern quat0 = interp_gyro_quatern(ts0, gyro_quat, gyro_ts, index0) + Quatern(calib.gyro_drift);
const double ts1 = frame_ts[fid + 1] + calib.gyro_delay;
Quatern quat1 = interp_gyro_quatern(ts1, gyro_quat, gyro_ts, index1) + Quatern(calib.gyro_drift);
Mat3 extr0 = calc_extrinsic(quat0);
Mat3 extr1 = calc_extrinsic(quat1);
Mat3 intrinsic = calc_intrinsic(calib.fx, calib.fy, calib.cx, calib.cy, calib.skew);
Mat3 extrinsic0 = rotate_coordinate_system(AXIS_X, AXIS_MINUS_Z) * extr0 * mirror_coordinate_system(AXIS_Y);
Mat3 extrinsic1 = rotate_coordinate_system(AXIS_X, AXIS_MINUS_Z) * extr1 * mirror_coordinate_system(AXIS_Y);
projective[fid] = intrinsic * extrinsic0 * extrinsic1.transpose() * intrinsic.inverse();
}
}
double volume(OpenMesh::Vec3f& pointA, OpenMesh::Vec3f& pointB, OpenMesh::Vec3f& pointC)
{
// http://www.ditutor.com/vectors/volume_tetrahedron.html
Mat3 m = pointsToMat(pointA , pointB, pointC);
// use minus sign or change the order of the points in above function call
return -m.determinant()/6.0;
}
void SpriteBatch::DrawAgent(Agent* a_agent)
{
Vec2 pos = a_agent->m_pos;
float width = (float)m_agentWidth;
float height = (float)m_agentHeight;
int xOff = (int)width / 2;
int yOff = (int)height / 2;
Vec2 tl = Vec2(-xOff, -yOff);
Vec2 tr = Vec2(xOff , -yOff);
Vec2 br = Vec2(-xOff, yOff);
Vec2 bl = Vec2(xOff , yOff);
float rot = Vec2(0,1).GetAngleBetween(a_agent->m_heading);
//create matrix for start point
Mat3 rotMat;
rotMat.Rotate(rot + 3.141592654);
rotMat.SetTranslation(pos);
//set the 4 vert points to correct rotation
tl = rotMat.TransformPoint(tl);
tr = rotMat.TransformPoint(tr);
br = rotMat.TransformPoint(br);
bl = rotMat.TransformPoint(bl);
processSprite(&tl, &tr, &bl, &br, m_agentIBO, m_agentVBO, m_texID_agent, SPRITE_COLOUR_WHITE);
}
void work() {
PiezoTensor pt = makePiezoTensor(3.655, 2.407, 0.328, 1.894);
MaterialTensor mt = makeMaterialTensor(19.886e10, 5.467e10, 6.799e10, 0.783e10, 23.418e10, 5.985e10, 7.209e10);
double rho = 4642.8;
double eps0 = 8.8542e-12;
double exx = eps0 * 44.9;
double ezz = eps0 * 26.7;
Vec3 n(0, 0, 1);
Mat3 christ = makePiezoChristoffel(pt, mt, n, exx, ezz);
Poly3 christPoly = christ.getPoly();
double g1, g2, g3;
christPoly.solve(&g1, &g2, &g3);
double v1 = sqrt(g1 / rho);
double v2 = sqrt(g2 / rho);
double v3 = sqrt(g3 / rho);
cout << "piezo tensor: " << endl << pt << endl;
cout << "material tensor: " << endl << mt << endl;
cout << "christoffel matrix: " << endl << christ << endl;
cout << "polynome " << christPoly << endl;
cout << "velocities = " << v1 << " " << v2 << " " << v3 << endl;
}
Mat3 mxrox (double& a, Vec3& v)
{
// Convert eigenvalue a (eigen angle in radians) and eigenvector v
// into a corresponding cosine rotation matrix m.
double q1, q2, q3, q4, q12, q22, q32, q42;
Mat3 result;
// calculate quaternions and their squares
q4 = sin ( 0.5 * a);
q1 = v[0] * q4;
q2 = v[1] * q4;
q3 = v[2] * q4;
q4 = cos (0.5 * a);
q12 = q1 * q1;
q22 = q2 * q2;
q32 = q3 * q3;
q42 = q4 * q4;
// now get the matrix elements
result.assign ((q12 - q22 - q32 + q42), (2.0 * (q1*q2 + q3*q4)),
(2.0 * (q1*q3 - q2*q4)), (2.0 * (q1*q2 - q3*q4)),
(-q12 + q22 - q32 + q42),(2.0 * (q2*q3 + q1*q4)),
(2.0 * (q1*q3 + q2*q4)), (2.0 * (q2*q3 - q1*q4)),
(-q12 - q22 + q32 + q42));
return result;
}
void Mat3::Rotate(const Vec3 &axis, const float angle)
{
Mat3 tmp;
tmp.BuildRotate(axis, angle);
(*this) *= tmp;
}
// Camera triangulation using the iterated linear method
Vec3 Triangulation::compute(int iter) const
{
const int nviews = int(views.size());
assert(nviews>=2);
// Iterative weighted linear least squares
Mat3 AtA;
Vec3 Atb, X;
Vec weights = Vec::Constant(nviews,1.0);
for (int it=0;it<iter;++it)
{
AtA.fill(0.0);
Atb.fill(0.0);
for (int i=0;i<nviews;++i)
{
const Mat34& PMat = views[i].first;
const Vec2 & p = views[i].second;
const double w = weights[i];
Vec3 v1, v2;
for (int j=0;j<3;j++)
{
v1[j] = w * ( PMat(0,j) - p(0) * PMat(2,j) );
v2[j] = w * ( PMat(1,j) - p(1) * PMat(2,j) );
Atb[j] += w * ( v1[j] * ( p(0) * PMat(2,3) - PMat(0,3) )
+ v2[j] * ( p(1) * PMat(2,3) - PMat(1,3) ) );
}
for (int k=0;k<3;k++)
{
for (int j=0;j<=k;j++)
{
const double v = v1[j] * v1[k] + v2[j] * v2[k];
AtA(j,k) += v;
if (j<k) AtA(k,j) += v;
}
}
}
X = AtA.inverse() * Atb;
// Compute reprojection error, min and max depth, and update weights
zmin = std::numeric_limits<double>::max();
zmax = - std::numeric_limits<double>::max();
err = 0.0;
for (int i=0;i<nviews;++i)
{
const Mat34& PMat = views[i].first;
const Vec2 & p = views[i].second;
const Vec3 xProj = PMat * X.homogeneous();
const double z = xProj(2);
if (z < zmin) zmin = z;
else if (z > zmax) zmax = z;
err += (p - xProj.hnormalized()).norm(); // residual error
weights[i] = 1.0 / z;
}
}
return X;
}
TEST(Image, Convolution_MeanBoxFilter)
{
Image<unsigned char> in(40,40);
in.block(10,10,20,20).fill(255.f);
Mat3 meanBoxFilterKernel;
meanBoxFilterKernel.fill(1.f/9.f);
Image<unsigned char> out;
ImageConvolution(in, meanBoxFilterKernel, out);
}
/**
* \brief 基本的相机坐标系，认为参考点的方向向量为z轴
*
* \param center 参考点的位置
* \param up 视点向上方向的向量.
*
* \return 世界坐标系转相机坐标系的矩阵
*/
Mat3 LookAt(const Vec3 ¢er, const Vec3 &up) {
Vec3 zc = center.normalized();//向量n
Vec3 xc = up.cross(zc).normalized();//向量u
Vec3 yc = zc.cross(xc);//向量v
Mat3 R;
R.row(0) = xc;
R.row(1) = yc;
R.row(2) = zc;
return R;
}
TEST(TinyMatrix, LookAt) {
// Simple orthogonality check.
Vec3 e; e[0]= 1; e[1] = 2; e[2] = 3;
Mat3 R = LookAt(e);
Mat3 I = Mat3::Identity();
Mat3 RRT = R*R.transpose();
Mat3 RTR = R.transpose()*R;
EXPECT_MATRIX_NEAR(I, RRT, 1e-15);
EXPECT_MATRIX_NEAR(I, RTR, 1e-15);
}
TEST(TinyMatrix, LookAt) {
// 简单的正交验证
Vec3 e; e[0]= 1; e[1] = 2; e[2] = 3;
Mat3 R = LookAt(e);//这个R是旋转矩阵，则R为正交矩阵，R与R的转置相乘为单位阵
Mat3 I = Mat3::Identity();
Mat3 RRT = R*R.transpose();
Mat3 RTR = R.transpose()*R;
EXPECT_MATRIX_NEAR(I, RRT, 1e-15);
EXPECT_MATRIX_NEAR(I, RTR, 1e-15);
}
//Draw Line
void SpriteBatch::DrawLine(Vec2 a_start, Vec2 a_end, SPRITE_COLOUR a_col, float a_width)
{
//width used to offsetting
float width;
if(a_width > 0)
width = a_width;
else
width = (float)m_line_width / 2;
//width override for debugging
//width = 50;
float size = (a_start - a_end).Length();
Vec2 origin = Vec2(-width / 2, 0);
//set the line around the zero point with the pivot
//at the top center (0,0)
//oriented for rotation zero along the x axis
Vec2 tl = Vec2(0, 0) + origin;
Vec2 tr = Vec2(width, 0) + origin;
Vec2 br = Vec2(0, size) + origin;
Vec2 bl = Vec2(width, size) + origin;
//get difference normalised
// Vec2 diff = Vec2(a_end.x - a_start.x, a_end.y - a_start.y).GetNormalised();
Vec2 diff = (a_start - a_end).GetNormalised();
float rot = Vec2(0,1).GetAngleBetween(diff);
//create matrix for start point
Mat3 rotMat;
rotMat.Rotate(rot + 3.141592654);
rotMat.SetTranslation(a_start);
//transform line start point
tl = rotMat.TransformPoint(tl);
tr = rotMat.TransformPoint(tr);
//transform line end point
br = rotMat.TransformPoint(br);
bl = rotMat.TransformPoint(bl);
processSprite(&tl, &tr, &bl, &br, m_lineIBO, m_lineVBO, m_texID_line, a_col);
}
MatX jointFrame(const Vect3 &loc, const Vect3 &axis_, const Vect3 &ref_)
{
Vect3 axis, ref, other;
Mat3 R;
axis.normalize(axis_);
ref.displace(ref_, axis, - ref_.dot(axis)); // in case ref & axis not perp
ref.normalize(ref);
other.cross(axis, ref);
R.setXcol(ref);
R.setYcol(other);
R.setZcol(axis);
return MatX(R, loc);
}
int rotCode(const Mat3 &R)
{
#define ALMOST_ONE 0.9
int i, j;
Vect3 axis, ref;
Vect3 dirs[6] =
{Vect3::I, Vect3::I_, Vect3::J, Vect3::J_, Vect3::K , Vect3::K_};
axis = R.zcol();
ref = R.xcol();
for (i = 0; axis.dot(dirs[i]) < ALMOST_ONE; i++);
for (j = 0; ref.dot(dirs[((i & ~1) + 2 + j) % 6]) < ALMOST_ONE; j++);
return i * 4 + j;
#undef ALMOST_ONE
}
IGL_INLINE void igl::fit_rotations(
const Eigen::PlainObjectBase<DerivedS> & S,
const bool single_precision,
Eigen::PlainObjectBase<DerivedD> & R)
{
using namespace std;
const int dim = S.cols();
const int nr = S.rows()/dim;
assert(nr * dim == S.rows());
assert(dim == 3);
// resize output
R.resize(dim,dim*nr); // hopefully no op (should be already allocated)
//std::cout<<"S=["<<std::endl<<S<<std::endl<<"];"<<std::endl;
//MatrixXd si(dim,dim);
Eigen::Matrix<typename DerivedS::Scalar,3,3> si;// = Eigen::Matrix3d::Identity();
// loop over number of rotations we're computing
for(int r = 0;r<nr;r++)
{
// build this covariance matrix
for(int i = 0;i<dim;i++)
{
for(int j = 0;j<dim;j++)
{
si(i,j) = S(i*nr+r,j);
}
}
typedef Eigen::Matrix<typename DerivedD::Scalar,3,3> Mat3;
typedef Eigen::Matrix<typename DerivedD::Scalar,3,1> Vec3;
Mat3 ri;
if(single_precision)
{
polar_svd3x3(si, ri);
}else
{
Mat3 ti,ui,vi;
Vec3 _;
igl::polar_svd(si,ri,ti,ui,_,vi);
}
assert(ri.determinant() >= 0);
R.block(0,r*dim,dim,dim) = ri.block(0,0,dim,dim).transpose();
//cout<<matlab_format(si,C_STR("si_"<<r))<<endl;
//cout<<matlab_format(ri.transpose().eval(),C_STR("ri_"<<r))<<endl;
}
}
Pinhole_Intrinsic(
unsigned int w = 0, unsigned int h = 0,
double focal_length_pix = 0.0,
double ppx = 0.0, double ppy = 0.0)
:IntrinsicBase(w,h)
{
_K << focal_length_pix, 0., ppx, 0., focal_length_pix, ppy, 0., 0., 1.;
_Kinv = _K.inverse();
}
SimpleCamera(
const Mat3 & K = Mat3::Identity(),
const Mat3 & R = Mat3::Identity(),
const Vec3 & t = Vec3::Zero())
: _K(K), _R(R), _t(t)
{
_C = -R.transpose() * t;
P_From_KRt(_K, _R, _t, &_P);
}
Mat3 operator*(const Mat3& n, const Mat3& m)
{
Mat3 A;
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
A(i,j) = n[i]*m.col(j);
return A;
}
Mat3 calc_projective (double* frame_ts,
Vec4* gyro_quat,
Vec3* acc_trans,
double* gyro_ts,
CalibrationParams calib)
{
double ts0 = frame_ts[0] + calib.gyro_delay;
Quatern rot0 = Quatern(gyro_quat[0] + calib.gyro_drift);
double ts1 = frame_ts[1] + calib.gyro_delay;
Quatern rot1 = Quatern(gyro_quat[1] + calib.gyro_drift);
Vec3 trans0 = acc_trans[0];
Vec3 trans1 = acc_trans[1];
Mat3 extr0 = calc_extrinsic(rot0);
Mat3 extr1 = calc_extrinsic(rot1);
Mat3 intrin = calc_intrinsic(calib.fx, calib.fy, calib.cx, calib.cy, calib.skew);
return intrin * extr0 * extr1.transpose() * intrin.inverse();
}
bool robustRelativePose(
const Mat3 & K1, const Mat3 & K2,
const Mat & x1, const Mat & x2,
RelativePose_Info & relativePose_info,
const std::pair<size_t, size_t> & size_ima1,
const std::pair<size_t, size_t> & size_ima2,
const size_t max_iteration_count)
{
// Use the 5 point solver to estimate E
typedef openMVG::essential::kernel::FivePointKernel SolverType;
// Define the AContrario adaptor
typedef ACKernelAdaptorEssential<
SolverType,
openMVG::fundamental::kernel::EpipolarDistanceError,
UnnormalizerT,
Mat3>
KernelType;
KernelType kernel(x1, size_ima1.first, size_ima1.second,
x2, size_ima2.first, size_ima2.second, K1, K2);
// Robustly estimation of the Essential matrix and it's precision
std::pair<double,double> acRansacOut = ACRANSAC(kernel, relativePose_info.vec_inliers,
max_iteration_count, &relativePose_info.essential_matrix, relativePose_info.initial_residual_tolerance, false);
relativePose_info.found_residual_precision = acRansacOut.first;
if (relativePose_info.vec_inliers.size() < 2.5 * SolverType::MINIMUM_SAMPLES )
return false; // no sufficient coverage (the model does not support enough samples)
// estimation of the relative poses
Mat3 R;
Vec3 t;
if (!estimate_Rt_fromE(
K1, K2, x1, x2,
relativePose_info.essential_matrix, relativePose_info.vec_inliers, &R, &t))
return false; // cannot find a valid [R|t] couple that makes the inliers in front of the camera.
// Store [R|C] for the second camera, since the first camera is [Id|0]
relativePose_info.relativePose = geometry::Pose3(R, -R.transpose() * t);
return true;
}
void for_each(Mat1 mat1, Mat2 mat2, Mat3 mat3, Func func)
{
assert(Mat1::FOR_EACH_ABLE == Mat1::FOR_EACH_ABLE);
assert(Mat2::FOR_EACH_ABLE == Mat2::FOR_EACH_ABLE);
assert(Mat3::FOR_EACH_ABLE == Mat3::FOR_EACH_ABLE);
assert(mat1.rows() == mat2.rows() && mat1.cols() == mat2.cols());
assert(mat1.rows() == mat3.rows() && mat1.cols() == mat3.cols());
for (int y = 0; y < mat1.rows(); ++y)
{
auto p1 = mat1[y];
auto p2 = mat2[y];
auto p3 = mat3[y];
for (int x = 0; x < mat1.cols(); ++x)
{
func(*p1, *p2, *p3);
p1 += 1, p2 += 1, p3 += 1;
}
}
}
ACKernelAdaptorResection_K(const Mat &x2d, const Mat &x3D, const Mat3 & K)
: x2d_(x2d.rows(), x2d.cols()), x3D_(x3D),
N1_(K.inverse()),
logalpha0_(log10(M_PI)), K_(K)
{
assert(2 == x2d_.rows());
assert(3 == x3D_.rows());
assert(x2d_.cols() == x3D_.cols());
// Normalize points by inverse(K)
ApplyTransformationToPoints(x2d, N1_, &x2d_);
}
double residual(const VNVels& vnv) {
PiezoTensor pt = makePiezoTensor(3.655, 2.407, 0.328, 1.894);
MaterialTensor mt = makeMaterialTensor(19.886e10, 5.467e10, 6.799e10, 0.783e10, 23.418e10, 5.985e10, 7.209e10);
double rho = 4642.8;
double eps0 = 8.8542e-12;
double exx = eps0 * 44.9;
double ezz = eps0 * 26.7;
double ret = 0;
for(VNVels::const_iterator it = vnv.begin(); it != vnv.end(); ++it) {
Vec3 n(it -> first);
Mat3 christ = makePiezoChristoffel(pt, mt, n, exx, ezz);
Poly3 christPoly = christ.getPoly();
double g1, g2, g3;
christPoly.solve(&g1, &g2, &g3);
double v1 = sqrt(g1 / rho);
double v2 = sqrt(g2 / rho);
double v3 = sqrt(g3 / rho);
double maxv = max(max(v1, v2), v3);
double minv = min(min(v1, v2), v3);
double medv = v1 + v2 + v3 - maxv - minv;
Vec3 tv(minv, medv, maxv);
Vec3 ex = it -> second;
double curResidual = (ex - tv).norm();
ret += curResidual;
if (curResidual > 30000) {
cout << "curResidual = " << curResidual << " n = " << it -> first
<< " experimental = " << it -> second << " theory = " << tv << endl;
}
}
return ret;
}
void SpatialMat::inertiaInvert(const SpatialMat &I)
{
Mat3 Binv;
Mat3 M, N; /* temp storage */
Binv.invert(I._B);
Binv.negate();
M.mult(I._D, Binv);
N.mult(M, I._A);
N.add(I._C);
_B.invert(N);
M.mult(_B, I._D);
_A.mult(M, Binv);
_D.xpose(_A);
M.mult(I._A, _A);
M.xrow().x -= 1.0;
M.yrow().y -= 1.0;
M.zrow().z -= 1.0;
_C.mult(Binv, M);
//Mat3 detI;
//detI.mult(I._A, I._D);
//M.mult(I._B, I._C);
//detI.sub(M);
//detI.invert();
//_A.mult(detI, I._D);
//_B.mult(detI, I._B);
//_B.negate();
//_C.mult(detI, I._C);
//_C.negate();
//_D.mult(detI, I._A);
return;
}
// Generates all necessary inputs and expected outputs for EuclideanResection.
void CreateCameraSystem(const Mat3& KK,
const Mat3X& x_image,
const Vec& X_distances,
const Mat3& R_input,
const Vec3& T_input,
Mat2X *x_camera,
Mat3X *X_world,
Mat3 *R_expected,
Vec3 *T_expected) {
int num_points = x_image.cols();
Mat3X x_unit_cam(3, num_points);
x_unit_cam = KK.inverse() * x_image;
// Create normalized camera coordinates to be used as an input to the PnP
// function, instead of using NormalizeColumnVectors(&x_unit_cam).
*x_camera = x_unit_cam.block(0, 0, 2, num_points);
for (int i = 0; i < num_points; ++i){
x_unit_cam.col(i).normalize();
}
// Create the 3D points in the camera system.
Mat X_camera(3, num_points);
for (int i = 0; i < num_points; ++i) {
X_camera.col(i) = X_distances(i) * x_unit_cam.col(i);
}
// Apply the transformation to the camera 3D points
Mat translation_matrix(3, num_points);
translation_matrix.row(0).setConstant(T_input(0));
translation_matrix.row(1).setConstant(T_input(1));
translation_matrix.row(2).setConstant(T_input(2));
*X_world = R_input * X_camera + translation_matrix;
// Create the expected result for comparison.
*R_expected = R_input.transpose();
*T_expected = *R_expected * ( - T_input);
};
// Check the properties of a fundamental matrix:
//
// 1. The determinant is 0 (rank deficient)
// 2. The condition x'T*F*x = 0 is satisfied to precision.
//
bool ExpectFundamentalProperties(const Mat3 &F,
const Mat &ptsA,
const Mat &ptsB,
double precision) {
bool bOk = true;
bOk &= F.determinant() < precision;
assert(ptsA.cols() == ptsB.cols());
const Mat hptsA = ptsA.colwise().homogeneous();
const Mat hptsB = ptsB.colwise().homogeneous();
for (int i = 0; i < ptsA.cols(); ++i) {
const double residual = hptsB.col(i).dot(F * hptsA.col(i));
bOk &= residual < precision;
}
return bOk;
}
void Sprite::Update(float deltaTime)
{
Mat3 translation = (translation.CreateTranslation( Vector3( m_position)));
Mat3 rotation = (rotation.CreateRotation(m_rotation));
m_transform = rotation * translation;
if (m_parent)
{
m_worldTransform = m_transform * m_parent->GetWorldTransform();
}
else
{
m_worldTransform = m_transform;
}
for (auto i = m_children.begin(); i != m_children.end(); i++)
{
(*i)->Update(deltaTime);
}
ApplyTransform();
}
BrownPinholeCamera(
double focal,
double ppx,
double ppy,
const Mat3 & R = Mat3::Identity(),
const Vec3 & t = Vec3::Zero(),
double k1 = 0.0,
double k2 = 0.0,
double k3 = 0.0)
: _R(R), _t(t), _f(focal), _ppx(ppx), _ppy(ppy),
_k1(k1), _k2(k2), _k3(k3)
{
_C = -R.transpose() * t;
_K << _f, 0, _ppx,
0, _f, _ppy,
0, 0, 1;
P_From_KRt(_K, _R, _t, &_P);
}
