C++ Matrix3d::transposeInPlace方法代码示例

CSUtils::ENUPoint CSUtils::ecef_to_enu(const CSUtils::ECEFPoint& _point, const CSUtils::GCSPoint& _origin)
{
  double phi = _origin.latitude;
  double lamda = _origin.longitude;
  double h = _origin.altitude;

  Eigen::Vector3d lccs_center_point;

  lccs_center_point(0) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * cos(lamda);
  lccs_center_point(1) = (a * cos(atan(b * tan(phi) / a)) + h * cos(phi)) * sin(lamda);
  lccs_center_point(2) = b * sin(atan(b * tan(phi) / a)) + h * sin(phi);

  Eigen::Matrix3d C_1 = Eigen::Matrix3d::Zero();
  C_1(0, 2) = -1.0; C_1(1, 1) = 1.0; C_1(2, 0) = 1.0;

  Eigen::Matrix3d C_2 = Eigen::Matrix3d::Zero();
  C_2(0, 0) = 1.0;
  C_2(1, 1) = cos(phi);   C_2(1, 2) = -sin(phi);
  C_2(2, 1) = sin(phi);   C_2(2, 2) = cos(phi);

  Eigen::Matrix3d C_3 = Eigen::Matrix3d::Zero();
  C_3(0, 0) = sin(lamda);    C_3(0, 1) = cos(lamda);   
  C_3(1, 0) = -cos(lamda);   C_3(1, 1) = sin(lamda);
  C_3(2, 2) = 1.0;

  Eigen::Matrix3d C = C_3 * C_2 * C_1;
  C.transposeInPlace();
  Eigen::Vector3d result = C * Eigen::Vector3d(_point.x, _point.y, _point.z) - C * lccs_center_point;

  return CSUtils::ENUPoint(result(0), result(1), result(2));
}

Eigen::Matrix3d createRotationMatrix(double x, double y, double z, double w) {
	KDL::Rotation kdl_rotaion = KDL::Rotation::Quaternion(x, y, z, w);
	Eigen::Matrix3d result;
	for(int i = 0; i < 9; i++) {
		result.data()[i] = kdl_rotaion.data[i];
	}
	result.transposeInPlace();
	return result;
}

  Eigen::Matrix3d CEMatrixEditor::validateEditor()
  {
    // Editing fractional matrix is not allowed. The widget is
    // disabled to ensure that this assertion passes.
    Q_ASSERT(m_ext->matrixCartFrac() != Fractional);

    // Clear selection, otherwise there is a crash on Qt 4.7.2.
    QTextCursor tc = ui.edit_matrix->textCursor();
    tc.clearSelection();
    ui.edit_matrix->setTextCursor(tc);

    QString text = ui.edit_matrix->document()->toPlainText();
    QStringList lines = text.split("\n",
                                   QString::SkipEmptyParts);
    if (lines.size() != 3) {
      emit invalidInput();
      return Eigen::Matrix3d::Zero();
    }

    QList<QStringList> stringVecs;
    Eigen::Matrix3d mat;
    for (int row = 0; row < 3; ++row) {
      stringVecs.append(lines.at(row).simplified()
                        .split(CE_PARSE_IGNORE_REGEXP,
                               QString::SkipEmptyParts));
      QStringList &stringVec = stringVecs[row];
      if (stringVec.size() != 3) {
        emit invalidInput();
        return Eigen::Matrix3d::Zero();
      }
      for (int col = 0; col < 3; ++col) {
        bool ok;
        double val = stringVec[col].toDouble(&ok);
        if (!ok) {
          emit invalidInput();
          return Eigen::Matrix3d::Zero();
        }
        mat(row,col) = val;
      }
    }

    // Transpose if needed
    if (m_ext->matrixVectorStyle() == ColumnVectors) {
      // Warning: mat = mat.transpose() will *not* work correctly with
      // Eigen matrices. Use transposeInPlace() instead.
      mat.transposeInPlace();
    }

    emit validInput();
    return mat;
  }

void Reader::draw_label( QGLPainter *painter, const QVector3D& position, const QColor4ub& color, const std::string& label )
{
    if( label.empty() ) { return; }
    painter->modelViewMatrix().push();
    painter->modelViewMatrix().translate( position - m_offset ); // painter->modelViewMatrix().translate( position );
    QMatrix4x4 world = painter->modelViewMatrix().top();
    Eigen::Matrix3d R;
    R << world( 0, 0 ) , world( 0, 1 ), world( 0, 2 ),
         world( 1, 0 ) , world( 1, 1 ), world( 1, 2 ),
         world( 2, 0 ) , world( 2, 1 ), world( 2, 2 );
    R.transposeInPlace();
    snark::rotation_matrix rotation( R );
    Eigen::Quaterniond q = rotation.quaternion();
    painter->modelViewMatrix().rotate( QQuaternion( q.w(), q.x(), q.y(), q.z() ) );
    //painter->modelViewMatrix().translate( m_offset );
    double scale = 1.0 / double( m_viewer.height() );
    scale *= m_viewer.camera()->projectionType() == QGLCamera::Orthographic
           ? ( 0.25 * m_viewer.camera()->viewSize().width() )
           : ( 0.2 * Eigen::Vector3d( world( 0, 3 ) , world( 1, 3 ), world( 2, 3 ) ).norm() );
    painter->modelViewMatrix().scale( scale ); // TODO make size configurable ?
    drawText( painter, &label[0], color );
    painter->modelViewMatrix().pop();
}