C++ Matrix3::col方法代码示例

  bool testRawDataAcces() {
    bool passed = true;
    Eigen::Matrix<Scalar, 4, 1> raw = {0, 1, 0, 0};
    Eigen::Map<RxSO3Type const> map_of_const_rxso3(raw.data());
    SOPHUS_TEST_APPROX(passed, map_of_const_rxso3.quaternion().coeffs().eval(),
                       raw, Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_const_rxso3.quaternion().coeffs().data(),
                      raw.data());
    Eigen::Map<RxSO3Type const> const_shallow_copy = map_of_const_rxso3;
    SOPHUS_TEST_EQUAL(passed, const_shallow_copy.quaternion().coeffs().eval(),
                      map_of_const_rxso3.quaternion().coeffs().eval());

    Eigen::Matrix<Scalar, 4, 1> raw2 = {1, 0, 0, 0};
    Eigen::Map<RxSO3Type> map_of_rxso3(raw.data());
    Eigen::Quaternion<Scalar> quat;
    quat.coeffs() = raw2;
    map_of_rxso3.setQuaternion(quat);
    SOPHUS_TEST_APPROX(passed, map_of_rxso3.quaternion().coeffs().eval(), raw2,
                       Constants<Scalar>::epsilon());
    SOPHUS_TEST_EQUAL(passed, map_of_rxso3.quaternion().coeffs().data(),
                      raw.data());
    SOPHUS_TEST_NEQ(passed, map_of_rxso3.quaternion().coeffs().data(),
                    quat.coeffs().data());
    Eigen::Map<RxSO3Type> shallow_copy = map_of_rxso3;
    SOPHUS_TEST_EQUAL(passed, shallow_copy.quaternion().coeffs().eval(),
                      map_of_rxso3.quaternion().coeffs().eval());

    RxSO3Type const const_so3(quat);
    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, const_so3.data()[i], raw2.data()[i]);
    }

    RxSO3Type so3(quat);
    for (int i = 0; i < 4; ++i) {
      so3.data()[i] = raw[i];
    }

    for (int i = 0; i < 4; ++i) {
      SOPHUS_TEST_EQUAL(passed, so3.data()[i], raw.data()[i]);
    }

    for (int i = 0; i < 10; ++i) {
      Matrix3<Scalar> M = Matrix3<Scalar>::Random();
      for (Scalar scale : {Scalar(0.01), Scalar(0.99), Scalar(1), Scalar(10)}) {
        Matrix3<Scalar> R = makeRotationMatrix(M);
        Matrix3<Scalar> sR = scale * R;
        SOPHUS_TEST(passed, isScaledOrthogonalAndPositive(sR),
                    "isScaledOrthogonalAndPositive(sR): % *\n%", scale, R);
        Matrix3<Scalar> sR_cols_swapped;
        sR_cols_swapped << sR.col(1), sR.col(0), sR.col(2);
        SOPHUS_TEST(passed, !isScaledOrthogonalAndPositive(sR_cols_swapped),
                    "isScaledOrthogonalAndPositive(-sR): % *\n%", scale, R);
      }
    }
    return passed;
  }

void PointMesher<T>::Mesher::createFaceAttributes(const Matrix vList, const Matrix fList, Matrix& fAttrList, Labels& fAttrLabels) const
{
	// Initialization
	Matrix3 vp;
	Vector3 vc;
	Vector3 vn;

	for (int i = 0; i < fList.cols(); i++)
	{
		// Get triangle
		vp.col(0) = vList.col(int(fList(0, i)));
		vp.col(1) = vList.col(int(fList(1, i)));
		vp.col(2) = vList.col(int(fList(2, i)));

		// Compute triangle centroid
		vc = this->computeCentroid(vp);

		// Compute normal at centroid
		vn = this->computeNormal(vp);

		// Generate data structure
		fAttrList.block(0, i, 3, 1) = vc;
		fAttrList.block(3, i, 3, 1) = vn;
	}

	fAttrLabels[0].text = "FaceCentroids";
	fAttrLabels[0].span = 3;
	fAttrLabels[1].text = "FaceNormals";
	fAttrLabels[1].span = 3;
}

template<typename Scalar> void orthomethods_3()
{
  typedef typename NumTraits<Scalar>::Real RealScalar;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;

  typedef Matrix<Scalar,4,1> Vector4;

  Vector3 v0 = Vector3::Random(),
          v1 = Vector3::Random(),
          v2 = Vector3::Random();

  // cross product
  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v1), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v1.dot(v1.cross(v2)), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(v2).dot(v2), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v2.dot(v1.cross(v2)), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN(v1.cross(Vector3::Random()).dot(v1), Scalar(1));
  Matrix3 mat3;
  mat3 << v0.normalized(),
         (v0.cross(v1)).normalized(),
         (v0.cross(v1).cross(v0)).normalized();
  VERIFY(mat3.isUnitary());
  
  mat3.setRandom();
  VERIFY_IS_APPROX(v0.cross(mat3*v1), -(mat3*v1).cross(v0));
  VERIFY_IS_APPROX(v0.cross(mat3.lazyProduct(v1)), -(mat3.lazyProduct(v1)).cross(v0));

  // colwise/rowwise cross product
  mat3.setRandom();
  Vector3 vec3 = Vector3::Random();
  Matrix3 mcross;
  int i = internal::random<int>(0,2);
  mcross = mat3.colwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
  
  VERIFY_IS_MUCH_SMALLER_THAN((mat3.adjoint() * mat3.colwise().cross(vec3)).diagonal().cwiseAbs().sum(), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN((mat3.adjoint() * mat3.colwise().cross(Vector3::Random())).diagonal().cwiseAbs().sum(), Scalar(1));
  
  VERIFY_IS_MUCH_SMALLER_THAN((vec3.adjoint() * mat3.colwise().cross(vec3)).cwiseAbs().sum(), Scalar(1));
  VERIFY_IS_MUCH_SMALLER_THAN((vec3.adjoint() * Matrix3::Random().colwise().cross(vec3)).cwiseAbs().sum(), Scalar(1));
  
  mcross = mat3.rowwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));

  // cross3
  Vector4 v40 = Vector4::Random(),
          v41 = Vector4::Random(),
          v42 = Vector4::Random();
  v40.w() = v41.w() = v42.w() = 0;
  v42.template head<3>() = v40.template head<3>().cross(v41.template head<3>());
  VERIFY_IS_APPROX(v40.cross3(v41), v42);
  VERIFY_IS_MUCH_SMALLER_THAN(v40.cross3(Vector4::Random()).dot(v40), Scalar(1));
  
  // check mixed product
  typedef Matrix<RealScalar, 3, 1> RealVector3;
  RealVector3 rv1 = RealVector3::Random();
  VERIFY_IS_APPROX(v1.cross(rv1.template cast<Scalar>()), v1.cross(rv1));
  VERIFY_IS_APPROX(rv1.template cast<Scalar>().cross(v1), rv1.cross(v1));
}

void Vert::calc_curvature() {
    Edge *e = edge;

    Matrix3 m = Matrix3();
    double wij_sum = 0.0;
    do {
        Vector tij = ((Matrix3::identity() - Matrix3(normal(), normal())) * e->pair->vect()).unit();
        double kij = 2 * normal().dot(e->pair->vect()) / pow(e->pair->vect().length(),2);
        double wij = 0.0;
        if (e->face != NULL)       { wij += e->face->area(); }
        if (e->pair->face != NULL) { wij += e->pair->face->area(); }
        wij_sum += wij;
        m = m + wij * kij * Matrix3(tij, tij);

        e = e->pair->prev;
    } while (e != edge);

    m = m / wij_sum;

    Vector e1 = Vector(1, 0, 0);
    Vector wvi;
    if ((e1 - normal()).length() > (e1 + normal()).length()) {
        wvi = (e1 - normal()).unit();
    } else {
        wvi = (e1 + normal()).unit();
    }

    Matrix3 qvi = Matrix3::identity() - 2 * Matrix3(wvi,wvi);

    Matrix3 m2 = qvi.transpose() * m * qvi;

    double m11   = m2.values[4],
           m12   = m2.values[7],
           m22   = m2.values[8],
           beta  = (m22 - m11) / 2 / m12,
           t     = sign(beta) / (abs(beta) + sqrt(pow(beta, 2) + 1)),
           c     = 1.0 / sqrt(pow(t,2) + 1),
           s     = c * t,
           m11_p = pow(c,2) * m11 + pow(s,2) * m22 - 2 * c * s * m12,
           m22_p = pow(s,2) * m11 + pow(c,2) * m22 + 2 * c * s * m12,
           phi   = atan(t),
           k1_p  = 3 * m11_p - m22_p,
           k2_p  = 3 * m22_p - m11_p;

    if (k1_p < k2_p) {
        mem_max_curvature_mag = k1_p;
        mem_max_curvature_dir = cos(phi) * qvi.col(1) - sin(phi) * qvi.col(2);
        mem_min_curvature_mag = k2_p;
        mem_min_curvature_dir = sin(phi) * qvi.col(1) + cos(phi) * qvi.col(2);
    } else {
        mem_max_curvature_mag = k2_p;
        mem_max_curvature_dir = sin(phi) * qvi.col(1) + cos(phi) * qvi.col(2);
        mem_min_curvature_mag = k1_p;
        mem_min_curvature_dir = cos(phi) * qvi.col(1) - sin(phi) * qvi.col(2);
    }
}

//.........这里部分代码省略.........
  t0.scale(v0);
  VERIFY_IS_APPROX(t0 * v1, Scaling3(v0) * v1);

  // test transform inversion
  t0.setIdentity();
  t0.translate(v0);
  t0.linear().setRandom();
  VERIFY_IS_APPROX(t0.inverse(Affine), t0.matrix().inverse());
  t0.setIdentity();
  t0.translate(v0).rotate(q1);
  VERIFY_IS_APPROX(t0.inverse(Isometry), t0.matrix().inverse());

  // test extract rotation and scaling
  t0.setIdentity();
  t0.translate(v0).rotate(q1).scale(v1);
  VERIFY_IS_APPROX(t0.rotation() * v1, Matrix3(q1) * v1);

  Matrix3 mat_rotation, mat_scaling;
  t0.setIdentity();
  t0.translate(v0).rotate(q1).scale(v1);
  t0.computeRotationScaling(&mat_rotation, &mat_scaling);
  VERIFY_IS_APPROX(t0.linear(), mat_rotation * mat_scaling);
  VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
  VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));
  t0.computeScalingRotation(&mat_scaling, &mat_rotation);
  VERIFY_IS_APPROX(t0.linear(), mat_scaling * mat_rotation);
  VERIFY_IS_APPROX(mat_rotation*mat_rotation.adjoint(), Matrix3::Identity());
  VERIFY_IS_APPROX(mat_rotation.determinant(), Scalar(1));

  // test casting
  Transform<float,3> t1f = t1.template cast<float>();
  VERIFY_IS_APPROX(t1f.template cast<Scalar>(),t1);
  Transform<double,3> t1d = t1.template cast<double>();
  VERIFY_IS_APPROX(t1d.template cast<Scalar>(),t1);

  Translation3 tr1(v0);
  Translation<float,3> tr1f = tr1.template cast<float>();
  VERIFY_IS_APPROX(tr1f.template cast<Scalar>(),tr1);
  Translation<double,3> tr1d = tr1.template cast<double>();
  VERIFY_IS_APPROX(tr1d.template cast<Scalar>(),tr1);

  Scaling3 sc1(v0);
  Scaling<float,3> sc1f = sc1.template cast<float>();
  VERIFY_IS_APPROX(sc1f.template cast<Scalar>(),sc1);
  Scaling<double,3> sc1d = sc1.template cast<double>();
  VERIFY_IS_APPROX(sc1d.template cast<Scalar>(),sc1);

  Quaternion<float> q1f = q1.template cast<float>();
  VERIFY_IS_APPROX(q1f.template cast<Scalar>(),q1);
  Quaternion<double> q1d = q1.template cast<double>();
  VERIFY_IS_APPROX(q1d.template cast<Scalar>(),q1);

  AngleAxis<float> aa1f = aa1.template cast<float>();
  VERIFY_IS_APPROX(aa1f.template cast<Scalar>(),aa1);
  AngleAxis<double> aa1d = aa1.template cast<double>();
  VERIFY_IS_APPROX(aa1d.template cast<Scalar>(),aa1);

  Rotation2D<Scalar> r2d1(ei_random<Scalar>());
  Rotation2D<float> r2d1f = r2d1.template cast<float>();
  VERIFY_IS_APPROX(r2d1f.template cast<Scalar>(),r2d1);
  Rotation2D<double> r2d1d = r2d1.template cast<double>();
  VERIFY_IS_APPROX(r2d1d.template cast<Scalar>(),r2d1);

  m = q1;
//   m.col(1) = Vector3(0,ei_random<Scalar>(),ei_random<Scalar>()).normalized();
//   m.col(0) = Vector3(-1,0,0).normalized();
//   m.col(2) = m.col(0).cross(m.col(1));
  #define VERIFY_EULER(I,J,K, X,Y,Z) { \
    Vector3 ea = m.eulerAngles(I,J,K); \
    Matrix3 m1 = Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z())); \
    VERIFY_IS_APPROX(m, m1); \
    VERIFY_IS_APPROX(m,  Matrix3(AngleAxisx(ea[0], Vector3::Unit##X()) * AngleAxisx(ea[1], Vector3::Unit##Y()) * AngleAxisx(ea[2], Vector3::Unit##Z()))); \
  }
  VERIFY_EULER(0,1,2, X,Y,Z);
  VERIFY_EULER(0,1,0, X,Y,X);
  VERIFY_EULER(0,2,1, X,Z,Y);
  VERIFY_EULER(0,2,0, X,Z,X);

  VERIFY_EULER(1,2,0, Y,Z,X);
  VERIFY_EULER(1,2,1, Y,Z,Y);
  VERIFY_EULER(1,0,2, Y,X,Z);
  VERIFY_EULER(1,0,1, Y,X,Y);

  VERIFY_EULER(2,0,1, Z,X,Y);
  VERIFY_EULER(2,0,2, Z,X,Z);
  VERIFY_EULER(2,1,0, Z,Y,X);
  VERIFY_EULER(2,1,2, Z,Y,Z);

  // colwise/rowwise cross product
  mat3.setRandom();
  Vector3 vec3 = Vector3::Random();
  Matrix3 mcross;
  int i = ei_random<int>(0,2);
  mcross = mat3.colwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.col(i), mat3.col(i).cross(vec3));
  mcross = mat3.rowwise().cross(vec3);
  VERIFY_IS_APPROX(mcross.row(i), mat3.row(i).cross(vec3));


}

// Original comment:
//   Triangulation happens in 2d. We could inverse transform the polygon around the normal direction, or we just use the two
//   most signficant axes. Here we find the two longest axes and use them to triangulate.  Inverse transforming them would
//   introduce more doubling point error and isn't worth it.
//
// SC says:
//   This doesn't work: the vertices can be collinear when projected onto the plane of the two longest axes of the bounding box.
//   Example (from real data):
//
//     v[0] = (-13.7199, 4.45725, -8.00059)
//     v[1] = (-0.115787, 12.3116, -4.96109)
//     v[2] = (0.88992, 12.8922, -3.80342)
//     v[3] = (-0.115787, 12.3116, -2.64576)
//     v[4] = (-13.7199, 4.45725, 0.393742)
//     v[5] = (-13.7199, 4.45725, -0.856258)
//     v[6] = (-12.5335, 5.14221, -3.80342)
//     v[7] = (-13.7199, 4.45725, -6.75059)
//
//   Instead, we will project onto the plane of the polygon.
long
Polygon3::triangulate(Array<long> & tri_indices, Real epsilon) const
{
  if (epsilon < 0)
    epsilon = Math::eps<Real>();

  if (vertices.size() < 3)
  {
    tri_indices.clear();
  }
  else if (vertices.size() == 3)
  {
    tri_indices.resize(3);
    tri_indices[0] = vertices[0].index;
    tri_indices[1] = vertices[1].index;
    tri_indices[2] = vertices[2].index;
  }
  else if (vertices.size() > 3)
  {
    tri_indices.clear();

    size_t n = vertices.size();
    proj_vertices.resize(n);

    Vector3 normal = computeNormal();
    Matrix3 basis = Math::orthonormalBasis(normal);
    Vector3 axis0 = basis.col(0);
    Vector3 axis1 = basis.col(1);

    Vector3 v0 = vertices[0].position;  // a reference point for the plane of the polygon
    for (size_t i = 0; i < n; ++i)
    {
      Vector3 v = vertices[i].position - v0;
      proj_vertices[i] = Vector2(v.dot(axis0), v.dot(axis1));
    }

    Array<size_t> indices(n);
    bool flipped = false;
    if (projArea() > 0)
    {
      for (size_t v = 0; v < n; ++v)
        indices[v] = v;
    }
    else
    {
      for (size_t v = 0; v < n; ++v)
        indices[v] = (n - 1) - v;

      flipped = true;
    }

    size_t nv = n;
    size_t count = 2 * nv;
    for (size_t v = nv - 1; nv > 2; )
    {
      if ((count--) <= 0)
        break;

      size_t u = v;
      if (nv <= u) u = 0;

      v = u + 1;
      if (nv <= v) v = 0;

      size_t w = v + 1;
      if (nv <= w) w = 0;

      if (snip(u, v, w, nv, indices, epsilon))
      {
        size_t a = indices[u];
        size_t b = indices[v];
        size_t c = indices[w];
        if (flipped)
        {
          tri_indices.push_back(vertices[c].index);
          tri_indices.push_back(vertices[b].index);
          tri_indices.push_back(vertices[a].index);
        }
        else
        {
          tri_indices.push_back(vertices[a].index);
//.........这里部分代码省略.........