void mitk::MicroBirdTrackingDevice::TrackTools()
{
  if (this->GetState() != Tracking)
    return;

  /* Frequency configuration */
  double updateRate = 1000.0 / m_SystemConfig.measurementRate;
  double measurementDuration = 0.0;


  // lock the TrackingFinishedMutex to signal that the execution rights
  //   are now transfered to the tracking thread
  MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope

  // Because m_StopTracking is used by two threads, access has to be guarded
  //   by a mutex. To minimize thread locking, a local copy is used here 
  bool localStopTracking;

  /* update the local copy of m_StopTracking */
  this->m_StopTrackingMutex->Lock();
  localStopTracking = this->m_StopTracking;
  this->m_StopTrackingMutex->Unlock();

  /* Tracking loop */
  while ((this->GetState() == Tracking) && (localStopTracking == false))
  {
    int errorCode;
    unsigned int nOfAttachedSensors = 0;
    double timeStamp = 0.0;   
    int toolNumber = 0; // Numbers for attached sensors only

    for (int sensorID = 0; sensorID < m_SystemConfig.numberSensors; sensorID++) // for each sensor grep data
    {
      if (!m_SensorConfig[sensorID].attached)
        continue;

      // sensor attached so get record
      errorCode = GetAsynchronousRecord(sensorID, pRecord, sizeof(record));
      if (CompareError(errorCode, BIRD_ERROR_SUCCESS))      
      { // On SUCCESS, parse sensor information
        nOfAttachedSensors++;       
        timeStamp += record.time; // Get timestamp from record
        ToolType* tool = GetMicroBirdTool(toolNumber); /// Get tool (current sensor)
        if (tool != NULL)
        {
          tool->SetTrackingError(record.quality); // Set tracking error (quality) from record
          mitk::Point3D position;
          position[0] = record.x;
          position[1] = record.y;
          position[2] = record.z;
          tool->SetPosition(position);  // Set position
          mitk::Quaternion orientation(record.q[1], record.q[2], record.q[3],record.q[0]);
          tool->SetOrientation(orientation); // Set orientation as quaternion \todo : verify quaternion order q(r,x,y,z)
          tool->SetDataValid(true); // Set data state to valid
        }        
        toolNumber++; // Increment tool number
      }  
      else
      { // ERROR while reading sensor information
        HandleError(errorCode);
      }      
    }

    /// @todo : is there any synchronisation?
    // Average timestamp: timeStamp/nOfAttachedSensors

    // Compute sleep time
    double sleepTime = updateRate - measurementDuration;
    // Sleep
    if (sleepTime > 0.0 && sleepTime < 500.0)
    {
      // Note: we only have to approximately sleep one measurement cycle,
      //    since the tracker keeps track of the measurement rate itself
      itksys::SystemTools::Delay(sleepTime)
      //Sleep(static_cast<DWORD>(sleepTime));
    }

    // Update the local copy of m_StopTracking
    this->m_StopTrackingMutex->Lock();  
    localStopTracking = m_StopTracking;
    this->m_StopTrackingMutex->Unlock();
  }

  // @bug (#1813) : maybe we need to check for localStopTracking=true here?
  //    m_StopTracking should only ever be updated by StopTracking(), so
  //    maybe we should not unlock a mutex that nobody is waiting for?

  return; // returning from this function (and ThreadStartTracking()) this will end the thread
}

glm::vec3 Camera::forward() const {
    glm::vec4 forward = glm::inverse(orientation()) * glm::vec4(0,0,-1,1);
    return glm::vec3(forward);
}

int Q3DockAreaLayout::layoutItems(const QRect &rect, bool testonly)
{
    if (dockWindows->isEmpty())
        return 0;

    dirty = false;

    // some corrections
    QRect r = rect;
    if (orientation() == Qt::Vertical)
        r.setHeight(r.height() - 3);

    // init
    lines.clear();
    ls.clear();
    int start = start_pos(r, orientation());
    int pos = start;
    int sectionpos = 0;
    int linestrut = 0;
    QList<Q3DockData> lastLine;
    int tbstrut = -1;
    int maxsize = size_extent(rect.size(), orientation());
    int visibleWindows = 0;

    // go through all widgets in the dock
    for (int i = 0; i < dockWindows->size(); ++i) {
        Q3DockWindow *dw = dockWindows->at(i);
        if (dw->isHidden())
            continue;
        ++visibleWindows;
        // find position for the widget: This is the maximum of the
        // end of the previous widget and the offset of the widget. If
        // the position + the width of the widget dosn't fit into the
        // dock, try moving it a bit back, if possible.
        int op = pos;
        int dockExtend = dock_extent(dw, orientation(), maxsize);
        if (!dw->isStretchable()) {
            pos = qMax(pos, dw->offset());
            if (pos + dockExtend > size_extent(r.size(), orientation()) - 1)
                pos = qMax(op, size_extent(r.size(), orientation()) - 1 - dockExtend);
        }
        if (!lastLine.isEmpty() && !dw->newLine() && space_left(rect, pos, orientation()) < dockExtend)
            shrink_extend(dw, dockExtend, space_left(rect, pos, orientation()), orientation());
        // if the current widget doesn't fit into the line anymore and it is not the first widget of the line
        if (!lastLine.isEmpty() &&
             (space_left(rect, pos, orientation()) < dockExtend || dw->newLine())) {
            if (!testonly) // place the last line, if not in test mode
                place_line(lastLine, orientation(), linestrut, size_extent(r.size(), orientation()), tbstrut, maxsize, this);
            // remember the line coordinats of the last line
            if (orientation() == Qt::Horizontal)
                lines.append(QRect(0, sectionpos, r.width(), linestrut));
            else
                lines.append(QRect(sectionpos, 0, linestrut, r.height()));
            // do some clearing for the next line
            lastLine.clear();
            sectionpos += linestrut;
            linestrut = 0;
            pos = start;
            tbstrut = -1;
        }

        // remember first widget of a line
        if (lastLine.isEmpty()) {
            ls.append(dw);
            // try to make the best position
            int op = pos;
            if (!dw->isStretchable())
                pos = qMax(pos, dw->offset());
            if (pos + dockExtend > size_extent(r.size(), orientation()) - 1)
                pos = qMax(op, size_extent(r.size(), orientation()) - 1 - dockExtend);
        }
        // do some calculations and add the remember the rect which the docking widget requires for the placing
        QRect dwRect(pos, sectionpos, dockExtend, dock_strut(dw, orientation() ));
        lastLine.append(Q3DockData(dw, dwRect));
        if (qobject_cast<Q3ToolBar*>(dw))
            tbstrut = qMax(tbstrut, dock_strut(dw, orientation()));
        linestrut = qMax(dock_strut(dw, orientation()), linestrut);
        add_size(dockExtend, pos, orientation());
    }

    // if some stuff was not placed/stored yet, do it now
    if (!testonly)
        place_line(lastLine, orientation(), linestrut, size_extent(r.size(), orientation()), tbstrut, maxsize, this);
    if (orientation() == Qt::Horizontal)
        lines.append(QRect(0, sectionpos, r.width(), linestrut));
    else
        lines.append(QRect(sectionpos, 0, linestrut, r.height()));
    if (lines.size() >= 2 && *(--lines.end()) == *(--(--lines.end())))
        lines.removeLast();

    bool hadResizable = false;
    for (int i = 0; i < dockWindows->size(); ++i) {
        Q3DockWindow *dw = dockWindows->at(i);
        if (!dw->isVisibleTo(parentWidget))
            continue;
        hadResizable = hadResizable || dw->isResizeEnabled();
        dw->updateSplitterVisibility(visibleWindows > 1); //!dw->area()->isLastDockWindow(dw));
        if (Q3ToolBar *tb = qobject_cast<Q3ToolBar *>(dw))
            tb->checkForExtension(dw->size());
    }
//.........这里部分代码省略.........

void mitk::NavigationDataRecorderDeprecated::Update()
{
  if (m_Recording)
  {
    DataObjectPointerArray inputs = this->GetInputs(); //get all inputs
    mitk::NavigationData::TimeStampType timestamp=0.0; // timestamp for mitk time
    timestamp = mitk::IGTTimeStamp::GetInstance()->GetElapsed();


    mitk::NavigationData::TimeStampType sysTimestamp = 0.0; // timestamp for system time
    sysTimestamp = m_SystemTimeClock->GetCurrentStamp();

    // cast system time double value to stringstream to avoid low precision rounding
    std::ostringstream strs;
    strs.precision(15); // rounding precision for system time double value
    strs << sysTimestamp;
    std::string sysTimeStr = strs.str();

    //if csv-mode: write csv header and timestamp at beginning
    if (m_OutputFormat == mitk::NavigationDataRecorderDeprecated::csv)
      {
      //write header only when it's the first line
      if (m_firstLine)
        {
        m_firstLine = false;
        *m_Stream << "TimeStamp";
        for (unsigned int index = 0; index < inputs.size(); index++){ *m_Stream << ";Valid_Tool" << index <<
                                                                                ";X_Tool" << index <<
                                                                                ";Y_Tool" << index <<
                                                                                ";Z_Tool" << index <<
                                                                                ";QX_Tool" << index <<
                                                                                ";QY_Tool" << index <<
                                                                                ";QZ_Tool" << index <<
                                                                                ";QR_Tool" << index;}
        *m_Stream << "\n";
        }
      //write timestamp (always)
      *m_Stream << timestamp;
      }

    //write tool data for every tool
    for (unsigned int index = 0; index < inputs.size(); index++)
    {
      mitk::NavigationData* nd = dynamic_cast<mitk::NavigationData*>(inputs[index].GetPointer());
      nd->Update(); // call update to propagate update to previous filters

      mitk::NavigationData::PositionType position;
      mitk::NavigationData::OrientationType orientation(0.0, 0.0, 0.0, 0.0);
      mitk::NavigationData::CovarianceMatrixType matrix;

      bool hasPosition = true;
      bool hasOrientation = true;
      bool dataValid = false;

      position.Fill(0.0);
      matrix.SetIdentity();

      position = nd->GetPosition();
      orientation = nd->GetOrientation();
      matrix = nd->GetCovErrorMatrix();

      hasPosition = nd->GetHasPosition();
      hasOrientation = nd->GetHasOrientation();
      dataValid = nd->IsDataValid();

      //use this one if you want the timestamps of the source
      //timestamp = nd->GetIGTTimeStamp();

      //a timestamp is never < 0! this case happens only if you are using the timestamp of the nd object instead of getting a new one
      if (timestamp >= 0)
      {
        if (this->m_OutputFormat == mitk::NavigationDataRecorderDeprecated::xml)
          {
          TiXmlElement* elem = new TiXmlElement("NavigationData");

          elem->SetDoubleAttribute("Time", timestamp);
          elem->SetAttribute("SystemTime", sysTimeStr); // tag for system time
          elem->SetDoubleAttribute("Tool", index);
          elem->SetDoubleAttribute("X", position[0]);
          elem->SetDoubleAttribute("Y", position[1]);
          elem->SetDoubleAttribute("Z", position[2]);

          elem->SetDoubleAttribute("QX", orientation[0]);
          elem->SetDoubleAttribute("QY", orientation[1]);
          elem->SetDoubleAttribute("QZ", orientation[2]);
          elem->SetDoubleAttribute("QR", orientation[3]);

          elem->SetDoubleAttribute("C00", matrix[0][0]);
          elem->SetDoubleAttribute("C01", matrix[0][1]);
          elem->SetDoubleAttribute("C02", matrix[0][2]);
          elem->SetDoubleAttribute("C03", matrix[0][3]);
          elem->SetDoubleAttribute("C04", matrix[0][4]);
          elem->SetDoubleAttribute("C05", matrix[0][5]);
          elem->SetDoubleAttribute("C10", matrix[1][0]);
          elem->SetDoubleAttribute("C11", matrix[1][1]);
          elem->SetDoubleAttribute("C12", matrix[1][2]);
          elem->SetDoubleAttribute("C13", matrix[1][3]);
          elem->SetDoubleAttribute("C14", matrix[1][4]);
          elem->SetDoubleAttribute("C15", matrix[1][5]);

//.........这里部分代码省略.........

Actor*
Parser::parseBox(xml_node_iterator sceneElement)
{
	vec3 center(0, 0, 0);
	quat orientation(vec3(0, 0, 0));
	vec3 scale(1, 1, 1);

	// opt values
	xml_node op;
	op = sceneElement->child("center");
	REAL x, y, z;
	if (op != NULL)
	{
		const char* center_vector = op.text().as_string();

		sscanf(center_vector, "%f %f %f", &x, &y, &z);
		center.set(x, y, z);
	}
	op = sceneElement->child("orientation");
	if (op != NULL){
		const char* orientation_vec = op.text().as_string();

		sscanf(orientation_vec, "%f %f %f", &x, &y, &z);
		orientation.eulerAngles(vec3(x, y, z));
	}
	op = sceneElement->child("scale");
	if (op != NULL){
		const char* scale_vec = op.text().as_string();

		sscanf(scale_vec, "%f %f %f", &x, &y, &z);
		scale.set(vec3(x, y, z));
	}

	// now, lets make the mesh of Sphere
	TriangleMesh* boxMesh = MeshSweeper::makeBox(center, orientation, scale);

	Primitive* primitive = new TriangleMeshShape(boxMesh);

	if ((op = sceneElement->child("transform")) != NULL) {
		vec3 position(0, 0, 0);
		quat q(vec3(0, 0, 0));
		vec3 scale(1, 1, 1);
		float x, y, z;

		xml_object_range<xml_node_iterator> transformations = op.children();

		for (xml_node_iterator transformation = transformations.begin(); transformation != transformations.end(); ++transformation)
		{
			if (strcmp(transformation->name(), "position") == 0)
			{
				const char * stringTranslation = transformation->text().as_string();

				sscanf(stringTranslation, "%f %f %f", &x, &y, &z);
				vec3 translationVec(x, y, z);
				position = translationVec;
			}
			else if (strcmp(transformation->name(), "scale") == 0)
			{
				float s_ = transformation->text().as_float();
				vec3 newS(s_, s_, s_);
				scale = newS;
			}
			else if (strcmp(transformation->name(), "rotation") == 0)
			{
				float angle = transformation->child("angle").text().as_float();

				const char * _Axis = transformation->child("axis").text().as_string();
				sscanf(_Axis, "%f %f %f", &x, &y, &z);
				vec3 axis(x, y, z);
				q = quat(axis, angle);
			}

			boxMesh->transform(mat4::TRS(position, q, scale));
		}
	}
	if ((op = sceneElement->child("material")) != NULL) {
		Material * material = parseMaterial(op);
		primitive->setMaterial(material);
	}
	else
	{
		Material * material = MaterialFactory::New();
		primitive->setMaterial(material->getDefault());

	}

	Actor* act = new Actor(*primitive);
	act->setName("box");

	return act;
}

void RDHeaderView::paintSection(QPainter *painter, const QRect &rect, int section) const
{
  if(!m_customSizing)
    return QHeaderView::paintSection(painter, rect, section);

  if(!rect.isValid())
    return;

  QStyleOptionHeader opt;
  initStyleOption(&opt);

  QAbstractItemModel *m = this->model();

  if(hasFocus())
    opt.state |= (QStyle::State_Active | QStyle::State_HasFocus);
  else
    opt.state &= ~(QStyle::State_Active | QStyle::State_HasFocus);

  QVariant textAlignment = m->headerData(section, orientation(), Qt::TextAlignmentRole);
  opt.rect = rect;
  opt.section = section;
  opt.textAlignment = defaultAlignment();
  opt.iconAlignment = Qt::AlignVCenter;

  QVariant variant;

  if(m_columnGroupRole)
  {
    variant = m->headerData(section, orientation(), m_columnGroupRole);
    if(variant.isValid() && variant.canConvert<QString>())
      opt.text = variant.toString();
  }

  if(opt.text.isEmpty())
    opt.text = m->headerData(section, orientation(), Qt::DisplayRole).toString();

  int margin = 2 * style()->pixelMetric(QStyle::PM_HeaderMargin, 0, this);

  if(textElideMode() != Qt::ElideNone)
    opt.text = opt.fontMetrics.elidedText(opt.text, textElideMode(), rect.width() - margin);

  if(section == 0 && section == m_sections.count() - 1)
    opt.position = QStyleOptionHeader::OnlyOneSection;
  else if(section == 0)
    opt.position = QStyleOptionHeader::Beginning;
  else if(section == m_sections.count() - 1)
    opt.position = QStyleOptionHeader::End;
  else
    opt.position = QStyleOptionHeader::Middle;

  opt.orientation = orientation();

  bool prevSel = section > 0 && selectionModel()->isColumnSelected(section - 1, QModelIndex());
  bool nextSel = section + 1 < m_sections.count() &&
                 selectionModel()->isColumnSelected(section + 1, QModelIndex());

  if(prevSel && nextSel)
    opt.selectedPosition = QStyleOptionHeader::NextAndPreviousAreSelected;
  else if(prevSel)
    opt.selectedPosition = QStyleOptionHeader::PreviousIsSelected;
  else if(nextSel)
    opt.selectedPosition = QStyleOptionHeader::NextIsSelected;
  else
    opt.selectedPosition = QStyleOptionHeader::NotAdjacent;

  style()->drawControl(QStyle::CE_Header, &opt, painter, this);
}

void RDHeaderView::cacheSections()
{
  if(m_suppressSectionCache)
    return;

  QAbstractItemModel *m = this->model();

  int oldCount = m_sections.count();
  m_sections.resize(m->columnCount());

  // give new sections a default minimum size
  for(int col = oldCount; col < m_sections.count(); col++)
    m_sections[col].size = 10;

  for(int col = 0; col < m_sections.count(); col++)
  {
    if(m_columnGroupRole > 0)
    {
      QVariant v = m->data(m->index(0, col), m_columnGroupRole);
      if(v.isValid())
        m_sections[col].group = v.toInt();
      else
        m_sections[col].group = -m_columnGroupRole - col;

      if(col > 0)
      {
        m_sections[col - 1].groupGap =
            (m_sections[col].group != m_sections[col - 1].group) && m_sections[col].group >= 0;
      }
    }
    else
    {
      m_sections[col].group = col;
      m_sections[col].groupGap = true;
    }
  }

  int accum = 0;

  for(int col = 0; col < m_sections.count(); col++)
  {
    if(col == m_pinnedColumns)
      m_pinnedWidth = accum;

    m_sections[col].offset = accum;
    accum += m_sections[col].size;

    if(hasGroupGap(col))
      accum += groupGapSize();
  }

  if(m_pinnedColumns >= m_sections.count())
    m_pinnedWidth = m_pinnedColumns;

  QStyleOptionHeader opt;
  initStyleOption(&opt);

  QFont f = font();
  f.setBold(true);

  opt.section = 0;
  opt.fontMetrics = QFontMetrics(f);
  opt.text = m->headerData(0, orientation(), Qt::DisplayRole).toString();

  m_sizeHint = style()->sizeFromContents(QStyle::CT_HeaderSection, &opt, QSize(), this);
  m_sizeHint.setWidth(accum);

  viewport()->update(viewport()->rect());
}

QStyleOptionHeader HierarchicalHeaderView::styleOptionForCell(int logicalInd) const
{
	QStyleOptionHeader opt;
	initStyleOption(&opt);
	if (window()->isActiveWindow())
		opt.state |= QStyle::State_Active;
	opt.textAlignment = Qt::AlignCenter;
	opt.iconAlignment = Qt::AlignVCenter;
	opt.section = logicalInd;

	int visual = visualIndex(logicalInd);

	if (count() == 1)
		opt.position = QStyleOptionHeader::OnlyOneSection;
	else
	{
		if (visual == 0)
			opt.position = QStyleOptionHeader::Beginning;
		else
			opt.position=(visual==count()-1 ? QStyleOptionHeader::End : QStyleOptionHeader::Middle);
	}

    if(isClickable())
	{
/*
        if (logicalIndex == d->hover)
            state |= QStyle::State_MouseOver;
        if (logicalIndex == d->pressed)
		{
            state |= QStyle::State_Sunken;
		}
        else*/
		{
			if(highlightSections() && selectionModel()) 
			{
                if(orientation()==Qt::Horizontal)
                {
				    if(selectionModel()->columnIntersectsSelection(logicalInd, rootIndex()))
					    opt.state |= QStyle::State_On;
                    if(selectionModel()->isColumnSelected(logicalInd, rootIndex()))
                        opt.state |= QStyle::State_Sunken;
                }
                else
                {
				    if(selectionModel()->rowIntersectsSelection(logicalInd, rootIndex()))
					    opt.state |= QStyle::State_On;
                    if(selectionModel()->isRowSelected(logicalInd, rootIndex()))
                        opt.state |= QStyle::State_Sunken;
                }
			}
        }
    }
	if(selectionModel())
	{
	    bool previousSelected=false;
        if(orientation()==Qt::Horizontal)
            previousSelected = selectionModel()->isColumnSelected(logicalIndex(visual - 1), rootIndex());
        else
            previousSelected = selectionModel()->isRowSelected(logicalIndex(visual - 1), rootIndex());
		bool nextSelected=false;
        if(orientation()==Qt::Horizontal)
            nextSelected = selectionModel()->isColumnSelected(logicalIndex(visual + 1), rootIndex());
        else
            nextSelected = selectionModel()->isRowSelected(logicalIndex(visual + 1), rootIndex());
		if (previousSelected && nextSelected)
			opt.selectedPosition = QStyleOptionHeader::NextAndPreviousAreSelected;
		else 
		{
			if (previousSelected)
				opt.selectedPosition = QStyleOptionHeader::PreviousIsSelected;
			else
			{
				if (nextSelected)
					opt.selectedPosition = QStyleOptionHeader::NextIsSelected;
				else
					opt.selectedPosition = QStyleOptionHeader::NotAdjacent;
			}
		}
	}
	return opt;
}

void RDHeaderView::resizeSectionsWithHints()
{
  if(m_sectionMinSizes.count() == 0)
    return;

  QVector<int> sizes = m_sectionMinSizes;

  int available = 0;

  if(orientation() == Qt::Horizontal)
    available = rect().width();
  else
    available = rect().height();

  // see if we even have any extra space to allocate
  if(available > m_sectionMinSizesTotal)
  {
    // this is how much space we can allocate to stretch sections
    available -= m_sectionMinSizesTotal;

    // distribute the available space between the sections. Dividing by the total stretch tells us
    // how many 'whole' multiples we can allocate:
    int wholeMultiples = available / m_sectionStretchHintTotal;

    if(wholeMultiples > 0)
    {
      for(int i = 0; i < sizes.count() && i < m_sectionStretchHints.count(); i++)
      {
        int hint = m_sectionStretchHints[i];
        if(hint > 0)
          sizes[i] += wholeMultiples * hint;
      }
    }

    available -= wholeMultiples * m_sectionStretchHintTotal;

    // we now have a small amount (less than m_sectionStretchHintTotal) of extra space to allocate.
    // we still want to assign this leftover proportional to the hints, otherwise we'd end up with a
    // stair-stepping effect.
    // To do this we calculate hint/total for each section then loop around adding on fractional
    // components to the sizes until one is above 1, then it gets a pixel, and we keep going until
    // all the remainder is allocated
    QVector<float> fractions, increment;
    fractions.resize(sizes.count());
    increment.resize(sizes.count());

    // set up increments
    for(int i = 0; i < sizes.count(); i++)
    {
      // don't assign any space to sections with negative hints, or sections without hints
      if(i >= m_sectionStretchHints.count() || m_sectionStretchHints[i] <= 0)
      {
        increment[i] = 0.0f;
        continue;
      }

      increment[i] = float(m_sectionStretchHints[i]) / float(m_sectionStretchHintTotal);
    }

    while(available > 0)
    {
      // loop along each section incrementing it.
      for(int i = 0; i < fractions.count(); i++)
      {
        fractions[i] += increment[i];

        // if we have a whole pixel now, assign it
        if(fractions[i] > 1.0f)
        {
          fractions[i] -= 1.0f;
          sizes[i]++;
          available--;

          // if we've assigned all pixels, stop
          if(available == 0)
            break;
        }
      }
    }

    for(int pix = 0; pix < available; pix++)
    {
      int minSection = 0;
      for(int i = 1; i < sizes.count(); i++)
      {
        // don't assign any space to sections with negative hints
        if(i < m_sectionStretchHints.count() && m_sectionStretchHints[i] <= 0)
          continue;

        if(sizes[i] < sizes[minSection])
          minSection = i;
      }

      sizes[minSection]++;
    }
  }

  resizeSections(sizes.toList());
}

void PointCloudDisplay::transformCloud()
{
  if ( message_.header.frame_id.empty() )
  {
    message_.header.frame_id = fixed_frame_;
  }

  tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), message_.header.stamp, message_.header.frame_id );

  try
  {
    tf_->transformPose( fixed_frame_, pose, pose );
  }
  catch(tf::TransformException& e)
  {
    ROS_ERROR( "Error transforming point cloud '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message_.header.frame_id.c_str(), fixed_frame_.c_str() );
  }

  Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
  robotToOgre( position );

  btScalar yaw, pitch, roll;
  pose.getBasis().getEulerZYX( yaw, pitch, roll );

  Ogre::Matrix3 orientation( ogreMatrixFromRobotEulers( yaw, pitch, roll ) );

  // First find the min/max intensity values
  float min_intensity = 999999.0f;
  float max_intensity = -999999.0f;

  typedef std::vector<std_msgs::ChannelFloat32> V_Chan;
  typedef std::vector<bool> V_bool;

  V_bool valid_channels(message_.chan.size());
  uint32_t point_count = message_.get_pts_size();
  V_Chan::iterator chan_it = message_.chan.begin();
  V_Chan::iterator chan_end = message_.chan.end();
  uint32_t index = 0;
  for ( ; chan_it != chan_end; ++chan_it, ++index )
  {
    std_msgs::ChannelFloat32& chan = *chan_it;
    uint32_t val_count = chan.vals.size();
    bool channel_size_correct = val_count == point_count;
    ROS_ERROR_COND(!channel_size_correct, "Point cloud '%s' on topic '%s' has channel 0 with fewer values than points (%d values, %d points)", name_.c_str(), topic_.c_str(), val_count, point_count);

    valid_channels[index] = channel_size_correct;

    if ( channel_size_correct && ( chan.name.empty() || chan.name == "intensity" || chan.name == "intensities" ) )
    {
      for(uint32_t i = 0; i < point_count; i++)
      {
        float& intensity = chan.vals[i];
        // arbitrarily cap to 4096 for now
        intensity = std::min( intensity, 4096.0f );
        min_intensity = std::min( min_intensity, intensity );
        max_intensity = std::max( max_intensity, intensity );
      }
    }
  }

  float diff_intensity = max_intensity - min_intensity;

  typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
  V_Point points;
  points.resize( point_count );
  for(uint32_t i = 0; i < point_count; i++)
  {
    Ogre::Vector3 color( color_.r_, color_.g_, color_.b_ );
    ogre_tools::PointCloud::Point& current_point = points[ i ];

    current_point.x_ = message_.pts[i].x;
    current_point.y_ = message_.pts[i].y;
    current_point.z_ = message_.pts[i].z;
    current_point.r_ = color.x;
    current_point.g_ = color.y;
    current_point.b_ = color.z;
  }

  chan_it = message_.chan.begin();
  index = 0;
  for ( ; chan_it != chan_end; ++chan_it, ++index )
  {
    if ( !valid_channels[index] )
    {
      continue;
    }

    std_msgs::ChannelFloat32& chan = *chan_it;
    enum ChannelType
    {
      CT_INTENSITY,
      CT_RGB,
      CT_R,
      CT_G,
      CT_B,

      CT_COUNT
    };

    ChannelType type = CT_INTENSITY;
//.........这里部分代码省略.........

QSize SplitterHandle::sizeHint() const
{
    return (orientation() == Qt::Horizontal) ? QSize( 1, height() ) : QSize( width(), 1 );
}

QSplitterHandle *PlaylistSplitter::createHandle()
{
    return new SplitterHandle( orientation(), this );
}

/*!
  Draw a tube

  Builds 2 curves from the upper and lower limits of the intervals
  and draws them with the pen(). The area between the curves is
  filled with the brush().

  \param painter Painter
  \param xMap Maps x-values into pixel coordinates.
  \param yMap Maps y-values into pixel coordinates.
  \param canvasRect Contents rect of the canvas
  \param from Index of the first sample to be painted
  \param to Index of the last sample to be painted. If to < 0 the
         series will be painted to its last sample.

  \sa drawSeries(), drawSymbols()
*/
void QwtPlotIntervalCurve::drawTube( QPainter *painter,
    const QwtScaleMap &xMap, const QwtScaleMap &yMap,
    const QRectF &canvasRect, int from, int to ) const
{
    const bool doAlign = QwtPainter::roundingAlignment( painter );

    painter->save();

    const size_t size = to - from + 1;
    QPolygonF polygon( 2 * size );
    QPointF *points = polygon.data();

    for ( uint i = 0; i < size; i++ )
    {
        QPointF &minValue = points[i];
        QPointF &maxValue = points[2 * size - 1 - i];

        const QwtIntervalSample intervalSample = sample( from + i );
        if ( orientation() == Qt::Vertical )
        {
            double x = xMap.transform( intervalSample.value );
            double y1 = yMap.transform( intervalSample.interval.minValue() );
            double y2 = yMap.transform( intervalSample.interval.maxValue() );
            if ( doAlign )
            {
                x = qRound( x );
                y1 = qRound( y1 );
                y2 = qRound( y2 );
            }

            minValue.rx() = x;
            minValue.ry() = y1;
            maxValue.rx() = x;
            maxValue.ry() = y2;
        }
        else
        {
            double y = yMap.transform( intervalSample.value );
            double x1 = xMap.transform( intervalSample.interval.minValue() );
            double x2 = xMap.transform( intervalSample.interval.maxValue() );
            if ( doAlign )
            {
                y = qRound( y );
                x1 = qRound( x1 );
                x2 = qRound( x2 );
            }

            minValue.rx() = x1;
            minValue.ry() = y;
            maxValue.rx() = x2;
            maxValue.ry() = y;
        }
    }

    if ( d_data->brush.style() != Qt::NoBrush )
    {
        painter->setPen( QPen( Qt::NoPen ) );
        painter->setBrush( d_data->brush );

        if ( d_data->paintAttributes & ClipPolygons )
        {
            const qreal m = 1.0;
            const QPolygonF p = QwtClipper::clipPolygonF( 
                canvasRect.adjusted(-m, -m, m, m), polygon, true );

            QwtPainter::drawPolygon( painter, p );
        }
        else
        {
            QwtPainter::drawPolygon( painter, polygon );
        }
    }

    if ( d_data->pen.style() != Qt::NoPen )
    {
        painter->setPen( d_data->pen );
        painter->setBrush( Qt::NoBrush );

        if ( d_data->paintAttributes & ClipPolygons )
        {
            qreal pw = qMax( qreal( 1.0 ), painter->pen().widthF());
            const QRectF clipRect = canvasRect.adjusted(-pw, -pw, pw, pw);

//.........这里部分代码省略.........

/* ==================================== */
int32_t lattribute(
        struct xvimage *img, /* image de depart */
        int32_t connex,          /* 4, 8  */
        int32_t typregion,       /* = <LABMIN | LABMAX | LABPLATEAU> */
        int32_t attrib,          /* 0: surface, 1: perimetre, 2: circularite, 3: nb. trous, 
                                4: excentricite, 5: orientation, 6: diamètre vertical, 7: diamètre horizontal */
        int32_t seuil,           /* en dessous (<=) de seuil, l'attribut est mis a 0 */
        struct xvimage *lab, /* resultat: image d'attributs */
        int32_t *nlabels)        /* resultat: nombre de regions traitees */
/* ==================================== */
#undef F_NAME
#define F_NAME "lattribute"
{
  int32_t k, l;
  index_t w, x, y, z;
  uint8_t *SOURCE = UCHARDATA(img);
  int32_t *LABEL = SLONGDATA(lab);
  index_t rs = rowsize(img);
  index_t cs = colsize(img);
  index_t d = depth(img);
  index_t N = rs * cs;          /* taille image */
  Lifo * LIFO;
  int32_t label;
  int32_t area;
  int32_t perim;
  int32_t min, max;
  int32_t val_attrib;
  double mx1, my1; // cumuls des variables x et y
  double mx2, my2, mxy2; // cumuls des x^2, y^2 et xy
  int32_t incr_vois;

  if (datatype(lab) != VFF_TYP_4_BYTE) 
  {
    fprintf(stderr, "%s: le resultat doit etre de type VFF_TYP_4_BYTE\n", F_NAME);
    return 0;
  }

  if ((rowsize(lab) != rs) || (colsize(lab) != cs) || (depth(lab) != d))
  {
    fprintf(stderr, "%s: tailles images incompatibles\n", F_NAME);
    return 0;
  }

  if (depth(img) != 1) 
  {
    fprintf(stderr, "%s: cette version ne traite pas les images volumiques\n", F_NAME);
    exit(0);
  }

  switch (connex)
  {
    case 4: incr_vois = 2; break;
    case 8: incr_vois = 1; break;
    default: 
      fprintf(stderr, "%s: mauvaise connexite: %d\n", F_NAME, connex);
      return 0;
  } /* switch (connex) */

  /* le LABEL initialement est mis a NONMARQUE */
  for (x = 0; x < N; x++) LABEL[x] = NONMARQUE;

  LIFO = CreeLifoVide(N);
  if (LIFO == NULL)
    {   fprintf(stderr, "%s: CreeLifoVide failed\n", F_NAME);
      return(0);
  }

  *nlabels = 0;

if ((typregion == LABMIN) || (typregion == LABMAX))
{
  for (x = 0; x < N; x++)
  {
    if (LABEL[x] == NONMARQUE)   /* on trouve un point x non etiquete */
    {
      *nlabels += 1;
      LABEL[x] = MARQUE;
#ifdef DEBUGTROU
printf("AMORCE p=%d,%d h=%d set LABEL = %d\n", x%rs, x/rs, SOURCE[x], LABEL[x]);
#endif
      switch (attrib)            /* on initialise les attributs de cette composante */
      {
	case AREA: val_attrib = 0; break;
	case PERIM: val_attrib = 0; break;
	case TROUS: val_attrib = 0; break;
	case CIRC: area = perim = 0; break;
	case EXCEN: 
	case ORIEN: area = 0; mx1 = my1 = mx2 = my2 = mxy2 = 0.0; break;
        case VDIAM: min = cs-1; max = 0; break;
        case HDIAM: min = rs-1; max = 0; break;
        default: 
          fprintf(stderr, "%s: mauvais attribut: %d\n", F_NAME, attrib);
          return 0;
      } /* switch (attrib) */
      LifoPush(LIFO, x);         /* on va parcourir le plateau auquel appartient x */
      while (! LifoVide(LIFO))
      {
        w = LifoPop(LIFO);
        label = LABEL[w];
//.........这里部分代码省略.........

void mitk::ClaronTrackingDevice::TrackTools()
{
  try
  {
    /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */
    MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope

    bool localStopTracking;       // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here
    this->m_StopTrackingMutex->Lock();  // update the local copy of m_StopTracking
    localStopTracking = this->m_StopTracking;
    this->m_StopTrackingMutex->Unlock();

    while ((this->GetState() == Tracking) && (localStopTracking == false))
    {
      this->GetDevice()->GrabFrame();

      std::vector<mitk::ClaronTool::Pointer> detectedTools = this->DetectTools();
      std::vector<mitk::ClaronTool::Pointer> allTools = this->GetAllTools();
      std::vector<mitk::ClaronTool::Pointer>::iterator itAllTools;
      for(itAllTools = allTools.begin(); itAllTools != allTools.end(); itAllTools++)
      {
        mitk::ClaronTool::Pointer currentTool = *itAllTools;
        //test if current tool was detected
        std::vector<mitk::ClaronTool::Pointer>::iterator itDetectedTools;
        bool foundTool = false;
        for(itDetectedTools = detectedTools.begin(); itDetectedTools != detectedTools.end(); itDetectedTools++)
        {
          mitk::ClaronTool::Pointer aktuDet = *itDetectedTools;
          std::string tempString(currentTool->GetCalibrationName());
          if (tempString.compare(aktuDet->GetCalibrationName())==0)
          {
            currentTool->SetToolHandle(aktuDet->GetToolHandle());
            foundTool = true;
          }
        }
        if (!foundTool)
        {
          currentTool->SetToolHandle(0);
        }

        if (currentTool->GetToolHandle() != 0)
        {
          currentTool->SetDataValid(true);
          //get tip position of tool:
          std::vector<double> pos_vector = this->GetDevice()->GetTipPosition(currentTool->GetToolHandle());
          //write tip position into tool:
          mitk::Point3D pos;
          pos[0] = pos_vector[0];
          pos[1] = pos_vector[1];
          pos[2] = pos_vector[2];
          currentTool->SetPosition(pos);
          //get tip quaternion of tool
          std::vector<double> quat = this->GetDevice()->GetTipQuaternions(currentTool->GetToolHandle());
          //write tip quaternion into tool
          mitk::Quaternion orientation(quat[1], quat[2], quat[3], quat[0]);
          currentTool->SetOrientation(orientation);
        }
        else
        {
          mitk::Point3D origin;
          origin.Fill(0);
          currentTool->SetPosition(origin);
          currentTool->SetOrientation(mitk::Quaternion(0,0,0,0));
          currentTool->SetDataValid(false);
        }
      }
      /* Update the local copy of m_StopTracking */
      this->m_StopTrackingMutex->Lock();
      localStopTracking = m_StopTracking;
      this->m_StopTrackingMutex->Unlock();
    }
  }
  catch(...)
  {
    this->StopTracking();
    this->SetErrorMessage("Error while trying to track tools. Thread stopped.");
  }
}

// update updates the position and orientation of the Sound Instance
//
void Sound::update() {

	if (apiSound && local) 
	    apiSound->update(position(), orientation('z'));
}

/*! @brief Updates the held information ready for a new frame.
*   Gets copies of the actions and sensors pointers from the blackboard and
*   gets a new image from the blackboard. Updates framecounts.
*   @return Whether the fetched data is valid.
*/
bool DataWrapper::updateFrame()
{
    digitalWrite (17, LOW);
    digitalWrite (18, LOW);
    digitalWrite (22, LOW);
    digitalWrite (23, LOW);

    if(m_ok) {
        if(!imagestrm.is_open()) {
            errorlog << "No image stream - " << streamname << std::endl;
            return false;
        }
        if(using_sensors && !sensorstrm.is_open()) {
            errorlog << "No sensor stream - " << sensorstreamname << std::endl;
            return false;
        }
        try {
            imagestrm >> current_frame;
        }
        catch(std::exception& e) {
            return false;
        }
        if(using_sensors) {
            try {
                sensorstrm >> sensor_data;
            }
            catch(std::exception& e){
                errorlog << "Sensor stream error: " << e.what() << std::endl;
                return false;
            }
        }

        //overwrite sensor horizon if using sensors
        std::vector<float> hor_data;
        if(using_sensors && sensor_data.getHorizon(hor_data)) {
            kinematics_horizon.setLine(hor_data.at(0), hor_data.at(1), hor_data.at(2));
        }

        //update kinematics snapshot
        if(using_sensors) {

            vector<float> orientation(3, 0);