visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToCentroids(const autoware_msgs::DetectedObjectArray &in_objects)
{
  visualization_msgs::MarkerArray centroid_markers;
  for (auto const &object: in_objects.objects)
  {
    if (IsObjectValid(object))
    {
      visualization_msgs::Marker centroid_marker;
      centroid_marker.lifetime = ros::Duration(marker_display_duration_);

      centroid_marker.header = in_objects.header;
      centroid_marker.type = visualization_msgs::Marker::SPHERE;
      centroid_marker.action = visualization_msgs::Marker::ADD;
      centroid_marker.pose = object.pose;
      centroid_marker.ns = ros_namespace_ + "/centroid_markers";

      centroid_marker.scale.x = 0.5;
      centroid_marker.scale.y = 0.5;
      centroid_marker.scale.z = 0.5;

      if (object.color.a == 0)
      {
        centroid_marker.color = centroid_color_;
      }
      else
      {
        centroid_marker.color = object.color;
      }
      centroid_marker.id = marker_id_++;
      centroid_markers.markers.push_back(centroid_marker);
    }
  }
  return centroid_markers;
}//ObjectsToCentroids
NTSTATUS
HOOKPORT_CALLTYPE
HookNtSetInformationThread(
    PSYSCALL_INFO               SysCallInfo,
    PSYSTEM_CALL_ACTION         Action,
    HANDLE                      ThreadHandle,
    THREAD_INFORMATION_CLASS    ThreadInformationClass,
    PVOID                       ThreadInformation,
    ULONG                       ThreadInformationLength
)
{
    NTSTATUS Status;

    switch (ThreadInformationClass)
    {
        case ThreadHideFromDebugger:
            Status = IsObjectValid(ThreadHandle);
            if (NT_SUCCESS(Status))
                break;

            return Status;

        default:
            return STATUS_SUCCESS;
    }

    Action->Action = BlockSystemCall;

    return Status;
}
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToModels(const autoware_msgs::DetectedObjectArray &in_objects)
{
  visualization_msgs::MarkerArray object_models;

  for (auto const &object: in_objects.objects)
  {
    if (IsObjectValid(object) &&
      object.label != "unknown" &&
        (object.dimensions.x + object.dimensions.y + object.dimensions.z) < object_max_linear_size_)
    {
      visualization_msgs::Marker model;

      model.lifetime = ros::Duration(marker_display_duration_);
      model.header = in_objects.header;
      model.type = visualization_msgs::Marker::MESH_RESOURCE;
      model.action = visualization_msgs::Marker::ADD;
      model.ns = ros_namespace_ + "/model_markers";
      model.mesh_use_embedded_materials = false;
      model.color = model_color_;
      if(object.label == "car")
      {
        model.mesh_resource = "package://detected_objects_visualizer/models/car.dae";
      }
      else if (object.label == "person")
      {
        model.mesh_resource = "package://detected_objects_visualizer/models/person.dae";
      }
      else if (object.label == "bicycle" || object.label == "bike")
      {
        model.mesh_resource = "package://detected_objects_visualizer/models/bike.dae";
      }
      else if (object.label == "bus")
      {
        model.mesh_resource = "package://detected_objects_visualizer/models/bus.dae";
      }
      else if(object.label == "truck")
      {
        model.mesh_resource = "package://detected_objects_visualizer/models/truck.dae";
      }
      else
      {
        model.mesh_resource = "package://detected_objects_visualizer/models/box.dae";
      }
      model.scale.x = 1;
      model.scale.y = 1;
      model.scale.z = 1;
      model.id = marker_id_++;
      model.pose.position = object.pose.position;
      model.pose.position.z-= object.dimensions.z/2;

      if (object.pose_reliable)
        model.pose.orientation = object.pose.orientation;

      object_models.markers.push_back(model);
    }
  }
  return object_models;
}//ObjectsToModels
NTSTATUS
HOOKPORT_CALLTYPE
HookNtClose(
    PSYSCALL_INFO       SysCallInfo,
    PSYSTEM_CALL_ACTION Action,
    HANDLE              Handle
)
{
    NTSTATUS Status;

    Status = IsObjectValid(Handle);
    if (!NT_SUCCESS(Status))
        Action->Action = BlockSystemCall;

    return Status;
}
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToHulls(const autoware_msgs::DetectedObjectArray &in_objects)
{
  visualization_msgs::MarkerArray polygon_hulls;

  for (auto const &object: in_objects.objects)
  {
    if (IsObjectValid(object) && !object.convex_hull.polygon.points.empty() && object.label == "unknown")
    {
      visualization_msgs::Marker hull;
      hull.lifetime = ros::Duration(marker_display_duration_);
      hull.header = in_objects.header;
      hull.type = visualization_msgs::Marker::LINE_STRIP;
      hull.action = visualization_msgs::Marker::ADD;
      hull.ns = ros_namespace_ + "/hull_markers";
      hull.id = marker_id_++;
      hull.scale.x = 0.2;

      for(auto const &point: object.convex_hull.polygon.points)
      {
        geometry_msgs::Point tmp_point;
        tmp_point.x = point.x;
        tmp_point.y = point.y;
        tmp_point.z = point.z;
        hull.points.push_back(tmp_point);
      }

      if (object.color.a == 0)
      {
        hull.color = hull_color_;
      }
      else
      {
        hull.color = object.color;
      }

      polygon_hulls.markers.push_back(hull);
    }
  }
  return polygon_hulls;
}
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToBoxes(const autoware_msgs::DetectedObjectArray &in_objects)
{
  visualization_msgs::MarkerArray object_boxes;

  for (auto const &object: in_objects.objects)
  {
    if (IsObjectValid(object) &&
       object.label != "unknown" &&
        (object.dimensions.x + object.dimensions.y + object.dimensions.z) < object_max_linear_size_)
    {
      visualization_msgs::Marker box;

      box.lifetime = ros::Duration(marker_display_duration_);
      box.header = in_objects.header;
      box.type = visualization_msgs::Marker::CUBE;
      box.action = visualization_msgs::Marker::ADD;
      box.ns = ros_namespace_ + "/box_markers";
      box.id = marker_id_++;
      box.scale = object.dimensions;
      box.pose.position = object.pose.position;

      if (object.pose_reliable)
        box.pose.orientation = object.pose.orientation;

      if (object.color.a == 0)
      {
        box.color = box_color_;
      }
      else
      {
        box.color = object.color;
      }

      object_boxes.markers.push_back(box);
    }
  }
  return object_boxes;
}//ObjectsToBoxes
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToLabels(const autoware_msgs::DetectedObjectArray &in_objects)
{
  visualization_msgs::MarkerArray label_markers;
  for (auto const &object: in_objects.objects)
  {
    if (IsObjectValid(object))
    {
      visualization_msgs::Marker label_marker;

      label_marker.lifetime = ros::Duration(marker_display_duration_);
      label_marker.header = in_objects.header;
      label_marker.ns = ros_namespace_ + "/label_markers";
      label_marker.action = visualization_msgs::Marker::ADD;
      label_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
      label_marker.scale.x = 1.5;
      label_marker.scale.y = 1.5;
      label_marker.scale.z = 1.5;

      label_marker.color = label_color_;

      label_marker.id = marker_id_++;

      if(!object.label.empty() && object.label != "unknown")
        label_marker.text = object.label + " "; //Object Class if available

      std::stringstream distance_stream;
      distance_stream << std::fixed << std::setprecision(1)
                      << sqrt((object.pose.position.x * object.pose.position.x) +
                                (object.pose.position.y * object.pose.position.y));
      std::string distance_str = distance_stream.str() + " m";
      label_marker.text += distance_str;

      if (object.velocity_reliable)
      {
        double velocity = object.velocity.linear.x;
        if (velocity < -0.1)
        {
          velocity *= -1;
        }

        if (abs(velocity) < object_speed_threshold_)
        {
          velocity = 0.0;
        }

        tf::Quaternion q(object.pose.orientation.x, object.pose.orientation.y,
                         object.pose.orientation.z, object.pose.orientation.w);

        double roll, pitch, yaw;
        tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

        // convert m/s to km/h
        std::stringstream kmh_velocity_stream;
        kmh_velocity_stream << std::fixed << std::setprecision(1) << (velocity * 3.6);
        std::string text = "\n<" + std::to_string(object.id) + "> " + kmh_velocity_stream.str() + " km/h";
        label_marker.text += text;
      }

      label_marker.pose.position.x = object.pose.position.x;
      label_marker.pose.position.y = object.pose.position.y;
      label_marker.pose.position.z = label_height_;
      label_marker.scale.z = 1.0;
      if (!label_marker.text.empty())
        label_markers.markers.push_back(label_marker);
    }
  }  // end in_objects.objects loop

  return label_markers;
}//ObjectsToLabels
visualization_msgs::MarkerArray
VisualizeDetectedObjects::ObjectsToArrows(const autoware_msgs::DetectedObjectArray &in_objects)
{
  visualization_msgs::MarkerArray arrow_markers;
  for (auto const &object: in_objects.objects)
  {
    if (IsObjectValid(object) && object.pose_reliable)
    {
      double velocity = object.velocity.linear.x;

      if (abs(velocity) >= arrow_speed_threshold_)
      {
        visualization_msgs::Marker arrow_marker;
        arrow_marker.lifetime = ros::Duration(marker_display_duration_);

        tf::Quaternion q(object.pose.orientation.x,
                         object.pose.orientation.y,
                         object.pose.orientation.z,
                         object.pose.orientation.w);
        double roll, pitch, yaw;

        tf::Matrix3x3(q).getRPY(roll, pitch, yaw);

        // in the case motion model fit opposite direction
        if (velocity < -0.1)
        {
          yaw += M_PI;
          // normalize angle
          while (yaw > M_PI)
            yaw -= 2. * M_PI;
          while (yaw < -M_PI)
            yaw += 2. * M_PI;
        }

        tf::Matrix3x3 obs_mat;
        tf::Quaternion q_tf;

        obs_mat.setEulerYPR(yaw, 0, 0);  // yaw, pitch, roll
        obs_mat.getRotation(q_tf);

        arrow_marker.header = in_objects.header;
        arrow_marker.ns = ros_namespace_ + "/arrow_markers";
        arrow_marker.action = visualization_msgs::Marker::ADD;
        arrow_marker.type = visualization_msgs::Marker::ARROW;

        // green
        if (object.color.a == 0)
        {
          arrow_marker.color = arrow_color_;
        }
        else
        {
          arrow_marker.color = object.color;
        }
        arrow_marker.id = marker_id_++;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        arrow_marker.pose.position.x = object.pose.position.x;
        arrow_marker.pose.position.y = object.pose.position.y;
        arrow_marker.pose.position.z = arrow_height_;

        arrow_marker.pose.orientation.x = q_tf.getX();
        arrow_marker.pose.orientation.y = q_tf.getY();
        arrow_marker.pose.orientation.z = q_tf.getZ();
        arrow_marker.pose.orientation.w = q_tf.getW();

        // Set the scale of the arrow -- 1x1x1 here means 1m on a side
        arrow_marker.scale.x = 3;
        arrow_marker.scale.y = 0.1;
        arrow_marker.scale.z = 0.1;

        arrow_markers.markers.push_back(arrow_marker);
      }//velocity threshold
    }//valid object
  }//end for
  return arrow_markers;
}//ObjectsToArrows
void CVisDemoSorterClassifier::DoProcessing()
{
	Uint32 i;
	
	m_nNumObjectsClassifier_last = 0;
	
	if ( m_pModel == NULL )
		return;

	FastLabelObject *	objLabels = (FastLabelObject*)m_iportLabelData.GetBuffer();
	ColorObject *		objColors = (ColorObject*)m_iportColorData.GetBuffer();

	// Get extents of valid area
	CVisFixpoint fLeft, fRight, fTop, fBottom;
	m_pModel->GetValidArea( fLeft, fRight, fTop, fBottom );
	
				
	for ( i=1; i<objLabels[0].unNumObjects; i++)
	{	
		// Clear the object's flags.
		objLabels[i].unFlags = 0;
		
		// See if the object is valid (i.e. it is in a valid position and has sensibel
		// sizes.
		if ( IsObjectValid( objLabels[i] ) )
		{			
			// The object is valid.
			m_nNumObjectsClassified++;
			m_nNumObjectsClassifier_last++;
			m_nNumFramesWithoutObjects = 0;
			
			// Classify the object.
			Int nClass = ClassifiyColor( objColors[i].Color.unHue, objColors[i].Color.unSat, objColors[i].Color.unLum );
			
			// Increment count for that class.
			if ( nClass != -1 )
				m_aryColorClasses[nClass].nCount++;
				
			// Mark the object as due for ejection if the class is valid and scheduled for ejection.
			if ( (nClass != -1) && ( m_aryColorClasses[nClass].nEject != 0 ) )			
				objLabels[i].unFlags |= FLF_EJECT;
			else
				objLabels[i].unFlags &= ~FLF_EJECT;
				
		} // if object valid

	} // for all objects
	
	// Resolve critical ejections
	//ResolveCriticalEjections( objLabels );
	
	// Generate ejection commands
	for ( i=1; i<objLabels[0].unNumObjects; i++)
	{
		if ( (objLabels[i].unFlags & FLF_EJECT) != 0 )
			EjectObject( objLabels[i] );
	}	
	
#ifndef _WINDOWS
	// See if we've seen no objects for a while and turn of the jets if that's the case
	Uint32 unTime = timeGetHighResTime() + timeFromMs( 10 );
	if ( m_nNumFramesWithoutObjects > 25 )
	{	
		for ( int i=0; i<m_pModel->GetNumJets(); i++ )
			COutputDispatcher::Instance()->Channel( i ).AddCommand( unTime , false );
	}
#endif
}
NTSTATUS
HOOKPORT_CALLTYPE
HookNtQueryInformationProcess(
    PSYSCALL_INFO               SysCallInfo,
    PSYSTEM_CALL_ACTION         Action,
    HANDLE                      ProcessHandle,
    PROCESS_INFORMATION_CLASS   ProcessInformationClass,
    PVOID                       ProcessInformation,
    ULONG                       ProcessInformationLength,
    PULONG                      ReturnLength
)
{
    NTSTATUS Status;

/*
    static ULONG Count;

    PrintConsoleW(
        L"%08X: %S\n"
        L"ProcessInformationClass   = %08d\n"
        L"ProcessInformation        = %08X\n"
        L"ProcessInformationLength  = %08X\n"
        L"ReturnLength              = %08X\n"
        L"\n",

        Count++,
        g_pSysCallFilters[ServiceIndex].Reserve,
        ProcessInformationClass,
        ProcessInformation,
        ProcessInformationLength,
        ReturnLength
    );
*/
    Status = IsObjectValid(ProcessHandle);
    if (!NT_SUCCESS(Status))
        goto _Exit;

    switch (ProcessInformationClass)
    {
        case ProcessDebugObjectHandle:
            Status = STATUS_PORT_NOT_SET;

        case ProcessDebugPort:
            if (ProcessInformation != NULL)
            {
                ZeroMemory(ProcessInformation, ProcessInformationLength);
            }
            if (ReturnLength != NULL)
            {
                *ReturnLength = 4;
            }
            break;
/*
        case ProcessDebugFlags:
            if (ProcessInformation != 0)
                *(PULONG)ProcessInformation = TRUE;
            if (ReturnLength != NULL)
                *ReturnLength = 4;
            break;
*/
/*
        case ProcessBasicInformation:
            if (ProcessHandle != NtCurrentProcess())
                goto _Exit;

            Status = CallSysCall(
                        NtQueryInformationProcess,
                        SysCallInfo,
                        ProcessHandle,
                        ProcessInformationClass,
                        ProcessInformation,
                        ProcessInformationLength,
                        ReturnLength
                     );
            if (NT_SUCCESS(Status))
                ((PPROCESS_BASIC_INFORMATION)ProcessInformation)->InheritedFromUniqueProcessId = g_ExplorerPID;

            break;
*/
        default:
            goto _Exit;
    }

    Action->Action = BlockSystemCall;

_Exit:

    return Status;
}