How to calculate distance from camera to object using CV toolbox?

Question

I created a custom object detector which works pretty well but i am getting NaN for all values in my matrix. I found this out after debugging the code and the bounded region in my image shows NaN meters. I used the code from Matlab to compute the distance but I do not know if that is a general method of calulcating the distance. Below is the code.

points3D = reconstructScene(disparityMapBM,stereoParams);
points3D = points3D ./ 1000;
trainingData = objectDetectorTrainingData(gTruth);
myAFCdetector = trainACFObjectDetector(trainingData,'NumStages',5, 'NegativeSamplesFactor',2);

img = imread('Right_Image.jpg');

[bboxes,scores] = detect(myAFCdetector,img);

for i = 1:length(scores)
   annotation = sprintf('Confidence = %.1f',scores(i));
   I1 = insertObjectAnnotation(img,'rectangle',bboxes(i,:),annotation);
end

figure
imshow(I1)

centroids = [round(bboxes(:, 1) + bboxes(:, 3) / 2), ...
    round(bboxes(:, 2) + bboxes(:, 4) / 2)];

% Find the 3-D world coordinates of the centroids.
centroidsIdx = sub2ind(size(disparityMapBM), centroids(:, 2), centroids(:, 1));
X = points3D(:, :, 1);
Y = points3D(:, :, 2);
Z = points3D(:, :, 3);
centroids3D = [X(centroidsIdx)'; Y(centroidsIdx)'; Z(centroidsIdx)'];

% Find the distances from the camera in meters.
dists = sqrt(sum(centroids3D .^ 2));

% Display the detected people and their distances.
labels = cell(1, numel(dists));
for i = 1:numel(dists)
    labels{i} = sprintf('%0.2f meters', dists(i));
end
figure;
imshow(insertObjectAnnotation(I1, 'rectangle', bboxes, labels));
title('Detected People');

I found out NaN matrix at centroids3D. Could there be any reason why I am getting these matrix filled with NaN?

Centoids3D = [Nan NaN NaN NaN; Nan NaN NaN NaN; Nan NaN NaN NaN]

Answer 1

First you need to take reference object for which distance is known to get focal length and then its feasible to find the distance of object from camera.

Here is the full tutorial for reference.