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); }