diff --git a/src/plugin/skeletonizer3D.cpp b/src/plugin/skeletonizer3D.cpp
index c8a90fa..3115a38 100644
--- a/src/plugin/skeletonizer3D.cpp
+++ b/src/plugin/skeletonizer3D.cpp
@@ -39,6 +39,7 @@
 
 #ifdef KINECT_AZURE
 // include Kinect libraries
+#include <k4a/k4a.h>
 #include <k4a/k4a.hpp>
 #include <k4abt.hpp>
 #endif
@@ -47,10 +48,19 @@ using namespace cv;
 using namespace std;
 using json = nlohmann::json;
 
+struct color_point_t
+{
+    int16_t xyz[3];
+    uint8_t rgb[3];
+};
+
 // Map of OpenPOSE keypoint names
 // TODO: update with Miroscic names
 map<int, string> keypoints_map = {
-    {0, "NOS_"}, {1, "NEC_"}, {2, "SHOR"}, {3, "ELBR"}, {4, "WRIR"}, {5, "SHOL"}, {6, "ELBL"}, {7, "WRIL"}, {8, "HIPR"}, {9, "KNER"}, {10, "ANKR"}, {11, "HIPL"}, {12, "KNEL"}, {13, "ANKL"}, {14, "EYEL"}, {15, "EYEL"}, {16, "EARR"}, {17, "EARL"}};
+    {0, "NOS_"}, {1, "NEC_"}, {2, "SHOR"}, {3, "ELBR"}, {4, "WRIR"}, {5, "SHOL"}, {6, "ELBL"}, {7, "WRIL"}, {8, "HIPR"}, {9, "KNER"}, {10, "ANKR"}, {11, "HIPL"}, {12, "KNEL"}, {13, "ANKL"}, {14, "EYER"}, {15, "EYEL"}, {16, "EARR"}, {17, "EARL"}};
+
+map<int, string> keypoints_map_azure = {
+    {27, "NOS_"}, {3, "NEC_"}, {12, "SHOR"}, {13, "ELBR"}, {14, "WRIR"}, {5, "SHOL"}, {6, "ELBL"}, {7, "WRIL"}, {22, "HIPR"}, {23, "KNER"}, {24, "ANKR"}, {18, "HIPL"}, {19, "KNEL"}, {20, "ANKL"}, {30, "EYER"}, {28, "EYEL"}, {31, "EARR"}, {29, "EARL"}};
 
 /**
  * @class Skeletonizer3D
@@ -302,6 +312,175 @@ class Skeletonizer3D : public Source<json>
          << endl;
   }
 
+  #ifdef KINECT_AZURE
+  return_type create_point_cloud(k4a_transformation_t transformation_handle,
+                                       const k4a_image_t depth_image,
+                                       const k4a_image_t color_image)
+{
+    int depth_image_width_pixels = k4a_image_get_width_pixels(depth_image);
+    int depth_image_height_pixels = k4a_image_get_height_pixels(depth_image);
+    k4a_image_t transformed_color_image = NULL;
+    if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_COLOR_BGRA32,
+                                                 depth_image_width_pixels,
+                                                 depth_image_height_pixels,
+                                                 depth_image_width_pixels * 4 * (int)sizeof(uint8_t),
+                                                 &transformed_color_image))
+    {
+        printf("Failed to create transformed color image\n");
+        return return_type::error;
+    }
+
+    k4a_image_t point_cloud_image = NULL;
+    if (K4A_RESULT_SUCCEEDED != k4a_image_create(K4A_IMAGE_FORMAT_CUSTOM,
+                                                 depth_image_width_pixels,
+                                                 depth_image_height_pixels,
+                                                 depth_image_width_pixels * 3 * (int)sizeof(int16_t),
+                                                 &point_cloud_image))
+    {
+        printf("Failed to create point cloud image\n");
+        return return_type::error;
+    }
+
+    if (K4A_RESULT_SUCCEEDED != k4a_transformation_color_image_to_depth_camera(transformation_handle,
+                                                                               depth_image,
+                                                                               color_image,
+                                                                               transformed_color_image))
+    {
+        printf("Failed to compute transformed color image\n");
+        return return_type::error;
+    }
+
+    if (K4A_RESULT_SUCCEEDED != k4a_transformation_depth_image_to_point_cloud(transformation_handle,
+                                                                              depth_image,
+                                                                              K4A_CALIBRATION_TYPE_DEPTH,
+                                                                              point_cloud_image))
+    {
+        printf("Failed to compute point cloud\n");
+        return return_type::error;
+    }
+
+    std::vector<color_point_t> points;
+
+    int width = k4a_image_get_width_pixels(point_cloud_image);
+    int height = k4a_image_get_height_pixels(transformed_color_image);
+
+    int16_t *point_cloud_image_data = (int16_t *)(void *)k4a_image_get_buffer(point_cloud_image);
+    uint8_t *color_image_data = k4a_image_get_buffer(transformed_color_image);
+
+    for (int i = 0; i < width * height; i++)
+    {
+        color_point_t point;
+        point.xyz[0] = point_cloud_image_data[3 * i + 0];
+        point.xyz[1] = point_cloud_image_data[3 * i + 1];
+        point.xyz[2] = point_cloud_image_data[3 * i + 2];
+        if (point.xyz[2] == 0)
+        {
+            continue;
+        }
+
+        point.rgb[0] = color_image_data[4 * i + 0];
+        point.rgb[1] = color_image_data[4 * i + 1];
+        point.rgb[2] = color_image_data[4 * i + 2];
+        uint8_t alpha = color_image_data[4 * i + 3];
+
+        if (point.rgb[0] == 0 && point.rgb[1] == 0 && point.rgb[2] == 0 && alpha == 0)
+        {
+            continue;
+        }
+
+        points.push_back(point);
+    }
+
+    // convert the points to a point cloud of Mat type
+    _point_cloud = cv::Mat(points.size(), 6, CV_32F);
+    for (size_t i = 0; i < points.size(); i++)
+    {
+        _point_cloud.at<float>(i, 0) = points[i].xyz[0];
+        _point_cloud.at<float>(i, 1) = points[i].xyz[1];
+        _point_cloud.at<float>(i, 2) = points[i].xyz[2];
+        _point_cloud.at<float>(i, 3) = points[i].rgb[2];
+        _point_cloud.at<float>(i, 4) = points[i].rgb[1];
+        _point_cloud.at<float>(i, 5) = points[i].rgb[0];
+    }
+    
+
+    // Save the point cloud to a ply file
+    //write_ply_from_points_vector(points, "../plugin_skeletonizer_3D/test_points.ply");
+    //write_ply_from_cv_mat(_point_cloud, "../plugin_skeletonizer_3D/test_cv_mat.ply");
+
+    return return_type::success;
+}
+
+// Write the point cloud to a ply file
+void write_ply_from_cv_mat(cv::Mat point_cloud, const char *file_name){
+
+#define PLY_START_HEADER "ply"
+#define PLY_END_HEADER "end_header"
+#define PLY_ASCII "format ascii 1.0"
+#define PLY_ELEMENT_VERTEX "element vertex"
+
+    // save to the ply file
+    std::ofstream ofs(file_name); // text mode first
+    ofs << PLY_START_HEADER << std::endl;
+    ofs << PLY_ASCII << std::endl;
+    ofs << PLY_ELEMENT_VERTEX << " " << point_cloud.rows << std::endl;
+    ofs << "property float x" << std::endl;
+    ofs << "property float y" << std::endl;
+    ofs << "property float z" << std::endl;
+    ofs << "property uchar red" << std::endl;
+    ofs << "property uchar green" << std::endl;
+    ofs << "property uchar blue" << std::endl;
+    ofs << PLY_END_HEADER << std::endl;
+    ofs.close();
+
+    std::stringstream ss;
+    for (int i = 0; i < point_cloud.rows; ++i)
+    {
+      ss << point_cloud.at<float>(i, 0) << " " << point_cloud.at<float>(i, 1) << " " << point_cloud.at<float>(i, 2);
+      ss << " " << point_cloud.at<float>(i, 3) << " " << point_cloud.at<float>(i, 4) << " " << point_cloud.at<float>(i, 5);
+      ss << std::endl;
+    }
+    std::ofstream ofs_text(file_name, std::ios::out | std::ios::app);
+    ofs_text.write(ss.str().c_str(), (std::streamsize)ss.str().length());
+}
+
+// Write the point cloud to a ply file
+static void write_ply_from_points_vector(std::vector<color_point_t> points,
+                                             const char *file_name){
+
+#define PLY_START_HEADER "ply"
+#define PLY_END_HEADER "end_header"
+#define PLY_ASCII "format ascii 1.0"
+#define PLY_ELEMENT_VERTEX "element vertex"
+
+    // save to the ply file
+    std::ofstream ofs(file_name); // text mode first
+    ofs << PLY_START_HEADER << std::endl;
+    ofs << PLY_ASCII << std::endl;
+    ofs << PLY_ELEMENT_VERTEX << " " << points.size() << std::endl;
+    ofs << "property float x" << std::endl;
+    ofs << "property float y" << std::endl;
+    ofs << "property float z" << std::endl;
+    ofs << "property uchar red" << std::endl;
+    ofs << "property uchar green" << std::endl;
+    ofs << "property uchar blue" << std::endl;
+    ofs << PLY_END_HEADER << std::endl;
+    ofs.close();
+
+    std::stringstream ss;
+    for (size_t i = 0; i < points.size(); ++i)
+    {
+        // image data is BGR
+        ss << (float)points[i].xyz[0] << " " << (float)points[i].xyz[1] << " " << (float)points[i].xyz[2];
+        ss << " " << (float)points[i].rgb[2] << " " << (float)points[i].rgb[1] << " " << (float)points[i].rgb[0];
+        ss << std::endl;
+    }
+    std::ofstream ofs_text(file_name, std::ios::out | std::ios::app);
+    ofs_text.write(ss.str().c_str(), (std::streamsize)ss.str().length());
+}
+
+  #endif
+
   /**
    * @brief Acquire a frame from a camera device. Camera ID is defined in the
    * parameters list.
@@ -337,6 +516,8 @@ class Skeletonizer3D : public Source<json>
 
       // acquire and store into _rgb (RGB) and _rgbd (RGBD), if available
       k4a::image colorImage = _k4a_rgbd.get_color_image();
+      k4a_image_t color_handle = colorImage.handle();
+      k4a_image_reference(color_handle);	
 
       // from k4a::image to cv::Mat --> color image
       if (colorImage != NULL)
@@ -352,20 +533,26 @@ class Skeletonizer3D : public Source<json>
         //_rgb.convertTo(_rgb, CV_8UC3);
       }
 
-      k4a::image depthImage = _k4a_rgbd.get_depth_image();
+      _depth_image = _k4a_rgbd.get_depth_image();
+      k4a_image_t depth_handle = _depth_image.handle();
+      k4a_image_reference(depth_handle);	
 
       // from k4a::image to cv::Mat --> depth image
-      if (depthImage != NULL)
+      if (_depth_image != NULL)
       {
         // get raw buffer
-        uint8_t *buffer = depthImage.get_buffer();
+        uint8_t *buffer = _depth_image.get_buffer();
 
         // convert the raw buffer to cv::Mat
-        int rows = depthImage.get_height_pixels();
-        int cols = depthImage.get_width_pixels();
+        int rows = _depth_image.get_height_pixels();
+        int cols = _depth_image.get_width_pixels();
         _rgbd = cv::Mat(rows, cols, CV_16U, (void *)buffer, cv::Mat::AUTO_STEP);
       }
 
+      // Debug function to save the point cloud in a .ply file
+      //point_cloud_color_to_depth(_pc_transformation, depth_handle, color_handle, "../plugin_skeletonizer_3D/test.ply");
+
+
 #else
       _cap >> _rgb;
 #endif
@@ -383,7 +570,7 @@ class Skeletonizer3D : public Source<json>
   }
 
   /* LEFT BRANCH =============================================================*/
-
+  
   /**
    * @brief Compute the skeleton from the depth map.
    *
@@ -406,20 +593,43 @@ class Skeletonizer3D : public Source<json>
 
     _body_frame = _tracker.pop_result();
     if (_body_frame != nullptr)
-    {
-      uint32_t num_bodies = _body_frame.get_num_bodies();
-      std::cout << num_bodies << " bodies are detected!" << endl;
+    {      
+      // Only take one body (always the first one)
+      k4abt_body_t body = _body_frame.get_body(0);
 
-      if (debug)
+      _skeleton3D.clear();
+      for (const auto& [index, keypoint_name] : keypoints_map_azure)
       {
+        k4a_float3_t position = body.skeleton.joints[index].position;
+
+        vector<float> keypoint_data;
+        keypoint_data.push_back(static_cast<float>(position.v[0]));
+        keypoint_data.push_back(static_cast<float>(position.v[1]));
+        keypoint_data.push_back(static_cast<float>(position.v[2]));
 
-        // Print the body information
-        for (uint32_t i = 0; i < num_bodies; i++)
+        _skeleton3D[keypoint_name] = keypoint_data;
+      }
+      
+      /*
+      if (debug)
+      {
+        cout << "\nSkeleton 3D:" << endl;
+        for (const auto& [keypoint_name, keypoint_data] : _skeleton3D)
         {
-          k4abt_body_t body = _body_frame.get_body(i);
-          // print_body_information(body);
+          cout << keypoint_name << ": (";
+          for (size_t i = 0; i < keypoint_data.size(); ++i)
+          {
+            cout << static_cast<int>(keypoint_data[i]);
+            if (i < keypoint_data.size() - 1)
+            {
+              cout << ", ";
+            }
+          }
+          cout << ")" << endl;
         }
       }
+      */
+        
     }
     else
     {
@@ -446,61 +656,88 @@ class Skeletonizer3D : public Source<json>
    */
   return_type point_cloud_filter(bool debug = false)
   {
+
 #ifdef KINECT_AZURE
+    // get the body index map from _k4a_rgbd
 
-    k4a::image pc = _pc_transformation.depth_image_to_point_cloud(_k4a_rgbd.get_depth_image(), K4A_CALIBRATION_TYPE_DEPTH);
+    // TODO: _body_frame is obtained in the skeleton_from_depth_compute method! We don't like this dependency
+    // Retrieve the body tracking result
 
-    // get raw buffer
-    uint8_t *buffer = pc.get_buffer();
+    if (_body_frame != nullptr)
+    {
+      k4a::image body_index_map = _body_frame.get_body_index_map();
 
-    // convert the raw buffer to cv::Mat
-    int rows = pc.get_height_pixels();
-    int cols = pc.get_width_pixels();
-    _point_cloud = cv::Mat(rows, cols, CV_16U, (void *)buffer, cv::Mat::AUTO_STEP);
 
-#endif
+      // mask the depth image with the body index map
+      k4a::image masked_depth_image = k4a::image::create(K4A_IMAGE_FORMAT_DEPTH16,
+                                                          _depth_image.get_width_pixels(), _depth_image.get_height_pixels(),
+                                                          _depth_image.get_stride_bytes());
+      
+      mask_depth_with_body_index(_depth_image, body_index_map, masked_depth_image);
 
-    // Converte la variabile cv::Mat in un point cloud di tipo pcl::PointCloud<pcl::PointXYZ>
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-    cloud->width = _point_cloud.cols;
-    cloud->height = _point_cloud.rows;
-    cloud->points.resize(cloud->width * cloud->height);
+      k4a_image_t masked_depth_handle = masked_depth_image.handle();
+      k4a_image_reference(masked_depth_handle);	
 
-    for (int i = 0; i < _point_cloud.rows; ++i)
-    {
-      for (int j = 0; j < _point_cloud.cols; ++j)
+      // from k4a::image to cv::Mat --> depth image
+      if (masked_depth_image != NULL)
       {
-        const cv::Vec3f &point = _point_cloud.at<cv::Vec3f>(i, j);
-        cloud->points[i * cloud->width + j].x = point[0];
-        cloud->points[i * cloud->width + j].y = point[1];
-        cloud->points[i * cloud->width + j].z = point[2];
+        // get raw buffer
+        uint8_t *buffer = masked_depth_image.get_buffer();
+
+        // convert the raw buffer to cv::Mat
+        int rows = masked_depth_image.get_height_pixels();
+        int cols = masked_depth_image.get_width_pixels();
+        _rgbd_filtered = cv::Mat(rows, cols, CV_16U, (void *)buffer, cv::Mat::AUTO_STEP);
       }
-    }
 
-    cout << "Before viewer - Filtering point cloud..." << endl;
+      // convert the depth image to a point cloud
+      k4a::image colorImage = _k4a_rgbd.get_color_image();
+      k4a_image_t color_handle = colorImage.handle();
+      k4a_image_reference(color_handle);
 
-    pcl::visualization::PCLVisualizer::Ptr viewer;
-    viewer = simpleVis(cloud);
+      // create the point cloud and save it in _point_cloud variable
+      create_point_cloud(_pc_transformation, masked_depth_handle, color_handle);
 
-    cout << "After viewer - Filtering point cloud..." << endl;
+    }
+    else
+    {
+      cout << "No body frame detected!" << endl;
+    }
+    
+#endif
 
-    // NOOP
     return return_type::success;
   }
 
-  pcl::visualization::PCLVisualizer::Ptr simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
-  {
-    // --------------------------------------------
-    // -----Open 3D viewer and add point cloud-----
-    // --------------------------------------------
-    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
-    viewer->setBackgroundColor(0, 0, 0);
-    viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
-    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
-    viewer->addCoordinateSystem(1.0);
-    viewer->initCameraParameters();
-    return (viewer);
+#ifdef KINECT_AZURE
+  void mask_depth_with_body_index(const k4a::image &depth_image, const k4a::image &body_index_map, k4a::image &masked_depth_image) {
+      // Get image dimensions
+      int width = depth_image.get_width_pixels();
+      int height = depth_image.get_height_pixels();
+
+      // Get pointers to the image data
+      const uint16_t* depth_data = reinterpret_cast<const uint16_t*>(depth_image.get_buffer());
+      const uint8_t* body_index_data = reinterpret_cast<const uint8_t*>(body_index_map.get_buffer());
+      uint16_t* masked_depth_data = reinterpret_cast<uint16_t*>(masked_depth_image.get_buffer());
+
+      // Iterate over each pixel
+      for (int y = 0; y < height; y++) {
+          for (int x = 0; x < width; x++) {
+              int idx = y * width + x;
+
+              // Check if the pixel belongs to a body
+              if (body_index_data[idx] != K4ABT_BODY_INDEX_MAP_BACKGROUND) {
+                  // Copy the depth value
+                  masked_depth_data[idx] = depth_data[idx];
+              } else {
+                  // Set to zero or any background value (e.g., 0 for no depth)
+                  masked_depth_data[idx] = 0;
+              }
+          }
+      }
+      
   }
+#endif
 
   /**
    * @brief Transform the 3D skeleton coordinates in the global reference frame
@@ -852,7 +1089,7 @@ class Skeletonizer3D : public Source<json>
 
     acquire_frame(_dummy);
     skeleton_from_depth_compute(_params["debug"]["skeleton_from_depth_compute"]);
-    // point_cloud_filter(_params["debug"]["point_cloud_filter"]);
+    point_cloud_filter(_params["debug"]["point_cloud_filter"]);
     skeleton_from_rgb_compute(_params["debug"]["skeleton_from_rgb_compute"]);
 
     if (!(_result))
@@ -896,16 +1133,16 @@ class Skeletonizer3D : public Source<json>
 #else
       max_depth = 2000;
 #endif
-
+      
       Mat rgbd_flipped;
-      flip(_rgbd, rgbd_flipped, 1);
+      flip(_rgbd_filtered, rgbd_flipped, 1);
       rgbd_flipped.convertTo(rgbd_flipped, CV_8U, 255.0 / max_depth);
       Mat rgbd_flipped_color;
       applyColorMap(rgbd_flipped, rgbd_flipped_color, COLORMAP_HSV); // Apply the colormap:
       imshow("rgbd", rgbd_flipped_color);
-
+      
       int key = cv::waitKey(1000.0 / _fps);
-
+      //system("pause");
       if (27 == key || 'q' == key || 'Q' == key)
       { // Esc
 #ifdef KINECT_AZURE
@@ -919,20 +1156,41 @@ class Skeletonizer3D : public Source<json>
       }
     }
 
-    // Prepare output
-
+    /*
+    // Prepare output of the keypoints in the 2D space from _keypoints_list
     if (_poses.size() > 0)
     {
       for (int kp = 0; kp < HPEOpenPose::keypointsNumber; kp++)
       {
         if (_keypoints_list[kp].x < 0 || _keypoints_list[kp].y < 0)
           continue;
-        (*out)["poses"][keypoints_map[kp]] = {_keypoints_list[kp].x,
-                                              _keypoints_list[kp].y};
+        (*out)["poses"][keypoints_map[kp]] = {_keypoints_list[kp].x,_keypoints_list[kp].y};
         (*out)["cov"][keypoints_map[kp]] = {
             _keypoints_cov[kp].x, _keypoints_cov[kp].y, _keypoints_cov[kp].z};
       }
     }
+    */
+
+    // Prepare the output of the poses in the 3D space from _skeleton3D
+    if (_skeleton3D.size() > 0)
+    {
+      for (const auto &[keypoint_name, keypoint_data] : _skeleton3D)
+      {
+        (*out)["poses"][keypoint_name] = keypoint_data;
+      }
+    }
+
+    // print output
+    cout << "Output: " << out->dump(4) << endl;
+
+    // Prepare the output of the point cloud as blob to add to out
+    if (!_point_cloud.empty()) {
+      vector<unsigned char> point_cloud_blob;
+      point_cloud_blob.assign((unsigned char*)_point_cloud.datastart, (unsigned char*)_point_cloud.dataend);
+      if (blob) {
+      *blob = point_cloud_blob;
+      }
+    }
 
     // store the output in the out parameter json and the point cloud in the
     // blob parameter
@@ -966,9 +1224,10 @@ class Skeletonizer3D : public Source<json>
 protected:
   Mat _rgbd; /**< the last RGBD frame */
   Mat _rgb;  /**< the last RGB frame */
-  map<string, vector<unsigned char>>
+  Mat _rgbd_filtered; /**< the last RGBD frame filtered with the body index mask*/
+  map<string, vector<float>>
       _skeleton2D; /**< the skeleton from 2D cameras only*/
-  map<string, vector<unsigned char>>
+  map<string, vector<float>>
       _skeleton3D;       /**< the skeleton from 3D cameras only*/
   vector<Mat> _heatmaps; /**< the joints heatmaps */
   Mat _point_cloud;      /**< the filtered body point cloud */
@@ -1008,7 +1267,7 @@ class Skeletonizer3D : public Source<json>
   k4a::capture _k4a_rgbd; /**< the last capture */
   k4a::image _depth_image;
   k4abt::frame _body_frame;
-  k4a::transformation _pc_transformation; /**< the transformation */
+  k4a_transformation_t _pc_transformation; /**< the transformation */
 
 #endif
   ov::Core _core;