<commit_before>/**
 * Copyright 2014 University of Bremen, Institute for Artificial Intelligence
 * Author: <NAME> <<EMAIL>>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#include <mutex>
#include <chrono>
#include <sys/stat.h>

#include <opencv2/opencv.hpp>

#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/SetCameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>

#include <compressed_depth_image_transport/compression_common.h>

#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/config.h>

#include <GLFW/glfw3.h>

#include <kinect2_definitions.h>
#include <depth_registration.h>

class Kinect2Bridge
{
public:
  enum DepthMethod
  {
    DEFAULT = 0,
    CPU,
    OPENCL,
    OPENGL
  };

private:
  const int32_t jpegQuality;
  const int32_t pngLevel;
  const double maxDepth;
  const size_t queueSize;
  std::vector<int> compressionParams;
  const bool rawDepth;
  std::string compression16BitExt;
  std::string compression16BitString;

  size_t frame;
  const cv::Size sizeColor, sizeIr, sizeDepth;
  cv::Mat color, ir, depth;
  cv::Mat cameraMatrixColor, distortionColor, cameraMatrixDepth;
  cv::Mat cameraMatrixIr, distortionIr;
  cv::Mat rotation, translation;
  cv::Mat map1Color, map2Color;
  cv::Mat map1Ir, map2Ir;
  cv::Mat map1ColorReg, map2ColorReg;

  std::vector<std::thread> threads;
  std::mutex lock;
  std::mutex lockProcess;
  std::mutex lockPub;
  std::mutex lockRegLowRes;
  std::mutex lockRegHighRes;
  size_t pubFrame;
  bool newFrame;

  libfreenect2::Freenect2 freenect2;
  libfreenect2::Freenect2Device *device;
  libfreenect2::SyncMultiFrameListener *listener;
  libfreenect2::FrameMap frames;
  libfreenect2::PacketPipeline *packetPipeline;

  ros::NodeHandle nh;

  DepthRegistration *depthRegLowRes;
  DepthRegistration *depthRegHighRes;

  const double deltaT;
  double depthShift;

  enum Image
  {
    IR = 0,
    IR_RECT,

    DEPTH,
    DEPTH_RECT,
    DEPTH_LORES,
    DEPTH_HIRES,

    COLOR,
    COLOR_RECT,
    COLOR_LORES,

    MONO,
    MONO_RECT,
    MONO_LORES,

    COUNT
  };

  enum Status
  {
    UNSUBCRIBED = 0,
    RAW,
    COMPRESSED,
    BOTH
  };

  std::vector<std::string> topics;

  std_msgs::Header header;
  std::vector<ros::Publisher> imagePubs;
  std::vector<ros::Publisher> compressedPubs;
  std::vector<ros::Publisher> infoPubs;
  std::vector<sensor_msgs::CameraInfo> infos;
  std::vector<Status> statusPubs;

public:
  Kinect2Bridge(const double fps, const bool rawDepth, const int compression, const int deviceIdDepth, const DepthRegistration::Method methodReg, const DepthMethod methodDepth, const bool useTiff)
    : jpegQuality(compression), pngLevel(1), maxDepth(10.0), queueSize(2), rawDepth(rawDepth), sizeColor(1920, 1080), sizeIr(512, 424),
      sizeDepth(rawDepth ? sizeIr : cv::Size(sizeColor.width / 2, sizeColor.height / 2)), newFrame(false), nh(),
      deltaT(fps > 0 ? 1.0 / fps : 0.0), depthShift(0), topics(COUNT)
  {
    topics[IR] = K2_TOPIC_IMAGE_IR;
    topics[IR_RECT] = K2_TOPIC_RECT_IR;
    topics[DEPTH] = K2_TOPIC_IMAGE_DEPTH;
    topics[DEPTH_RECT] = K2_TOPIC_RECT_DEPTH;
    topics[DEPTH_LORES] = K2_TOPIC_LORES_DEPTH;
    topics[DEPTH_HIRES] = K2_TOPIC_HIRES_DEPTH;
    topics[COLOR] = K2_TOPIC_IMAGE_COLOR;
    topics[COLOR_RECT] = K2_TOPIC_RECT_COLOR;
    topics[COLOR_LORES] = K2_TOPIC_LORES_COLOR;
    topics[MONO] = K2_TOPIC_IMAGE_MONO;
    topics[MONO_RECT] = K2_TOPIC_RECT_MONO;
    topics[MONO_LORES] = K2_TOPIC_LORES_MONO;

    color = cv::Mat::zeros(sizeColor, CV_8UC3);
    ir = cv::Mat::zeros(sizeIr, CV_32F);
    depth = cv::Mat::zeros(sizeIr, CV_32F);

    compressionParams.resize(7, 0);
    compressionParams[0] = CV_IMWRITE_JPEG_QUALITY;
    compressionParams[1] = jpegQuality;
    compressionParams[2] = CV_IMWRITE_PNG_COMPRESSION;
    compressionParams[3] = pngLevel;
    compressionParams[4] = CV_IMWRITE_PNG_STRATEGY;
    compressionParams[5] = CV_IMWRITE_PNG_STRATEGY_RLE;
    compressionParams[6] = 0;

    depthRegLowRes = DepthRegistration::New(methodReg);
    depthRegHighRes = DepthRegistration::New(methodReg);

    if(methodDepth == CPU)
    {
      packetPipeline = new libfreenect2::CpuPacketPipeline();
    }
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
    else if(methodDepth == OPENCL)
    {
      packetPipeline = new libfreenect2::OpenCLPacketPipeline(deviceIdDepth);
    }
#endif
    else if(methodDepth == OPENGL)
    {
      glfwInit();
      packetPipeline = new libfreenect2::OpenGLPacketPipeline();
    }
    else
    {
      glfwInit();
      packetPipeline = new libfreenect2::DefaultPacketPipeline();
    }

    if(useTiff)
    {
      compression16BitExt = ".tiff";
      compression16BitString = "; tiff compressed ";
    }<commit_msg>added configuration of depth packet processor.<commit_after>/**
 * Copyright 2014 University of Bremen, Institute for Artificial Intelligence
 * Author: <NAME> <<EMAIL>>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#include <mutex>
#include <chrono>
#include <sys/stat.h>

#include <opencv2/opencv.hpp>

#include <ros/ros.h>
#include <std_msgs/Header.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/SetCameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/image_encodings.h>

#include <compressed_depth_image_transport/compression_common.h>

#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/config.h>

#include <GLFW/glfw3.h>

#include <kinect2_definitions.h>
#include <depth_registration.h>

class Kinect2Bridge
{
public:
  enum DepthMethod
  {
    DEFAULT = 0,
    CPU,
    OPENCL,
    OPENGL
  };

private:
  const int32_t jpegQuality;
  const int32_t pngLevel;
  const double maxDepth;
  const size_t queueSize;
  std::vector<int> compressionParams;
  const bool rawDepth;
  std::string compression16BitExt;
  std::string compression16BitString;

  size_t frame;
  const cv::Size sizeColor, sizeIr, sizeDepth;
  cv::Mat color, ir, depth;
  cv::Mat cameraMatrixColor, distortionColor, cameraMatrixDepth;
  cv::Mat cameraMatrixIr, distortionIr;
  cv::Mat rotation, translation;
  cv::Mat map1Color, map2Color;
  cv::Mat map1Ir, map2Ir;
  cv::Mat map1ColorReg, map2ColorReg;

  std::vector<std::thread> threads;
  std::mutex lock;
  std::mutex lockProcess;
  std::mutex lockPub;
  std::mutex lockRegLowRes;
  std::mutex lockRegHighRes;
  size_t pubFrame;
  bool newFrame;

  libfreenect2::Freenect2 freenect2;
  libfreenect2::Freenect2Device *device;
  libfreenect2::SyncMultiFrameListener *listener;
  libfreenect2::FrameMap frames;
  libfreenect2::PacketPipeline *packetPipeline;

  ros::NodeHandle nh;

  DepthRegistration *depthRegLowRes;
  DepthRegistration *depthRegHighRes;

  const double deltaT;
  double depthShift;

  enum Image
  {
    IR = 0,
    IR_RECT,

    DEPTH,
    DEPTH_RECT,
    DEPTH_LORES,
    DEPTH_HIRES,

    COLOR,
    COLOR_RECT,
    COLOR_LORES,

    MONO,
    MONO_RECT,
    MONO_LORES,

    COUNT
  };

  enum Status
  {
    UNSUBCRIBED = 0,
    RAW,
    COMPRESSED,
    BOTH
  };

  std::vector<std::string> topics;

  std_msgs::Header header;
  std::vector<ros::Publisher> imagePubs;
  std::vector<ros::Publisher> compressedPubs;
  std::vector<ros::Publisher> infoPubs;
  std::vector<sensor_msgs::CameraInfo> infos;
  std::vector<Status> statusPubs;

public:
  Kinect2Bridge(const double fps, const bool rawDepth, const int compression, const int deviceIdDepth, const DepthRegistration::Method methodReg, const DepthMethod methodDepth, const bool useTiff)
    : jpegQuality(compression), pngLevel(1), maxDepth(12.0), queueSize(2), rawDepth(rawDepth), sizeColor(1920, 1080), sizeIr(512, 424),
      sizeDepth(rawDepth ? sizeIr : cv::Size(sizeColor.width / 2, sizeColor.height / 2)), newFrame(false), nh(),
      deltaT(fps > 0 ? 1.0 / fps : 0.0), depthShift(0), topics(COUNT)
  {
    topics[IR] = K2_TOPIC_IMAGE_IR;
    topics[IR_RECT] = K2_TOPIC_RECT_IR;
    topics[DEPTH] = K2_TOPIC_IMAGE_DEPTH;
    topics[DEPTH_RECT] = K2_TOPIC_RECT_DEPTH;
    topics[DEPTH_LORES] = K2_TOPIC_LORES_DEPTH;
    topics[DEPTH_HIRES] = K2_TOPIC_HIRES_DEPTH;
    topics[COLOR] = K2_TOPIC_IMAGE_COLOR;
    topics[COLOR_RECT] = K2_TOPIC_RECT_COLOR;
    topics[COLOR_LORES] = K2_TOPIC_LORES_COLOR;
    topics[MONO] = K2_TOPIC_IMAGE_MONO;
    topics[MONO_RECT] = K2_TOPIC_RECT_MONO;
    topics[MONO_LORES] = K2_TOPIC_LORES_MONO;

    color = cv::Mat::zeros(sizeColor, CV_8UC3);
    ir = cv::Mat::zeros(sizeIr, CV_32F);
    depth = cv::Mat::zeros(sizeIr, CV_32F);

    compressionParams.resize(7, 0);
    compressionParams[0] = CV_IMWRITE_JPEG_QUALITY;
    compressionParams[1] = jpegQuality;
    compressionParams[2] = CV_IMWRITE_PNG_COMPRESSION;
    compressionParams[3] = pngLevel;
    compressionParams[4] = CV_IMWRITE_PNG_STRATEGY;
    compressionParams[5] = CV_IMWRITE_PNG_STRATEGY_RLE;
    compressionParams[6] = 0;

    depthRegLowRes = DepthRegistration::New(methodReg);
    depthRegHighRes = DepthRegistration::New(methodReg);

    if(methodDepth == CPU)
    {
      packetPipeline = new libfreenect2::CpuPacketPipeline();
    }
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
    else if(methodDepth == OPENCL)
    {
      packetPipeline = new libfreenect2::OpenCLPacketPipeline(deviceIdDepth);
    }
#endif
    else if(methodDepth == OPENGL)
    {
      glfwInit();
      packetPipeline = new libfreenect2::OpenGLPacketPipeline();
    }
    else
    {
      glfwInit();
      packetPipeline = new libfreenect2::DefaultPacketPipeline();
    }

    libfreenect2::DepthPacketProcessor::Config config;
    config.EnableBilateralFilter = true;
    config.EnableEdgeAwareFilter = true;
    config.MinDepth = 0.1;
    config.MaxDepth = maxDepth;
    packetPipeline->getDepthPacketProcessor()->setConfiguration(config);

    if(useTiff)
    {
      compression16BitExt = ".tiff";
      compression16BitString = "; tiff compressed ";
    }
{'fraction_non_alphanumeric': 0.10783369803063457, 'fraction_numerical': 0.01899343544857768, 'mean_word_length': 4.514478764478764, 'pattern_counts': {'":': 0, '<': 77, '<?xml version=': 0, '>': 81, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7304212', 'n_tokens_mistral': 4179, 'n_tokens_neox': 4030, 'n_words': 836}