- Where did you get the camera module(s)?
ArduCAM Website
- Model number of the product(s)?
B0410 Camera ToF for Raspberry Pi et Jetson N
- What hardware/platform were you working on?
Raspbian for Raspberry Pi 3B+
- Instructions you have followed. (link/manual/etc.)
“Getting Started: Arducam ToF Camera for Raspberry Pi”
https://docs.arducam.com/Raspberry-Pi-Camera/Tof-camera/Getting-Started/
- Problems you were having?
I carried out several tests on ToF Camera for Raspberry Pi and Jetson N.
The first and simplest test was to assess the stability of depth measurement in a static scene, using the ./compile.sh program.
Unless I’m mistaken, this program allows me to obtain the average depth value of 8*8 pixels, when the left mouse click release event is TRUE, and the cursor is in the preview image.
Is it normal to obtain, despite the +/-2cm precision given in the technical documentation, for a real effective distance of 2m :
- a min at 1.1m
- a max at 1.977m
- an average from a series of 465 measurements = 1.500m
?
I’ve also modified the main in example/c to acquire the initial static environment (reference frame), followed by real-time acquisition and comparison of depth values.
Unless I store the depth values, for each pixel, of a large number of samples, then average these values, and finally compare these average values between the reference frame and real time, I find it very difficult to obtain repeatable acquisitions, for the same static reference frame.
This process is also very time-consuming, and doesn’t meet the requirements of my specifications for people detection or security presence detection, as it takes far too long and doesn’t match the kinetics of a daily scene.
Here’s my C source code. Can you help me and/or give me an alternative technical solution?
#include “ArducamDepthCamera.h”
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#define COLUMN 240 // 2⁴35
#define ROW 180 // 2²3²5 >> diviseurs communs 2/3/4/5
#define SIZE 43200
#define ITER_NB 600
float f_DepthTab[SIZE] = { 0 };
float f_SumTab[ROW][COLUMN] = { 0 };
float f_PreviousStatusTab[ROW][COLUMN] = { 0 };
float f_CurrentStatusTab[ROW][COLUMN] = { 0 };
void getPreview(uint8_t *preview_ptr, float *phase_image_ptr, float *amplitude_image_ptr);
void initTab2D(float tab[ROW][COLUMN]);
int compareArrays(float array1[ROW][COLUMN], float array2[ROW][COLUMN]);
void printProgressBar(int percentage);
void performInitialCapture(ArducamDepthCamera tof, uint8_t *preview_ptr, float resultTab[ROW][COLUMN]);
void performRealTimeAcquisition(ArducamDepthCamera tof, uint8_t *preview_ptr, float resultTab[ROW][COLUMN]);
int main()
{
ArducamDepthCamera tof = createArducamDepthCamera();
if (arducamCameraOpen(tof, CSI, 0))
exit(-1);
if (arducamCameraStart(tof, DEPTH_FRAME))
exit(-1);
uint8_t *preview_ptr = malloc(180 * 240 * sizeof(uint8_t));
float resultTab[ROW][COLUMN];
// Perform the initial capture
performInitialCapture(tof, preview_ptr, resultTab);
printf("\n");
for (;;)
{
performRealTimeAcquisition(tof, preview_ptr, resultTab);
//printf("\n\n");
int count = compareArrays(f_PreviousStatusTab, f_CurrentStatusTab);
/*
if (count >= 50){
printf("Attention !!! Target detection !\n");
}
else {
printf("No obstruction :)\n");
}
printf("\n\n");*/
sleep(1);
}
free(preview_ptr);
return 0;
}
void getPreview(uint8_t *preview_ptr, float *phase_image_ptr, float *amplitude_image_ptr)
{
unsigned long int len = 240 * 180;
for (unsigned long int i = 0; i < len; i++)
{
uint8_t amplitude = (amplitude_image_ptr + i) > 30 ? 254 : 0;
float phase = ((1 - ((phase_image_ptr + i) / 2)) * 255);
uint8_t depth = phase > 255 ? 255 : phase;
*(preview_ptr + i) = depth & amplitude;
}
}
void initTab2D(float tab[ROW][COLUMN]){
memset(tab, 0, ROW * COLUMN * sizeof(float));
}
int compareArrays(float array1[ROW][COLUMN], float array2[ROW][COLUMN]) {
int count = 0;
for (int x = 0; x < ROW; x++) {
for (int y = 0; y < COLUMN; y++) {
float diff = fabs(array1[x][y]-array2[x][y]);
float threshold = 0.25 * array1[x][y];
if (diff > threshold){
count++;
}
}
}
//printf("nb de cases dont les valeurs diffèrent de +- 5% : %d\n", count);
printf("%d\n", count);
return count;
}
void printProgressBar(int percentage){
printf(“[”);
for (int i = 0; i < 100; i += 2) {
if (i < percentage) {
printf(“#”);
}
else {
printf(" “);
}
}
printf(”] %d%%\r", percentage);
fflush(stdout);
}
void performInitialCapture(ArducamDepthCamera tof, uint8_t *preview_ptr, float resultTab[ROW][COLUMN]){
ArducamFrameBuffer frame;
int count = 0;
int progress = 0;
initTab2D(f_SumTab);
printf(“Acquisition de l’environnement de départ en cours:\n”);
float *amplitude_ptr = 0;
while (count < ITER_NB) {
if ((frame = arducamCameraRequestFrame(tof, 1000)) != 0x00) {
memcpy(f_DepthTab, arducamCameraGetDepthData(frame), SIZE * sizeof(float));
amplitude_ptr = (float *)arducamCameraGetAmplitudeData(frame);
getPreview(preview_ptr, f_DepthTab, amplitude_ptr);
for (int i = 0; i < SIZE; i++) {
int x = i / COLUMN;
int y = i % COLUMN;
f_SumTab[x][y] += f_DepthTab[i];
}
count++;
arducamCameraReleaseFrame(tof, frame);
// Mise à jour de la barre de progression
int newProgress = (int)((float)count / ITER_NB * 100);
if (newProgress != progress){
progress = newProgress;
printProgressBar(progress);
}
}
}
for (int x = 0; x < ROW; x++) {
for (int y = 0; y < COLUMN; y++) {
resultTab[x][y] = f_SumTab[x][y] / count;
}
}
memcpy(f_PreviousStatusTab, resultTab, ROW * COLUMN * sizeof(float));
sleep(1);
}
void performRealTimeAcquisition(ArducamDepthCamera tof, uint8_t *preview_ptr, float resultTab[ROW][COLUMN]){
ArducamFrameBuffer frame;
int count = 0;
int progress = 0;
initTab2D(f_SumTab);
//printf(“Acquisition en temps réel en cours:\n”);
float *amplitude_ptr = 0;
while (count < 60) {
if ((frame = arducamCameraRequestFrame(tof, 1000)) != 0x00) {
memcpy(f_DepthTab, arducamCameraGetDepthData(frame), SIZE * sizeof(float));
amplitude_ptr = (float *)arducamCameraGetAmplitudeData(frame);
getPreview(preview_ptr, f_DepthTab, amplitude_ptr);
for (int i = 0; i < SIZE; i++) {
int x = i / COLUMN;
int y = i % COLUMN;
f_SumTab[x][y] += f_DepthTab[i];
}
count++;
arducamCameraReleaseFrame(tof, frame);
/*
// Mise à jour de la barre de progression
int newProgress = (int)((float)count / 600 * 100);
if (newProgress != progress){
progress = newProgress;
printProgressBar(progress);
}*/
}
}
for (int x = 0; x < ROW; x++) {
for (int y = 0; y < COLUMN; y++) {
resultTab[x][y] = f_SumTab[x][y] / count;
}
}
memcpy(f_CurrentStatusTab, resultTab, ROW * COLUMN * sizeof(float));
}