Need help in visualising the lines with self implemented hough line detector in matlab

Question

I was able to recreate the hough() and houghpeaks() function in Matlab with the following codes, but I am currently struggling to dehough them from parameter space to image space.

%%% hough() function

corridor = imread('Corridor1.jpg');
grayCorridor = rgb2gray(corridor);
Canny_img = edge(grayCorridor, 'Canny');
[rows, cols] = size(Canny_img);
theta_maximum = 90;
rho_maximum = hypot(rows,cols) - 1;
theta_range = -theta_maximum:theta_maximum-1;
rho_range = -rho_maximum:rho_maximum;

Hough = zeros(length(rho_range), length(theta_range));
for j = 1:rows
    for i = 1:cols
        if Canny_img(j, i) ==1
            x = i - 1;
            y = j - 1;
            for T = theta_range
                R = round((x * cosd(T)) + (y * sind(T)));
                R_Index = R + rho_maximum + 1;
                T_Index = T + theta_maximum + 1;
                Hough(R_Index, T_Index) = Hough(R_Index, T_Index) + 1;
            end
        end
    end
end

%%% houghpeaks() function

[X,Y] = find(Hough>0.47*max(Hough(:)))

%Visualising them 

imshow(Hough,[],'XData',theta_range,'YData',rho_range,...
            'InitialMagnification','fit');
xlabel('\theta'), ylabel('\rho');
axis on, axis normal, hold on;

pointsRho = rho_range(X(:));
pointsTheta = theta_range(Y(:));
plot(pointsTheta,pointsRho,'s','color','red');

Here is my attempt at dehoughing them to image space. I first get the Rho and Theta pair into a list. I then created a set of binary matrices of the size imageRow x imageColumn x number of rhotheta pair. Then I substitute all the x and y pixel coordinates into the function xcos(theta) + ysin(theta) = rho, and if left - right == 0, I would mark the location of the binary matrix(x,y) as 1 and I run this for all the theta pairs against all the x and y value and store them separately in the set of binary matrices mentioned above.

In the second for loop, I just went through all of the set of binary matrices I had obtained and just aggregate them into one binary matrix. My problem comes here, how do I combine the lines that I dehoughed into one line when they are close to each other? If I am able to do that, I might be able to visualize more houghpeaks by lowering the threshold I have set with [X,Y] = find(Hough>0.47*max(Hough(:))). Because right now if I were to lower the threshold by even 0.01, I would have many more lines that are so close to each other. And do you perhaps have a more elegant and better way of dehoughing?

for k = 1:length(pointsRho)
    for i = 1:1280 % Shoulda put 1:size(Canny_img,1) but putting number helps me visualise my logic 
                   % better while I am coding
        for j = 1:960
            r = pointsRho(k);
            t = pointsTheta(k);
            threshold = abs(i*cosd(t)+j*sind(t)-r);
            if threshold ==0
                allLines(j,i,k) = 1;
            end     
        end
    end
end

aggregatedImage = zeros(size(Canny_img));
for k = 1:length(pointsRho)
    for j = 1:1280
        for i = 1:960
            if allLines(i,j,k) ==1
                aggregatedImage(i,j) = 1;
            end
        end
    end
end

figure,imshow(imfuse(aggregatedImage,corridor))

P/S: I thought that I could have possibly made the two loops into one for loop if I were to mark all of the pixel coordinates that fulfill xcos(theta) + ycos(theta) = r right away in only one binary matrix (as shown in the following code). However, I am not sure why when I did that the aggregatedImage is just all 0. Hence I decided to go with the above approach.

aggregatedImage = zeros(size(Canny_img));

for k = 1:length(pointsRho)
    for i = 1:1280
        for j = 1:960
            r = pointsRho(k);
            t = pointsTheta(k);
            threshold = abs(i*cosd(t)+j*sind(t)-r);
            if threshold ==0
                aggregatedImage(j,i) = 1;
            end     
        end
    end
end