From 8d0c7487c4fd31252bd81aeb2b5b12a04bc0734e Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Thu, 6 Mar 2025 18:53:54 +0100
Subject: [PATCH 1/9] threaded bottleneck 130ms -> 50ms
---
dep.mk | 4 +-
inc/projection.h | 2 +
src/common.mk | 2 +-
src/components/projection.cpp | 125 +++++++++++++++++++++++++++-------
4 files changed, 104 insertions(+), 29 deletions(-)
diff --git a/dep.mk b/dep.mk
index 6988cb5..3a9af19 100644
--- a/dep.mk
+++ b/dep.mk
@@ -1,5 +1,5 @@
-OPENCVFLAG=`pkg-config --libs --cflags opencv`
+OPENCVFLAG=`pkg-config --libs --cflags opencv3`
CAMERAFLAG=-lrealsense2
-YAMLFLAG=-lyaml-cpp
+YAMLFLAG=-I/usr/local/include -L/usr/local/lib -lyaml-cpp
DEP_SANDBOX=$(OPENCVFLAG) $(CAMERAFLAG) $(YAMLFLAG)
diff --git a/inc/projection.h b/inc/projection.h
index 1af6361..e877db1 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -25,6 +25,8 @@ class Projection{
cv::Point2f fxy;
cv::Point2f ppxy;
+ void deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
+
void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
diff --git a/src/common.mk b/src/common.mk
index 6f95981..ac9de75 100644
--- a/src/common.mk
+++ b/src/common.mk
@@ -1,2 +1,2 @@
CCP=g++
-CFLAGS=-std=c++11 -Wall -Wextra -g -Ilib -fPIC
+CFLAGS=-std=c++11 -Wall -Wextra -g -Ilib -fPIC -I/opt/opencv3/include -I/usr/local/include -L/usr/local/lib -lyaml-cpp
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index d0c30c0..82a2a89 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -5,7 +5,7 @@
* Contains the matrix to compensate for the rotation of the beamer
* and the distance to the plane at the top of the sandbox.
*/
-
+#include <thread>
#include "../../inc/projection.h"
/*
@@ -36,6 +36,7 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
fxy = profil.at(0);
ppxy = profil.at(1);
}
+
// do nothing if contains matrix with same size
// otherwise => release => allocate
deprojectMap.create(dst.rows, dst.cols);
@@ -51,20 +52,30 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
cv::resize(src, resized_src, dst.size());
cv::resize(depth, resized_depth, dst.size());
+ std::chrono::milliseconds start_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
+ // 130ms EXEC
deprojectPixelsFromDepth(resized_depth, camera, camera->getCroppingMask() , beamer_pos, deprojectMap, fxy, ppxy);
+
+ std::chrono::milliseconds now_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
+ int time_adj = (now_adj_ms - start_adj_ms).count();
+ std::cout << "Time for execution" << time_adj << std::endl;
+
+ // 40ms EXEC
filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
+
+ // 30ms EXEC
buildFrame(resized_depth, frameMap, resized_src, dst);
+
+ // 46ms
holeFilling(dst, frameMap);
+
cv::warpAffine(dst, dst, adjustingMatrix, dst.size()); // apply the rotation on the image
}
-
-
/*
* Private
*/
-
/*
* Deproject pixels in 3D, then adapt to Beamer's POV, and go back to 2D
* This gives us the location of pixels adapted to the Beamer projection
@@ -77,28 +88,64 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
* fxy : function x and y adapted to the projection matching the original depth matrix(without ROI) of the camera
* ppxy : coordinates x and y of the central pixel adapted to the projection matching the original depth matrix(without ROI) of the camera
*/
-void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
-
+void Projection::deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
// scale coord of the mask with the new resolution of the depth frame
- float mask_scale_x = mask.x * depth.cols / mask.width;
- float mask_scale_y = mask.y * depth.rows / mask.height;
-
- for (int i = 0; i < depth.rows; i++){
- for (int j = 0; j < depth.cols; j++){
- // coordinates of the pixel relative to the orginial image taken from the camera
- int base_x = mask_scale_x+j;
- int base_y = mask_scale_y+i;
- float z = depth.at<float>(i,j);
-
+ float mask_scale_x = mask.x * cols / mask.width;
+ float mask_scale_y = mask.y * rows / mask.height;
+
+ // coordinates of the pixel relative to the orginial image taken from the camera
+ for(int y = start_y; y < start_y + partition_len; y++){
+ for(int x = start_x; x < start_x + partition_len; x++){
+ if( 0 <= x && x < depth.cols && 0 <= y && y < depth.rows ){
+ // Check boundaries
+ int base_x = mask_scale_x+x;
+ int base_y = mask_scale_y+y;
+ float z = depth.at<float>(y,x);
+
// pixels based on the original depth frame taken from the camera
+
cv::Point2i pixel = findMatchingPixel( base_y, base_x, z, camera, beamer_pos, fxy, ppxy );
-
+
// pixel relative to the cropping mask (the area where the frame is projected)
pixel.x -= mask_scale_x;
pixel.y -= mask_scale_y;
- deprojectMap.at<cv::Point2i>(i,j) = pixel;
+ // No need to lock ressource, we are sure to write at a different point each time
+ deprojectMap.at<cv::Point2i>(x,y) = pixel;
}
+ }
+ }
+// int g = x+y;
+}
+
+
+/*
+* Deproject pixels in 3D, then adapt to Beamer's POV, and go back to 2D
+* This gives us the location of pixels adapted to the Beamer projection
+*
+* depth : Topology of the sandbox under the projection
+* camera : Active camera
+* mask : ROI (Range Of Interest) delimiting the zone of the projection from the camera POV
+* beamer_pos : 3D position of the beamer relative to the camera
+* deprojectMap : Indicates for each pixel of src, where it'll be displayed
+* fxy : function x and y adapted to the projection matching the original depth matrix(without ROI) of the camera
+* ppxy : coordinates x and y of the central pixel adapted to the projection matching the original depth matrix(without ROI) of the camera
+*/
+void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
+ std::vector<std::thread> threads;
+ // Creation of n submatrix of size 100x100
+ // Each matrix will be managed by a different thread
+ int partition_len = 100;
+ // TODO THREAD IT
+ // Create 50 threads to compute only acertain part of the matrix
+ for (int start_y = 0; start_y < depth.rows; start_y += partition_len){
+ for(int start_x = 0; start_x < depth.cols; start_x += partition_len){
+ threads.emplace_back(std::thread(&Projection::deprojectPixelsFromDepthThread, this, start_x, start_y, partition_len, std::ref(depth), depth.cols, depth.rows, camera, mask, beamer_pos, std::ref(deprojectMap), fxy, ppxy));
+ }
+ }
+
+ for(auto& th: threads){
+ th.join();
}
}
@@ -108,9 +155,9 @@ void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera
*
* depth : Topology of the sandbox matching the projection
* deprojectMap : Indicates for each pixel of src, where it'll be displayed
-* frameMap : Indicates for each pixel of dst, where it should get the value from in src
+* frameMap : Indicates for each pixel of dst, where it should get the value from in src
*/
-void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap){
+void Projection::filterLowestDeprojectedPointsThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap){
for (int i = 0; i < deprojectMap.rows; i++){
for (int j = 0; j < deprojectMap.cols; j++){
@@ -121,21 +168,47 @@ void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<
if( (0 <= deprojectedPixel.x && deprojectedPixel.x < depth.cols) &&
(0 <= deprojectedPixel.y && deprojectedPixel.y < depth.rows) ){
-
+
// check and keep the highest point at the location pointed by pixel
cv::Point2i currentDepthPixel = frameMap.at<cv::Point2i>(deprojectedPixel);
if( (0 <= currentDepthPixel.x && currentDepthPixel.x < depth.cols) &&
(0 <= currentDepthPixel.y && currentDepthPixel.y < depth.rows) ){
- if(depth.at<float>(currentDepthPixel) <= depth.at<float>(i,j)){
- highestDepthPixel = currentDepthPixel;
- }
- }
+ if(depth.at<float>(currentDepthPixel) <= depth.at<float>(i,j)){
+ highestDepthPixel = currentDepthPixel;
+ }
+ }
frameMap.at<cv::Point2i>(deprojectedPixel) = highestDepthPixel;
- }
+ }
}
}
}
+/*
+* Save the highest points in deprojectMap into frameMap,
+* because some points can be deprojected at the same location
+*
+* depth : Topology of the sandbox matching the projection
+* deprojectMap : Indicates for each pixel of src, where it'll be displayed
+* frameMap : Indicates for each pixel of dst, where it should get the value from in src
+*/
+void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap){
+ std::vector<std::thread> threads;
+ // Creation of n submatrix of size 100x100
+ // Each matrix will be managed by a different thread
+ int partition_len = 100;
+ // TODO THREAD IT
+ // Create 50 threads to compute only acertain part of the matrix
+ for (int start_y = 0; start_y < depth.rows; start_y += partition_len){
+ for(int start_x = 0; start_x < depth.cols; start_x += partition_len){
+ threads.emplace_back(std::thread(&Projection::filterLowestDeprojectedPointsThread, this, start_x, start_y, partition_len, std::ref(depth), std::ref(deprojectMap), std::ref(frameMap));
+ }
+ }
+
+ for(auto& th: threads){
+ th.join();
+ }
+}
+
/*
* Build the frame using frameMap, we assume that all the buffers have the same size
--
GitLab
From 042597c47bfe66bc85c4f9349d913588e2f2d391 Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Thu, 6 Mar 2025 19:01:06 +0100
Subject: [PATCH 2/9] threaded 2nd function
---
inc/projection.h | 2 +-
src/components/projection.cpp | 28 ++++++++++++----------------
2 files changed, 13 insertions(+), 17 deletions(-)
diff --git a/inc/projection.h b/inc/projection.h
index e877db1..f1f73b7 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -26,7 +26,7 @@ class Projection{
cv::Point2f ppxy;
void deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
-
+ void filterLowestDeprojectedPointsThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index 82a2a89..adc46d5 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -52,17 +52,16 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
cv::resize(src, resized_src, dst.size());
cv::resize(depth, resized_depth, dst.size());
- std::chrono::milliseconds start_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
- // 130ms EXEC
+ // 130ms EXEC -> ~50ms
deprojectPixelsFromDepth(resized_depth, camera, camera->getCroppingMask() , beamer_pos, deprojectMap, fxy, ppxy);
+ std::chrono::milliseconds start_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
+ // 40ms EXEC -> ~20ms
+ filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
std::chrono::milliseconds now_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
int time_adj = (now_adj_ms - start_adj_ms).count();
std::cout << "Time for execution" << time_adj << std::endl;
- // 40ms EXEC
- filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
-
// 30ms EXEC
buildFrame(resized_depth, frameMap, resized_src, dst);
@@ -136,8 +135,6 @@ void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera
// Creation of n submatrix of size 100x100
// Each matrix will be managed by a different thread
int partition_len = 100;
- // TODO THREAD IT
- // Create 50 threads to compute only acertain part of the matrix
for (int start_y = 0; start_y < depth.rows; start_y += partition_len){
for(int start_x = 0; start_x < depth.cols; start_x += partition_len){
threads.emplace_back(std::thread(&Projection::deprojectPixelsFromDepthThread, this, start_x, start_y, partition_len, std::ref(depth), depth.cols, depth.rows, camera, mask, beamer_pos, std::ref(deprojectMap), fxy, ppxy));
@@ -158,13 +155,13 @@ void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera
* frameMap : Indicates for each pixel of dst, where it should get the value from in src
*/
void Projection::filterLowestDeprojectedPointsThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap){
-
- for (int i = 0; i < deprojectMap.rows; i++){
- for (int j = 0; j < deprojectMap.cols; j++){
+ for(int y = start_y; y < start_y + partition_len; y++){
+ for(int x = start_x; x < start_x + partition_len; x++){
+ if( 0 <= x && x < depth.cols && 0 <= y && y < depth.rows ){
// coords of the new pixel
- cv::Point2i deprojectedPixel = deprojectMap.at<cv::Point2i>(i,j);
- cv::Point2i highestDepthPixel = cv::Point2i(j,i);
+ cv::Point2i deprojectedPixel = deprojectMap.at<cv::Point2i>(y,x);
+ cv::Point2i highestDepthPixel = cv::Point2i(x,y);
if( (0 <= deprojectedPixel.x && deprojectedPixel.x < depth.cols) &&
(0 <= deprojectedPixel.y && deprojectedPixel.y < depth.rows) ){
@@ -173,12 +170,13 @@ void Projection::filterLowestDeprojectedPointsThread(int start_x, int start_y, i
cv::Point2i currentDepthPixel = frameMap.at<cv::Point2i>(deprojectedPixel);
if( (0 <= currentDepthPixel.x && currentDepthPixel.x < depth.cols) &&
(0 <= currentDepthPixel.y && currentDepthPixel.y < depth.rows) ){
- if(depth.at<float>(currentDepthPixel) <= depth.at<float>(i,j)){
+ if(depth.at<float>(currentDepthPixel) <= depth.at<float>(y,x)){
highestDepthPixel = currentDepthPixel;
}
}
frameMap.at<cv::Point2i>(deprojectedPixel) = highestDepthPixel;
}
+ }
}
}
}
@@ -196,11 +194,9 @@ void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<
// Creation of n submatrix of size 100x100
// Each matrix will be managed by a different thread
int partition_len = 100;
- // TODO THREAD IT
- // Create 50 threads to compute only acertain part of the matrix
for (int start_y = 0; start_y < depth.rows; start_y += partition_len){
for(int start_x = 0; start_x < depth.cols; start_x += partition_len){
- threads.emplace_back(std::thread(&Projection::filterLowestDeprojectedPointsThread, this, start_x, start_y, partition_len, std::ref(depth), std::ref(deprojectMap), std::ref(frameMap));
+ threads.emplace_back(std::thread(&Projection::filterLowestDeprojectedPointsThread, this, start_x, start_y, partition_len, std::ref(depth), std::ref(deprojectMap), std::ref(frameMap)));
}
}
--
GitLab
From af6511153d8e71ab08932dd79efe96c1c16d5cd2 Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Thu, 6 Mar 2025 19:27:54 +0100
Subject: [PATCH 3/9] threaded buildFrame
---
inc/projection.h | 2 ++
src/components/projection.cpp | 57 ++++++++++++++++++++++++++---------
2 files changed, 44 insertions(+), 15 deletions(-)
diff --git a/inc/projection.h b/inc/projection.h
index f1f73b7..bc60262 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -27,6 +27,8 @@ class Projection{
void deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
void filterLowestDeprojectedPointsThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
+ void buildFrameThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
+
void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index adc46d5..618ae46 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -55,16 +55,16 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
// 130ms EXEC -> ~50ms
deprojectPixelsFromDepth(resized_depth, camera, camera->getCroppingMask() , beamer_pos, deprojectMap, fxy, ppxy);
- std::chrono::milliseconds start_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
// 40ms EXEC -> ~20ms
filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
+
+ std::chrono::milliseconds start_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
+ // 30ms EXEC -> ~10ms
+ buildFrame(resized_depth, frameMap, resized_src, dst);
std::chrono::milliseconds now_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
int time_adj = (now_adj_ms - start_adj_ms).count();
std::cout << "Time for execution" << time_adj << std::endl;
- // 30ms EXEC
- buildFrame(resized_depth, frameMap, resized_src, dst);
-
// 46ms
holeFilling(dst, frameMap);
@@ -216,20 +216,48 @@ void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<
* dst : Output image adapted from src to project
*/
// TODO : enlever depth en paramètre et vérifier que le pixel soit dans la range d'un des autres buffer
-void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst){
-
- for (int i = 0; i < frameMap.rows; i++){
- for (int j = 0; j < frameMap.cols; j++){
+void Projection::buildFrameThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst){
+ for(int y = start_y; y < start_y + partition_len; y++){
+ for(int x = start_x; x < start_x + partition_len; x++){
+ if( 0 <= x && x < depth.cols && 0 <= y && y < depth.rows ){
- cv::Point2i pixel_src = frameMap.at<cv::Point2i>(i,j);
- cv::Point2i pixel_dst = cv::Point2i(j,i);
+ cv::Point2i pixel_src = frameMap.at<cv::Point2i>(y,x);
+ cv::Point2i pixel_dst = cv::Point2i(x,y);
if( (0<=pixel_src.x && pixel_src.x<depth.cols) && (0<=pixel_src.y && pixel_src.y<depth.rows) ){
// src and dst must be of same size
dst.at<cv::Vec3b>(pixel_dst) = src.at<cv::Vec3b>(pixel_src);
- }
+ }
+ }
+ }
+ }
+}
+
+/*
+* Build the frame using frameMap, we assume that all the buffers have the same size
+* where each pixel describes in which pixel of the source it should take the value from
+* dst[i] = src[frameMap[i]]
+*
+* frameMap : The map describing where are the source pixels for each pixel in our output image
+* src : Image source to adapt
+* dst : Output image adapted from src to project
+*/
+// TODO : enlever depth en paramètre et vérifier que le pixel soit dans la range d'un des autres buffer
+void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst){
+ std::vector<std::thread> threads;
+ // Creation of n submatrix of size 100x100
+ // Each matrix will be managed by a different thread
+ int partition_len = 100;
+ for (int start_y = 0; start_y < depth.rows; start_y += partition_len){
+ for(int start_x = 0; start_x < depth.cols; start_x += partition_len){
+ threads.emplace_back(std::thread(&Projection::buildFrameThread, this, start_x, start_y, partition_len, std::ref(depth), std::ref(frameMap), std::ref(src), std::ref(dst)));
}
}
+
+
+ for(auto& th: threads){
+ th.join();
+ }
}
@@ -242,10 +270,9 @@ void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frame
* frameMap : The map describing where are the source pixels for each pixel in our output image
*/
void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap){
-
for (int i = 0; i < dst.rows; i++){
for (int j = 0; j < dst.cols; j++){
-
+
if(frameMap.at<cv::Point2i>(i,j) == cv::Point2i(-1,-1)){
cv::Vec3b color;
@@ -253,9 +280,9 @@ void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &fr
for(int d_y = -1; d_y <= 1; d_y++){
for(int d_x = -1; d_x <= 1; d_x++){
-
+
if(!(d_x == 0 && d_y == 0)){
-
+
int y = i+d_y;
int x = j+d_x;
if( ((0 <= x && x < dst.cols) && (0 <= y && y < dst.rows)) && (frameMap.at<cv::Point2i>(y,x) != cv::Point2i(-1,-1)) ){
--
GitLab
From 9ec11c0e8998231ace2649ed7cffd9d95db84b88 Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Thu, 6 Mar 2025 19:39:07 +0100
Subject: [PATCH 4/9] threaded hole filling
---
inc/projection.h | 1 +
src/components/projection.cpp | 51 +++++++++++++++++++++++++----------
2 files changed, 38 insertions(+), 14 deletions(-)
diff --git a/inc/projection.h b/inc/projection.h
index bc60262..6f521f7 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -28,6 +28,7 @@ class Projection{
void deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
void filterLowestDeprojectedPointsThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void buildFrameThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
+ void holeFillingThread(int startx, int start_y, int partition_len, cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap);
void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index 618ae46..bb61b49 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -58,14 +58,10 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
// 40ms EXEC -> ~20ms
filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
- std::chrono::milliseconds start_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
// 30ms EXEC -> ~10ms
buildFrame(resized_depth, frameMap, resized_src, dst);
- std::chrono::milliseconds now_adj_ms = std::chrono::duration_cast< std::chrono::milliseconds >( std::chrono::system_clock::now().time_since_epoch() );
- int time_adj = (now_adj_ms - start_adj_ms).count();
- std::cout << "Time for execution" << time_adj << std::endl;
- // 46ms
+ // 46ms EXEC -> 20ms
holeFilling(dst, frameMap);
cv::warpAffine(dst, dst, adjustingMatrix, dst.size()); // apply the rotation on the image
@@ -269,11 +265,12 @@ void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frame
* dst : The output image to project
* frameMap : The map describing where are the source pixels for each pixel in our output image
*/
-void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap){
- for (int i = 0; i < dst.rows; i++){
- for (int j = 0; j < dst.cols; j++){
+void Projection::holeFillingThread(int start_x, int start_y, int partition_len, cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap){
+ for(int y = start_y; y < start_y + partition_len; y++){
+ for(int x = start_x; x < start_x + partition_len; x++){
+ if( 0 <= x && x < frameMap.cols && 0 <= y && y < frameMap.rows ){
- if(frameMap.at<cv::Point2i>(i,j) == cv::Point2i(-1,-1)){
+ if(frameMap.at<cv::Point2i>(y,x) == cv::Point2i(-1,-1)){
cv::Vec3b color;
bool found = false;
@@ -283,10 +280,10 @@ void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &fr
if(!(d_x == 0 && d_y == 0)){
- int y = i+d_y;
- int x = j+d_x;
- if( ((0 <= x && x < dst.cols) && (0 <= y && y < dst.rows)) && (frameMap.at<cv::Point2i>(y,x) != cv::Point2i(-1,-1)) ){
- color = dst.at<cv::Vec3b>(y,x);
+ int offset_y = y+d_y;
+ int offset_x = x+d_x;
+ if( ((0 <= offset_x && offset_x < dst.cols) && (0 <= offset_y && offset_y < dst.rows)) && (frameMap.at<cv::Point2i>(offset_y,offset_x) != cv::Point2i(-1,-1)) ){
+ color = dst.at<cv::Vec3b>(offset_y,offset_x);
found = true;
break;
}
@@ -297,7 +294,8 @@ void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &fr
}
if(found){
- dst.at<cv::Vec3b>(i,j) = color;
+ dst.at<cv::Vec3b>(y,x) = color;
+ }
}
}
}
@@ -305,6 +303,31 @@ void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &fr
}
+/*
+* Fixe holes formed by the deprojection due to round up coordinates
+* (because deproject goes from 3D floats values to 2D uint),
+* by filling with value of the 1st non null neighbour
+*
+* dst : The output image to project
+* frameMap : The map describing where are the source pixels for each pixel in our output image
+*/
+void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap){
+ std::vector<std::thread> threads;
+ // Creation of n submatrix of size 100x100
+ // Each matrix will be managed by a different thread
+ int partition_len = 100;
+ for (int start_y = 0; start_y < frameMap.rows; start_y += partition_len){
+ for(int start_x = 0; start_x < frameMap.cols; start_x += partition_len){
+ threads.emplace_back(std::thread(&Projection::holeFillingThread, this, start_x, start_y, partition_len, std::ref(dst), std::ref(frameMap)));
+ }
+ }
+
+ for(auto& th: threads){
+ th.join();
+ }
+}
+
+
/*
* C : Camera position
* B : Beamer position
--
GitLab
From 44eabb047b32c2ae1626375d00ee8a43eadd4ab3 Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Mon, 10 Mar 2025 16:18:21 +0100
Subject: [PATCH 5/9] multithread should work
---
inc/projection.h | 4 +-
inc/projection.h.old | 50 +++++
src/common.mk | 2 +-
src/components/projection.cpp | 14 +-
src/components/projection.cpp.old | 305 ++++++++++++++++++++++++++++++
5 files changed, 365 insertions(+), 10 deletions(-)
create mode 100644 inc/projection.h.old
create mode 100644 src/components/projection.cpp.old
diff --git a/inc/projection.h b/inc/projection.h
index 6f521f7..219e688 100644
--- a/inc/projection.h
+++ b/inc/projection.h
@@ -25,12 +25,12 @@ class Projection{
cv::Point2f fxy;
cv::Point2f ppxy;
- void deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
+ void deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos);
void filterLowestDeprojectedPointsThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void buildFrameThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
void holeFillingThread(int startx, int start_y, int partition_len, cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap);
- void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
+ void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos);
void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
void holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap);
diff --git a/inc/projection.h.old b/inc/projection.h.old
new file mode 100644
index 0000000..3dc495d
--- /dev/null
+++ b/inc/projection.h.old
@@ -0,0 +1,50 @@
+#ifndef SANDBOX_PROJECTION_H
+#define SANDBOX_PROJECTION_H
+
+#include <opencv2/opencv.hpp>
+#include "beamer.h"
+#include "camera.h"
+
+class Projection{
+ private:
+ cv::Mat_<float> adjustingMatrix;
+ // based angle to find adjustingMatrix
+ double angleRotation = 0.0;
+ float distanceTopSandbox;
+
+ // resized depth frame to dst resolution
+ cv::Mat_<float> resized_depth;
+ // resized src to project to dst resolution
+ cv::Mat_<cv::Vec3b> resized_src;
+ // Buffer containing the pixels's new location when deprojected to beamer's POV
+ cv::Mat_<cv::Point2i> deprojectMap;
+ // Buffer indicating from where to get the pixels in the source frame to build the output
+ cv::Mat_<cv::Point2i> frameMap;
+
+ // intrinsics parameters for deprojection, which are adapted to the projection's resolution
+ cv::Point2f fxy;
+ cv::Point2f ppxy;
+
+ void deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy);
+ void filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap);
+ void buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst);
+ void holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap);
+ cv::Point2i findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f beamer_pos, cv::Point2f fxy, cv::Point2f ppxy);
+
+ public:
+ Projection();
+
+ void setAdjustingMatrix(cv::Mat_<float> matrix){ adjustingMatrix = matrix; };
+ cv::Mat_<float> getAdjustingMatrix(){ return adjustingMatrix; };
+ void setAngleRotation(double angle){ angleRotation = angle; };
+ double getAngleRotation(){ return angleRotation; };
+ void setDistanceTopSandbox(float dist){ distanceTopSandbox = dist; };
+ float getDistanceTopSandbox(){ return distanceTopSandbox; };
+
+ cv::Point2i rotatePixel(cv::Point2i center, double angle, cv::Point2i pixel);
+ cv::Point2i revertRotatePixel(cv::Point2i center, double angle, cv::Point2i rotatedPixel);
+ void adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos);
+ void printAdjustingMatrix();
+
+};
+#endif
diff --git a/src/common.mk b/src/common.mk
index ac9de75..68b62e1 100644
--- a/src/common.mk
+++ b/src/common.mk
@@ -1,2 +1,2 @@
CCP=g++
-CFLAGS=-std=c++11 -Wall -Wextra -g -Ilib -fPIC -I/opt/opencv3/include -I/usr/local/include -L/usr/local/lib -lyaml-cpp
+CFLAGS=-std=c++11 -Wall -Wextra -g -Ilib -fPIC -I/opt/opencv3/include -I/usr/local/include -L/usr/local/lib -lyaml-cpp -lpthread
diff --git a/src/components/projection.cpp b/src/components/projection.cpp
index bb61b49..c2e539b 100644
--- a/src/components/projection.cpp
+++ b/src/components/projection.cpp
@@ -53,7 +53,7 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
cv::resize(depth, resized_depth, dst.size());
// 130ms EXEC -> ~50ms
- deprojectPixelsFromDepth(resized_depth, camera, camera->getCroppingMask() , beamer_pos, deprojectMap, fxy, ppxy);
+ deprojectPixelsFromDepth(resized_depth, camera, camera->getCroppingMask() , beamer_pos);
// 40ms EXEC -> ~20ms
filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
@@ -83,10 +83,10 @@ void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, c
* fxy : function x and y adapted to the projection matching the original depth matrix(without ROI) of the camera
* ppxy : coordinates x and y of the central pixel adapted to the projection matching the original depth matrix(without ROI) of the camera
*/
-void Projection::deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, int cols, int rows, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
+void Projection::deprojectPixelsFromDepthThread(int start_x, int start_y, int partition_len, cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos){
// scale coord of the mask with the new resolution of the depth frame
- float mask_scale_x = mask.x * cols / mask.width;
- float mask_scale_y = mask.y * rows / mask.height;
+ float mask_scale_x = mask.x * depth.cols / mask.width;
+ float mask_scale_y = mask.y * depth.rows / mask.height;
// coordinates of the pixel relative to the orginial image taken from the camera
for(int y = start_y; y < start_y + partition_len; y++){
@@ -106,7 +106,7 @@ void Projection::deprojectPixelsFromDepthThread(int start_x, int start_y, int pa
pixel.y -= mask_scale_y;
// No need to lock ressource, we are sure to write at a different point each time
- deprojectMap.at<cv::Point2i>(x,y) = pixel;
+ deprojectMap.at<cv::Point2i>(y,x) = pixel;
}
}
}
@@ -126,14 +126,14 @@ void Projection::deprojectPixelsFromDepthThread(int start_x, int start_y, int pa
* fxy : function x and y adapted to the projection matching the original depth matrix(without ROI) of the camera
* ppxy : coordinates x and y of the central pixel adapted to the projection matching the original depth matrix(without ROI) of the camera
*/
-void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
+void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos){
std::vector<std::thread> threads;
// Creation of n submatrix of size 100x100
// Each matrix will be managed by a different thread
int partition_len = 100;
for (int start_y = 0; start_y < depth.rows; start_y += partition_len){
for(int start_x = 0; start_x < depth.cols; start_x += partition_len){
- threads.emplace_back(std::thread(&Projection::deprojectPixelsFromDepthThread, this, start_x, start_y, partition_len, std::ref(depth), depth.cols, depth.rows, camera, mask, beamer_pos, std::ref(deprojectMap), fxy, ppxy));
+ threads.emplace_back(std::thread(&Projection::deprojectPixelsFromDepthThread, this, start_x, start_y, partition_len, std::ref(depth), camera, mask, beamer_pos));
}
}
diff --git a/src/components/projection.cpp.old b/src/components/projection.cpp.old
new file mode 100644
index 0000000..c6b1436
--- /dev/null
+++ b/src/components/projection.cpp.old
@@ -0,0 +1,305 @@
+/*
+* Projection
+*
+* Class which adapts a projected image to the topology of the sandbox and the beamer point of view.
+* Contains the matrix to compensate for the rotation of the beamer
+* and the distance to the plane at the top of the sandbox.
+*/
+
+#include "../../inc/projection.h"
+
+/*
+ * MAIN
+ */
+
+Projection::Projection(){
+ adjustingMatrix = cv::getRotationMatrix2D(cv::Point(0, 0), 0, 1);
+ distanceTopSandbox = 1.0f;
+}
+
+
+/*
+* Adjust the projected frame with the topology from the camera to the beamer POV (point of view)
+*
+* depth : Topology of the sandbox under the projection
+* src : Image source projected to adjust to the topology
+* dst : Output which will contain the adapted image
+* camera : Active camera
+* beamer_pos : 3D position of the beamer relative to the camera
+*/
+void Projection::adjustFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst, Camera *camera, cv::Point3f beamer_pos){
+
+ // update if dst.size changed, since for all buffers :
+ // buff.size == dst.size
+ if(deprojectMap.size() != dst.size()){
+ std::vector<cv::Point2f> profil = camera->getAdaptedIntrinsics(dst);
+ fxy = profil.at(0);
+ ppxy = profil.at(1);
+ }
+ // do nothing if contains matrix with same size
+ // otherwise => release => allocate
+ deprojectMap.create(dst.rows, dst.cols);
+ frameMap.create(dst.rows, dst.cols);
+ resized_depth.create(dst.rows, dst.cols);
+ resized_src.create(dst.rows, dst.cols);
+
+ deprojectMap = cv::Point2i(-1,-1);
+ frameMap = cv::Point2i(-1,-1);
+
+ // resize to match 1:1 ratio with dst, since we'll do later:
+ // dst[i] = src[i]
+ cv::resize(src, resized_src, dst.size());
+ cv::resize(depth, resized_depth, dst.size());
+
+ deprojectPixelsFromDepth(resized_depth, camera, camera->getCroppingMask() , beamer_pos, deprojectMap, fxy, ppxy);
+ filterLowestDeprojectedPoints(resized_depth, deprojectMap, frameMap);
+ buildFrame(resized_depth, frameMap, resized_src, dst);
+ holeFilling(dst, frameMap);
+ cv::warpAffine(dst, dst, adjustingMatrix, dst.size()); // apply the rotation on the image
+}
+
+
+
+/*
+ * Private
+ */
+
+
+/*
+* Deproject pixels in 3D, then adapt to Beamer's POV, and go back to 2D
+* This gives us the location of pixels adapted to the Beamer projection
+*
+* depth : Topology of the sandbox under the projection
+* camera : Active camera
+* mask : ROI (Range Of Interest) delimiting the zone of the projection from the camera POV
+* beamer_pos : 3D position of the beamer relative to the camera
+* deprojectMap : Indicates for each pixel of src, where it'll be displayed
+* fxy : function x and y adapted to the projection matching the original depth matrix(without ROI) of the camera
+* ppxy : coordinates x and y of the central pixel adapted to the projection matching the original depth matrix(without ROI) of the camera
+*/
+void Projection::deprojectPixelsFromDepth(cv::Mat_<float> &depth, Camera *camera, cv::Rect mask, cv::Point3f beamer_pos, cv::Mat_<cv::Point2i> &deprojectMap, cv::Point2f fxy, cv::Point2f ppxy){
+
+ // scale coord of the mask with the new resolution of the depth frame
+ float mask_scale_x = mask.x * depth.cols / mask.width;
+ float mask_scale_y = mask.y * depth.rows / mask.height;
+
+ for (int i = 0; i < depth.rows; i++){
+ for (int j = 0; j < depth.cols; j++){
+ // coordinates of the pixel relative to the orginial image taken from the camera
+ int base_x = mask_scale_x+j;
+ int base_y = mask_scale_y+i;
+ float z = depth.at<float>(i,j);
+
+ // pixels based on the original depth frame taken from the camera
+ cv::Point2i pixel = findMatchingPixel( base_y, base_x, z, camera, beamer_pos, fxy, ppxy );
+
+ // pixel relative to the cropping mask (the area where the frame is projected)
+ pixel.x -= mask_scale_x;
+ pixel.y -= mask_scale_y;
+
+ deprojectMap.at<cv::Point2i>(i,j) = pixel;
+ }
+ }
+}
+
+/*
+* Save the highest points in deprojectMap into frameMap,
+* because some points can be deprojected at the same location
+*
+* depth : Topology of the sandbox matching the projection
+* deprojectMap : Indicates for each pixel of src, where it'll be displayed
+* frameMap : Indicates for each pixel of dst, where it should get the value from in src
+*/
+void Projection::filterLowestDeprojectedPoints(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &deprojectMap, cv::Mat_<cv::Point2i> &frameMap){
+
+ for (int i = 0; i < deprojectMap.rows; i++){
+ for (int j = 0; j < deprojectMap.cols; j++){
+
+ // coords of the new pixel
+ cv::Point2i deprojectedPixel = deprojectMap.at<cv::Point2i>(i,j);
+ cv::Point2i highestDepthPixel = cv::Point2i(j,i);
+
+ if( (0 <= deprojectedPixel.x && deprojectedPixel.x < depth.cols) &&
+ (0 <= deprojectedPixel.y && deprojectedPixel.y < depth.rows) ){
+
+ // check and keep the highest point at the location pointed by pixel
+ cv::Point2i currentDepthPixel = frameMap.at<cv::Point2i>(deprojectedPixel);
+ if( (0 <= currentDepthPixel.x && currentDepthPixel.x < depth.cols) &&
+ (0 <= currentDepthPixel.y && currentDepthPixel.y < depth.rows) ){
+ if(depth.at<float>(currentDepthPixel) <= depth.at<float>(i,j)){
+ highestDepthPixel = currentDepthPixel;
+ }
+ }
+ frameMap.at<cv::Point2i>(deprojectedPixel) = highestDepthPixel;
+ }
+ }
+ }
+}
+
+
+/*
+* Build the frame using frameMap, we assume that all the buffers have the same size
+* where each pixel describes in which pixel of the source it should take the value from
+* dst[i] = src[frameMap[i]]
+*
+* frameMap : The map describing where are the source pixels for each pixel in our output image
+* src : Image source to adapt
+* dst : Output image adapted from src to project
+*/
+// TODO : enlever depth en paramètre et vérifier que le pixel soit dans la range d'un des autres buffer
+void Projection::buildFrame(cv::Mat_<float> &depth, cv::Mat_<cv::Point2i> &frameMap, cv::Mat_<cv::Vec3b> &src, cv::Mat_<cv::Vec3b> &dst){
+
+ for (int i = 0; i < frameMap.rows; i++){
+ for (int j = 0; j < frameMap.cols; j++){
+
+ cv::Point2i pixel_src = frameMap.at<cv::Point2i>(i,j);
+ cv::Point2i pixel_dst = cv::Point2i(j,i);
+
+ if( (0<=pixel_src.x && pixel_src.x<depth.cols) && (0<=pixel_src.y && pixel_src.y<depth.rows) ){
+ // src and dst must be of same size
+ dst.at<cv::Vec3b>(pixel_dst) = src.at<cv::Vec3b>(pixel_src);
+ }
+ }
+ }
+}
+
+
+/*
+* Fixe holes formed by the deprojection due to round up coordinates
+* (because deproject goes from 3D floats values to 2D uint),
+* by filling with value of the 1st non null neighbour
+*
+* dst : The output image to project
+* frameMap : The map describing where are the source pixels for each pixel in our output image
+*/
+void Projection::holeFilling(cv::Mat_<cv::Vec3b> &dst, cv::Mat_<cv::Point2i> &frameMap){
+
+ for (int i = 0; i < dst.rows; i++){
+ for (int j = 0; j < dst.cols; j++){
+
+ if(frameMap.at<cv::Point2i>(i,j) == cv::Point2i(-1,-1)){
+
+ cv::Vec3b color;
+ bool found = false;
+
+ for(int d_y = -1; d_y <= 1; d_y++){
+ for(int d_x = -1; d_x <= 1; d_x++){
+
+ if(!(d_x == 0 && d_y == 0)){
+
+ int y = i+d_y;
+ int x = j+d_x;
+ if( ((0 <= x && x < dst.cols) && (0 <= y && y < dst.rows)) && (frameMap.at<cv::Point2i>(y,x) != cv::Point2i(-1,-1)) ){
+ color = dst.at<cv::Vec3b>(y,x);
+ found = true;
+ break;
+ }
+ }
+ }
+ if(found)
+ break;
+ }
+
+ if(found){
+ dst.at<cv::Vec3b>(i,j) = color;
+ }
+ }
+ }
+ }
+}
+
+
+/*
+* C : Camera position
+* B : Beamer position
+* P : Point computed by camera depth
+* V : Point at the plane height adjusted to the beamer POV (Point Of View)
+* E : Point of the right-angle triangle VEB at the plane height
+* A : Point of the right-angle triangle PAB at the point P height
+*
+* Where
+* CP : distance from camera to point (value of depth_frame)
+* CB : distance from camera to beamer (beamer's position is relative to the camera)
+*
+* i : y coordinate of the pixel to adjust
+* j : x coordinate of the pixel to adjust
+* z : Z coordinate of the pixel from the depth matrix
+* camera : Camera active
+* CB : Position of the beamer relative to the camera (vector camera-beamer)
+* fxy : function x and y adapted to the projection matching the original depth matrix(without ROI) of the camera
+* ppxy : coordinates x and y of the central pixel adapted to the projection matching the original depth matrix(without ROI) of the camera
+*
+* Return the coordinates of the pixel source adapted to the beamer POV
+*/
+cv::Point2i Projection::findMatchingPixel(int i, int j, float z, Camera *camera, cv::Point3f CB, cv::Point2f fxy, cv::Point2f ppxy){
+
+ float pixel[2] = {static_cast<float>(j), static_cast<float>(i)};
+
+ cv::Point3f CP = camera->deprojectPixelToPoint(pixel, z, fxy, ppxy);
+ cv::Point3f BP = CP - CB;
+
+ const float BEz = distanceTopSandbox - CB.z;
+ float BAz = BP.z;
+ float alpha = BEz / BAz;
+
+ cv::Point3f BV = (alpha * BP);
+ cv::Point3f CV = CB + BV;
+
+ return camera->projectPointToPixel(CV, fxy, ppxy);
+}
+
+
+
+
+
+
+/*
+* Sandbox Setup purpose
+*/
+
+// TODO : move rotatePixel and revertRotatePixel in SandboxSetup (they don't depend on Projection anymore)
+
+
+/*
+* Rotate a pixel to compensate for the rotate of the beamer
+*/
+cv::Point2i Projection::rotatePixel(cv::Point2i center, double angle, cv::Point2i pixel){
+
+ cv::Mat_<float> matRotation = cv::getRotationMatrix2D(center, angle, 1);
+ cv::Mat tmp = (cv::Mat_<cv::Vec2f>(1, 1) << cv::Vec2f(pixel.x, pixel.y));
+ cv::transform(tmp, tmp, matRotation);
+ return cv::Point2i(tmp.at<cv::Vec2f>(0, 0));
+}
+
+/*
+* Rotate back a rotated pixel to match the projection of the beamer
+*/
+cv::Point2i Projection::revertRotatePixel(cv::Point2i center, double angle, cv::Point2i rotatedPixel){
+
+ cv::Mat_<float> matRotation = cv::getRotationMatrix2D(center, angle, 1);
+ cv::Mat tmp = (cv::Mat_<cv::Vec2f>(1, 1) << cv::Vec2f(rotatedPixel.x, rotatedPixel.y));
+ cv::Mat invMat;
+ cv::invertAffineTransform(matRotation, invMat);
+ cv::transform(tmp, tmp, invMat);
+ return cv::Point2i(tmp.at<cv::Vec2f>(0, 0));
+}
+
+
+
+
+
+
+/*
+* Debug
+*/
+
+void Projection::printAdjustingMatrix(){
+
+ cv::Mat matrix = getAdjustingMatrix();
+ for (int y = 0; y < matrix.rows; y++){
+ for (int x = 0; x < matrix.cols; x++){
+ std::cout << (float)matrix.at<double>(y, x) << " ";
+ }
+ std::cout << std::endl;
+ }
+}
--
GitLab
From 6594f79efb5c38842036a08dff05fe5960c6937e Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Mon, 10 Mar 2025 19:03:52 +0100
Subject: [PATCH 6/9] added a cmake file to help building more efficiently
---
.gitignore | 2 ++
CMakeLists.txt | 68 ++++++++++++++++++++++++++++++++++++++++++++++++++
dep.mk | 3 +++
3 files changed, 73 insertions(+)
create mode 100644 .gitignore
create mode 100644 CMakeLists.txt
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..dc45f35
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+.idea/
+build_test/
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..82c4887
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.10)
+project(libsandbox)
+
+# Définition de la version de la bibliothèque
+set(LIBNAME "sandbox")
+set(LIB_MINOR_VERS "0.0")
+set(LIB_MAJOR_VERS "1")
+set(LIB_VERS "${LIB_MAJOR_VERS}.${LIB_MINOR_VERS}")
+set(LIB_FULL_NAME "${LIBNAME}.so.${LIB_VERS}")
+
+# Define the output directory for shared libraries
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
+
+# Ensure the directory exists (not strictly necessary, but good practice)
+file(MAKE_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY})
+
+# Trouver OpenCV (ou d'autres bibliothèques si besoin)
+find_package(OpenCV REQUIRED)
+include_directories(inc ${OpenCV_INCLUDE_DIRS})
+
+find_package(realsense2 REQUIRED )
+include_directories(inc ${realsense_INCLUDE_DIR})
+
+find_package(yaml-cpp REQUIRED)
+include_directories(inc ${YAML_CPP_INCLUDE_DIRS})
+
+# Définition des dossiers d'inclusion (headers)
+include_directories(inc)
+
+# Recherche tous les fichiers sources dans src/ et ses sous-dossiers
+file(GLOB_RECURSE SOURCES
+ src/components/*.cpp
+ src/lib/*.cpp
+ src/tools/*.cpp
+)
+
+# Création de la bibliothèque partagée
+add_library(${LIBNAME} SHARED ${SOURCES})
+set_target_properties(${LIBNAME} PROPERTIES OUTPUT_NAME ${LIBNAME} VERSION ${LIB_VERS} SOVERSION ${LIB_MAJOR_VERS})
+
+target_link_libraries(${LIBNAME} ${OpenCV_LIBS})
+target_link_libraries(${LIBNAME} ${realsense_LIBS})
+target_link_libraries(${LIBNAME} ${YAML_CPP_LIBRARIES})
+
+# Définition des options de compilation
+target_compile_options(${LIBNAME} PRIVATE
+ -std=c++11 # Utilisation de C++11
+ -Wall # Affichage des avertissements classiques
+ -Wextra # Avertissements supplémentaires
+ -g # Ajout des symboles de debug
+ -fPIC # Position Indépendante du Code (obligatoire pour les bibliothèques partagées)
+)
+
+# Création des liens symboliques après la compilation
+add_custom_command(TARGET ${LIBNAME} POST_BUILD
+ COMMAND ln -sf ${LIBNAME}.so.${LIB_MAJOR_VERS}
+ COMMAND ln -sf ${LIBNAME}.so
+ WORKING_DIRECTORY ${CMAKE_LIBRARY_OUTPUT_DIRECTORY}
+ COMMENT "Création des liens symboliques pour ${LIBNAME}"
+)
+
+# Installation de la bibliothèque et des fichiers d'en-tête
+install(TARGETS ${LIBNAME} DESTINATION lib)
+install(DIRECTORY inc/ DESTINATION include)
+
+# Installation des liens symboliques
+install(CODE "execute_process(COMMAND ln -sf ${LIB_FULL_NAME} ${CMAKE_INSTALL_PREFIX}/lib/${LIBNAME}.so.${LIB_MAJOR_VERS})")
+install(CODE "execute_process(COMMAND ln -sf ${LIB_FULL_NAME} ${CMAKE_INSTALL_PREFIX}/lib/${LIBNAME}.so)")
diff --git a/dep.mk b/dep.mk
index 3a9af19..1df8847 100644
--- a/dep.mk
+++ b/dep.mk
@@ -3,3 +3,6 @@ CAMERAFLAG=-lrealsense2
YAMLFLAG=-I/usr/local/include -L/usr/local/lib -lyaml-cpp
DEP_SANDBOX=$(OPENCVFLAG) $(CAMERAFLAG) $(YAMLFLAG)
+
+CCP=g++
+CFLAGS=-std=c++11 -Wall -Wextra -g -Ilib -fPIC -I/opt/opencv3/include -I/usr/local/include -L/usr/local/lib -lyaml-cpp -lpthread
--
GitLab
From b17347e5409545b47e4f4b634581f255533105f5 Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Wed, 12 Mar 2025 15:55:40 +0100
Subject: [PATCH 7/9] Works with latest version of opencv and yaml-cpp
---
inc/sandboxConfig.h | 1 +
src/components/beamer.cpp | 6 +++---
2 files changed, 4 insertions(+), 3 deletions(-)
diff --git a/inc/sandboxConfig.h b/inc/sandboxConfig.h
index 0fc9b23..5379707 100644
--- a/inc/sandboxConfig.h
+++ b/inc/sandboxConfig.h
@@ -1,6 +1,7 @@
#ifndef SANDBOX_CONFIG_H
#define SANDBOX_CONFIG_H
+#include <fstream>
#include <yaml-cpp/yaml.h>
#include <opencv2/opencv.hpp>
#include "camera.h"
diff --git a/src/components/beamer.cpp b/src/components/beamer.cpp
index f8a9c5a..4155c19 100644
--- a/src/components/beamer.cpp
+++ b/src/components/beamer.cpp
@@ -86,7 +86,7 @@ std::vector<cv::Point3i> Beamer::findCircles(cv::Mat &rgb, double contrast, int
std::vector<cv::Vec3f> circles;
circles.clear();
- cv::cvtColor(rgb, src_gray, CV_BGR2GRAY);
+ cv::cvtColor(rgb, src_gray, cv::COLOR_BGR2GRAY);
// Reduce the noise so we avoid false circle detection
cv::GaussianBlur(src_gray, src_gray, cv::Size(9, 9), 2, 2);
src_gray = editContrast(src_gray, (double)contrast, (double)brightness);
@@ -102,7 +102,7 @@ std::vector<cv::Point3i> Beamer::findCircles(cv::Mat &rgb, double contrast, int
// param_2 : Threshold for the center detection, after the accumulator is complet, threshold to keep only the possible centers
// min_radius : Min radius of the circles drawn on the accumulator
// max_radius : Max radius of the circles drawn on the accumulator
- cv::HoughCircles(src_gray, circles, CV_HOUGH_GRADIENT, 1, centersMinDist, (double)cannyEdgeThreshold, (double)houghAccThreshold, minRadius, maxRadius);
+ cv::HoughCircles(src_gray, circles, cv::HOUGH_GRADIENT, 1, centersMinDist, (double)cannyEdgeThreshold, (double)houghAccThreshold, minRadius, maxRadius);
// Point with (x,y) and radius of the circle
std::vector<cv::Point3i> result;
@@ -160,7 +160,7 @@ cv::Mat Beamer::buildCrossFrame(cv::Point projectedCross, int step, int max, boo
void Beamer::findLinearLine(std::vector<cv::Point3f> *capturedPoints, std::vector<cv::Point3d> *bases, std::vector<cv::Point3d> *directions){
cv::Vec6f line;
- cv::fitLine(*capturedPoints, line, CV_DIST_L2, 0, 0.01, 0.01);
+ cv::fitLine(*capturedPoints, line, cv::DIST_L2, 0, 0.01, 0.01);
cv::Point3d direction = cv::Point3d(line[0], line[1], line[2]);
cv::Point3d base = cv::Point3d(line[3], line[4], line[5]);
--
GitLab
From 8c63af3bfb50a5c1336e9c1d1f9efdf5cb7a271b Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Wed, 12 Mar 2025 16:04:14 +0100
Subject: [PATCH 8/9] Updated README for multithread version of library
---
README.md | 30 ++++++++++++------------------
1 file changed, 12 insertions(+), 18 deletions(-)
diff --git a/README.md b/README.md
index 2f320f4..a5d90b0 100644
--- a/README.md
+++ b/README.md
@@ -5,31 +5,25 @@
- The camera must be pluged with an usb 3.0 extension (otherwise the resolution of the frames will be lowered)
## Depedencies
- - OS : Ubuntu 18.04
+ - OS : Arch (Endeavour), Ubuntu
- C++ : 11.0
- g++ : 7.5.0
- - OpenCV : 3.2.0
- - librealsense 2 : 2.35.2
- - yaml-cpp : https://github.com/jbeder/yaml-cpp @commit : 9fb51534877d16597cfd94c18890d87af0879d65
+ - OpenCV : 4.x.x
+ - librealsense 2 : 2.x.x
+ - yaml-cpp : https://github.com/jbeder/yaml-cpp (latest)
- Qt : 5.9.5
-## Makefile
- - make lib : build the librairy
- - make apps : build the setup application
- - make clean : clean all generated files
-
## Makefile for applications
Examples can be found in the project ar_sandbox_app
-### Compilation
-You need to specify :
- - The path (can be relative) to the headers of the classes in the project with the parameter -I (this is an upper case i)
-
-### Linker
-You need to specify :
- - The path (can be relative) to the generated shared object library (file .so) with the parameter -L
- - The flag of the generated library (-lsandbox in our case)
- - The flags of the libraries used in the project for the linker, which are availible throught the variable DEP_SANDBOX in file dep.mk
+## Installation
+ - run `mkdir build` if build directory does not exist and `cd` into it.
+ - in build directory, run `cmake ..`
+ - in build directory run `cmake --build .`
+ - Lib should be compiled in lib directory of the build directory.
+ - If a problem arise, check if all dependencies are met.
+ - to clean project run `cmake --build . --target clean`
+ - to uninstall completly, delete build directory.
## Before launching your application
- LD_LIBRARY_PATH must contain the path to the directory containing libsandbox.so (which is generated in /build)
--
GitLab
From 174a448b02dda1132837756fe1f0716f922de69c Mon Sep 17 00:00:00 2001
From: quentin <quentin.leblanc@hesge.ch>
Date: Wed, 12 Mar 2025 16:05:50 +0100
Subject: [PATCH 9/9] updated gitignore to ignore build directory
---
.gitignore | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/.gitignore b/.gitignore
index dc45f35..2388c53 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,2 @@
.idea/
-build_test/
+build/
\ No newline at end of file
--
GitLab