I’m having issues with the text function. I am copying your example from the documentation, and I get the following error:
"Subscript indices must either be real positive integers or logicals."
Here is the example:
x = 0:pi/20:2*pi;
y = sin(x);
plot(x,y)
text(pi,0,’leftarrow sin(pi)’)
I am using Matlab 2013a. Very simple example, not sure why I can’t replicate the documentation example. Thank in advance!I’m having issues with the text function. I am copying your example from the documentation, and I get the following error:
"Subscript indices must either be real positive integers or logicals."
Here is the example:
x = 0:pi/20:2*pi;
y = sin(x);
plot(x,y)
text(pi,0,’leftarrow sin(pi)’)
I am using Matlab 2013a. Very simple example, not sure why I can’t replicate the documentation example. Thank in advance! I’m having issues with the text function. I am copying your example from the documentation, and I get the following error:
"Subscript indices must either be real positive integers or logicals."
Here is the example:
x = 0:pi/20:2*pi;
y = sin(x);
plot(x,y)
text(pi,0,’leftarrow sin(pi)’)
I am using Matlab 2013a. Very simple example, not sure why I can’t replicate the documentation example. Thank in advance! text, not a bug MATLAB Answers — New Questions

​

my extracted watermark image is not similar with original watermark image where there exist gray background in the extracted watermark image and the color also did not same as original.

this is my code using FWHT and SVD algorithm
% Read the watermarked image
%watermarked = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimageswatermarked_12.png’);

% Read the original host image
host = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimageshost 1.png’);
wm = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimagesw image 2.png’);

%EMBEDDING
% Resize watermark to match the host image size if necessary
wm = imresize(wm, size(host(:,:,1)));

% Extract individual color channels of the host image
h_red = host(:,:,1);
h_green = host(:,:,2);
h_blue = host(:,:,3);

% Extract individual color channels of the watermark image
wm_red = wm(:,:,1);
wm_green = wm(:,:,2);
wm_blue = wm(:,:,3);

% Apply FWHT to host image
transformed_h_red = fwht2(h_red);
transformed_h_green = fwht2(h_green);
transformed_h_blue = fwht2(h_blue);

% Apply FWHT to watermark image
transformed_wm_red = fwht2(wm_red);
transformed_wm_green = fwht2(wm_green);
transformed_wm_blue = fwht2(wm_blue);

% Apply SVD to FWHT-transformed host images
[U_h_red, S_h_red, V_h_red] = svd(transformed_h_red);
[U_h_green, S_h_green, V_h_green] = svd(transformed_h_green);
[U_h_blue, S_h_blue, V_h_blue] = svd(transformed_h_blue);

% Apply SVD to FWHT-transformed watermark images
[~, S_wm_red] = svd(transformed_wm_red);
[~, S_wm_green] = svd(transformed_wm_green);
[~, S_wm_blue] = svd(transformed_wm_blue);

% Modify the singular values of the host image by the singular values of the watermark image
alpha = 0.01; % Example scaling factor, adjust as needed
S_new_red = S_h_red + (alpha * S_wm_red);
S_new_green = S_h_green + (alpha * S_wm_green);
S_new_blue = S_h_blue + (alpha * S_wm_blue);

% Apply the inverse SVD to get the watermarked image channels
watermarked_red = U_h_red * S_new_red * V_h_red’;
watermarked_green = U_h_green * S_new_green * V_h_green’;
watermarked_blue = U_h_blue * S_new_blue * V_h_blue’;

% Apply inverse FWHT to each color channel of the watermarked image
watermarked_red = ifwht2(watermarked_red);
watermarked_green = ifwht2(watermarked_green);
watermarked_blue = ifwht2(watermarked_blue);

% Combine the RGB blocks to form the final watermarked color image
watermarked_color_image = cat(3, watermarked_red, watermarked_green, watermarked_blue);

% Scale the image data to the range [0, 1] and convert to uint8
watermarked = (255 * mat2gray(watermarked_color_image));
%watermarked = uint8(watermarked);

% Display the final watermarked color image in original size
%figure; % Open a new figure window
%imshow(watermarked, ‘InitialMagnification’, ‘fit’);
%title(‘Watermarked Image’);

% Open a dialog box to get the file path
%[filename, pathname] = uiputfile({‘*.png’;’*.jpg’;’*.bmp’;’*.tiff’}, ‘Save as’);

%if isequal(filename,0) || isequal(pathname,0)
% disp(‘User pressed cancel’)
%else
% Create the full file path
% full_filename = fullfile(pathname, filename);

% Save the watermarked color image to the chosen location
% imwrite(watermarked_color_image, full_filename);

% Display a message to indicate the image has been saved
% disp([‘Watermarked image saved as ‘, full_filename]);
%end

%END EMBED

%%
%EXTRACTION PROCESS
% Extract individual color channels of the watermarked image
%watermarked = imread(‘C:UsersUserDownloadswatermarked_12.png’);
% Read the original host image
%host = imread(‘C:UsersUserDownloadshost 1.png’);
%wm = imread(‘C:UsersUserDownloadsw image 1.png’);

%EMBEDDING
% Resize watermark to match the host image size if necessary
wm = imresize(wm, size(host(:,:,1)));

% % Extract individual color channels of the host image
% h_red = host(:,:,1);
% h_green = host(:,:,2);
% h_blue = host(:,:,3);

% Extract individual color channels of the watermark image
wm_red = wm(:,:,1);
wm_green = wm(:,:,2);
wm_blue = wm(:,:,3);

w_red = watermarked(:,:,1);
w_green = watermarked(:,:,2);
w_blue = watermarked(:,:,3);

% Extract individual color channels of the host image
h_red = host(:,:,1);
h_green = host(:,:,2);
h_blue = host(:,:,3);

% Apply FWHT to the watermarked image and the host image
transformed_w_red = fwht2(w_red);
transformed_w_green = fwht2(w_green);
transformed_w_blue = fwht2(w_blue);

transformed_wm_red = fwht2(wm_red);
transformed_wm_green = fwht2(wm_green);
transformed_wm_blue = fwht2(wm_blue);

transformed_h_red = fwht2(h_red);
transformed_h_green = fwht2(h_green);
transformed_h_blue = fwht2(h_blue);

% Apply SVD to the FWHT-transformed watermarked image
[U_w_red, S_w_red, V_w_red] = svd(transformed_w_red);
[U_w_green, S_w_green, V_w_green] = svd(transformed_w_green);
[U_w_blue, S_w_blue, V_w_blue] = svd(transformed_w_blue);

% Apply SVD to the FWHT-transformed watermarked image
[U_wm_red, S_wm_red, V_wm_red] = svd(transformed_wm_red);
[U_wm_green, S_wm_green, V_wm_green] = svd(transformed_wm_green);
[U_wm_blue, S_wm_blue, V_wm_blue] = svd(transformed_wm_blue);

% Apply SVD to the FWHT-transformed host image
[~, S_h_red, ~] = svd(transformed_h_red);
[~, S_h_green, ~] = svd(transformed_h_green);
[~, S_h_blue, ~] = svd(transformed_h_blue);

% Subtract the singular values of the host image from the singular values of the watermarked image
S_ext_red = (S_w_red – S_h_red)/alpha;
S_ext_green = (S_w_green – S_h_green)/alpha;
S_ext_blue = (S_w_blue – S_h_blue)/alpha;

% Apply the inverse SVD to acquire the extracted watermark image
extracted_red = U_wm_red * S_ext_red * V_wm_red’;
extracted_green = U_wm_green * S_ext_green * V_wm_green’;
extracted_blue = U_wm_blue * S_ext_blue * V_wm_blue’;

% Apply the inverse FWHT
extracted_red = ifwht2(extracted_red);
extracted_green = ifwht2(extracted_green);
extracted_blue = ifwht2(extracted_blue);

% Combine the RGB blocks to form the final extracted watermark image
extracted_watermark = cat(3, extracted_red, extracted_green, extracted_blue);

%%
% figure;imshow(uint8(extracted_watermark));
extracted_watermark = uint8(255 * mat2gray( (extracted_watermark)));
% extracted_watermark = uint8(extracted_watermark);
figure;imshow(extracted_watermark);

% Resize the extracted watermark image to 128 x 128
extracted_watermark = imresize(extracted_watermark, [128 128]);

% Display the host image, watermark image, watermarked image, and extracted image side by side
figure; % Open a new figure window
subplot(1, 4, 1);
imshow(host, ‘InitialMagnification’, ‘fit’);
title(‘Host Image’);

subplot(1, 4, 2);
imshow(wm, ‘InitialMagnification’, ‘fit’);
title(‘Watermark Image’);

watermarked = uint8(watermarked);
subplot(1, 4, 3);
imshow(watermarked, ‘InitialMagnification’, ‘fit’);
title(‘Watermarked Image’);

subplot(1, 4, 4);
imshow(extracted_watermark, ‘InitialMagnification’, ‘fit’);
title(‘Extracted Watermark Image’);

% Function to apply 2D FWHT
function transformed_image = fwht2(image)
% Apply the 2D FWHT
transformed_image = fwht(fwht(double(image)).’).’;
end

% Function to apply 2D inverse FWHT
function image = ifwht2(transformed_image)
% Apply the 2D inverse FWHT
image = ifwht(ifwht(double(transformed_image)).’).’;
endmy extracted watermark image is not similar with original watermark image where there exist gray background in the extracted watermark image and the color also did not same as original.

this is my code using FWHT and SVD algorithm
% Read the watermarked image
%watermarked = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimageswatermarked_12.png’);

% Read the original host image
host = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimageshost 1.png’);
wm = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimagesw image 2.png’);

%EMBEDDING
% Resize watermark to match the host image size if necessary
wm = imresize(wm, size(host(:,:,1)));

% Extract individual color channels of the host image
h_red = host(:,:,1);
h_green = host(:,:,2);
h_blue = host(:,:,3);

% Extract individual color channels of the watermark image
wm_red = wm(:,:,1);
wm_green = wm(:,:,2);
wm_blue = wm(:,:,3);

% Apply FWHT to host image
transformed_h_red = fwht2(h_red);
transformed_h_green = fwht2(h_green);
transformed_h_blue = fwht2(h_blue);

% Apply FWHT to watermark image
transformed_wm_red = fwht2(wm_red);
transformed_wm_green = fwht2(wm_green);
transformed_wm_blue = fwht2(wm_blue);

% Apply SVD to FWHT-transformed host images
[U_h_red, S_h_red, V_h_red] = svd(transformed_h_red);
[U_h_green, S_h_green, V_h_green] = svd(transformed_h_green);
[U_h_blue, S_h_blue, V_h_blue] = svd(transformed_h_blue);

% Apply SVD to FWHT-transformed watermark images
[~, S_wm_red] = svd(transformed_wm_red);
[~, S_wm_green] = svd(transformed_wm_green);
[~, S_wm_blue] = svd(transformed_wm_blue);

% Modify the singular values of the host image by the singular values of the watermark image
alpha = 0.01; % Example scaling factor, adjust as needed
S_new_red = S_h_red + (alpha * S_wm_red);
S_new_green = S_h_green + (alpha * S_wm_green);
S_new_blue = S_h_blue + (alpha * S_wm_blue);

% Apply the inverse SVD to get the watermarked image channels
watermarked_red = U_h_red * S_new_red * V_h_red’;
watermarked_green = U_h_green * S_new_green * V_h_green’;
watermarked_blue = U_h_blue * S_new_blue * V_h_blue’;

% Apply inverse FWHT to each color channel of the watermarked image
watermarked_red = ifwht2(watermarked_red);
watermarked_green = ifwht2(watermarked_green);
watermarked_blue = ifwht2(watermarked_blue);

% Combine the RGB blocks to form the final watermarked color image
watermarked_color_image = cat(3, watermarked_red, watermarked_green, watermarked_blue);

% Scale the image data to the range [0, 1] and convert to uint8
watermarked = (255 * mat2gray(watermarked_color_image));
%watermarked = uint8(watermarked);

% Display the final watermarked color image in original size
%figure; % Open a new figure window
%imshow(watermarked, ‘InitialMagnification’, ‘fit’);
%title(‘Watermarked Image’);

% Open a dialog box to get the file path
%[filename, pathname] = uiputfile({‘*.png’;’*.jpg’;’*.bmp’;’*.tiff’}, ‘Save as’);

%if isequal(filename,0) || isequal(pathname,0)
% disp(‘User pressed cancel’)
%else
% Create the full file path
% full_filename = fullfile(pathname, filename);

% Save the watermarked color image to the chosen location
% imwrite(watermarked_color_image, full_filename);

% Display a message to indicate the image has been saved
% disp([‘Watermarked image saved as ‘, full_filename]);
%end

%END EMBED

%%
%EXTRACTION PROCESS
% Extract individual color channels of the watermarked image
%watermarked = imread(‘C:UsersUserDownloadswatermarked_12.png’);
% Read the original host image
%host = imread(‘C:UsersUserDownloadshost 1.png’);
%wm = imread(‘C:UsersUserDownloadsw image 1.png’);

%EMBEDDING
% Resize watermark to match the host image size if necessary
wm = imresize(wm, size(host(:,:,1)));

% % Extract individual color channels of the host image
% h_red = host(:,:,1);
% h_green = host(:,:,2);
% h_blue = host(:,:,3);

% Extract individual color channels of the watermark image
wm_red = wm(:,:,1);
wm_green = wm(:,:,2);
wm_blue = wm(:,:,3);

w_red = watermarked(:,:,1);
w_green = watermarked(:,:,2);
w_blue = watermarked(:,:,3);

% Extract individual color channels of the host image
h_red = host(:,:,1);
h_green = host(:,:,2);
h_blue = host(:,:,3);

% Apply FWHT to the watermarked image and the host image
transformed_w_red = fwht2(w_red);
transformed_w_green = fwht2(w_green);
transformed_w_blue = fwht2(w_blue);

transformed_wm_red = fwht2(wm_red);
transformed_wm_green = fwht2(wm_green);
transformed_wm_blue = fwht2(wm_blue);

transformed_h_red = fwht2(h_red);
transformed_h_green = fwht2(h_green);
transformed_h_blue = fwht2(h_blue);

% Apply SVD to the FWHT-transformed watermarked image
[U_w_red, S_w_red, V_w_red] = svd(transformed_w_red);
[U_w_green, S_w_green, V_w_green] = svd(transformed_w_green);
[U_w_blue, S_w_blue, V_w_blue] = svd(transformed_w_blue);

% Apply SVD to the FWHT-transformed watermarked image
[U_wm_red, S_wm_red, V_wm_red] = svd(transformed_wm_red);
[U_wm_green, S_wm_green, V_wm_green] = svd(transformed_wm_green);
[U_wm_blue, S_wm_blue, V_wm_blue] = svd(transformed_wm_blue);

% Apply SVD to the FWHT-transformed host image
[~, S_h_red, ~] = svd(transformed_h_red);
[~, S_h_green, ~] = svd(transformed_h_green);
[~, S_h_blue, ~] = svd(transformed_h_blue);

% Subtract the singular values of the host image from the singular values of the watermarked image
S_ext_red = (S_w_red – S_h_red)/alpha;
S_ext_green = (S_w_green – S_h_green)/alpha;
S_ext_blue = (S_w_blue – S_h_blue)/alpha;

% Apply the inverse SVD to acquire the extracted watermark image
extracted_red = U_wm_red * S_ext_red * V_wm_red’;
extracted_green = U_wm_green * S_ext_green * V_wm_green’;
extracted_blue = U_wm_blue * S_ext_blue * V_wm_blue’;

% Apply the inverse FWHT
extracted_red = ifwht2(extracted_red);
extracted_green = ifwht2(extracted_green);
extracted_blue = ifwht2(extracted_blue);

% Combine the RGB blocks to form the final extracted watermark image
extracted_watermark = cat(3, extracted_red, extracted_green, extracted_blue);

%%
% figure;imshow(uint8(extracted_watermark));
extracted_watermark = uint8(255 * mat2gray( (extracted_watermark)));
% extracted_watermark = uint8(extracted_watermark);
figure;imshow(extracted_watermark);

% Resize the extracted watermark image to 128 x 128
extracted_watermark = imresize(extracted_watermark, [128 128]);

% Display the host image, watermark image, watermarked image, and extracted image side by side
figure; % Open a new figure window
subplot(1, 4, 1);
imshow(host, ‘InitialMagnification’, ‘fit’);
title(‘Host Image’);

subplot(1, 4, 2);
imshow(wm, ‘InitialMagnification’, ‘fit’);
title(‘Watermark Image’);

watermarked = uint8(watermarked);
subplot(1, 4, 3);
imshow(watermarked, ‘InitialMagnification’, ‘fit’);
title(‘Watermarked Image’);

subplot(1, 4, 4);
imshow(extracted_watermark, ‘InitialMagnification’, ‘fit’);
title(‘Extracted Watermark Image’);

% Function to apply 2D FWHT
function transformed_image = fwht2(image)
% Apply the 2D FWHT
transformed_image = fwht(fwht(double(image)).’).’;
end

% Function to apply 2D inverse FWHT
function image = ifwht2(transformed_image)
% Apply the 2D inverse FWHT
image = ifwht(ifwht(double(transformed_image)).’).’;
end my extracted watermark image is not similar with original watermark image where there exist gray background in the extracted watermark image and the color also did not same as original.

this is my code using FWHT and SVD algorithm
% Read the watermarked image
%watermarked = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimageswatermarked_12.png’);

% Read the original host image
host = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimageshost 1.png’);
wm = imread(‘C:Userswindow10DocumentsJAMILAHKELAS CS249FYPWatermarkingimagesw image 2.png’);

%EMBEDDING
% Resize watermark to match the host image size if necessary
wm = imresize(wm, size(host(:,:,1)));

% Extract individual color channels of the host image
h_red = host(:,:,1);
h_green = host(:,:,2);
h_blue = host(:,:,3);

% Extract individual color channels of the watermark image
wm_red = wm(:,:,1);
wm_green = wm(:,:,2);
wm_blue = wm(:,:,3);

% Apply FWHT to host image
transformed_h_red = fwht2(h_red);
transformed_h_green = fwht2(h_green);
transformed_h_blue = fwht2(h_blue);

% Apply FWHT to watermark image
transformed_wm_red = fwht2(wm_red);
transformed_wm_green = fwht2(wm_green);
transformed_wm_blue = fwht2(wm_blue);

% Apply SVD to FWHT-transformed host images
[U_h_red, S_h_red, V_h_red] = svd(transformed_h_red);
[U_h_green, S_h_green, V_h_green] = svd(transformed_h_green);
[U_h_blue, S_h_blue, V_h_blue] = svd(transformed_h_blue);

% Apply SVD to FWHT-transformed watermark images
[~, S_wm_red] = svd(transformed_wm_red);
[~, S_wm_green] = svd(transformed_wm_green);
[~, S_wm_blue] = svd(transformed_wm_blue);

% Modify the singular values of the host image by the singular values of the watermark image
alpha = 0.01; % Example scaling factor, adjust as needed
S_new_red = S_h_red + (alpha * S_wm_red);
S_new_green = S_h_green + (alpha * S_wm_green);
S_new_blue = S_h_blue + (alpha * S_wm_blue);

% Apply the inverse SVD to get the watermarked image channels
watermarked_red = U_h_red * S_new_red * V_h_red’;
watermarked_green = U_h_green * S_new_green * V_h_green’;
watermarked_blue = U_h_blue * S_new_blue * V_h_blue’;

% Apply inverse FWHT to each color channel of the watermarked image
watermarked_red = ifwht2(watermarked_red);
watermarked_green = ifwht2(watermarked_green);
watermarked_blue = ifwht2(watermarked_blue);

% Combine the RGB blocks to form the final watermarked color image
watermarked_color_image = cat(3, watermarked_red, watermarked_green, watermarked_blue);

% Scale the image data to the range [0, 1] and convert to uint8
watermarked = (255 * mat2gray(watermarked_color_image));
%watermarked = uint8(watermarked);

% Display the final watermarked color image in original size
%figure; % Open a new figure window
%imshow(watermarked, ‘InitialMagnification’, ‘fit’);
%title(‘Watermarked Image’);

% Open a dialog box to get the file path
%[filename, pathname] = uiputfile({‘*.png’;’*.jpg’;’*.bmp’;’*.tiff’}, ‘Save as’);

%if isequal(filename,0) || isequal(pathname,0)
% disp(‘User pressed cancel’)
%else
% Create the full file path
% full_filename = fullfile(pathname, filename);

% Save the watermarked color image to the chosen location
% imwrite(watermarked_color_image, full_filename);

% Display a message to indicate the image has been saved
% disp([‘Watermarked image saved as ‘, full_filename]);
%end

%END EMBED

%%
%EXTRACTION PROCESS
% Extract individual color channels of the watermarked image
%watermarked = imread(‘C:UsersUserDownloadswatermarked_12.png’);
% Read the original host image
%host = imread(‘C:UsersUserDownloadshost 1.png’);
%wm = imread(‘C:UsersUserDownloadsw image 1.png’);

%EMBEDDING
% Resize watermark to match the host image size if necessary
wm = imresize(wm, size(host(:,:,1)));

% % Extract individual color channels of the host image
% h_red = host(:,:,1);
% h_green = host(:,:,2);
% h_blue = host(:,:,3);

% Extract individual color channels of the watermark image
wm_red = wm(:,:,1);
wm_green = wm(:,:,2);
wm_blue = wm(:,:,3);

w_red = watermarked(:,:,1);
w_green = watermarked(:,:,2);
w_blue = watermarked(:,:,3);

% Extract individual color channels of the host image
h_red = host(:,:,1);
h_green = host(:,:,2);
h_blue = host(:,:,3);

% Apply FWHT to the watermarked image and the host image
transformed_w_red = fwht2(w_red);
transformed_w_green = fwht2(w_green);
transformed_w_blue = fwht2(w_blue);

transformed_wm_red = fwht2(wm_red);
transformed_wm_green = fwht2(wm_green);
transformed_wm_blue = fwht2(wm_blue);

transformed_h_red = fwht2(h_red);
transformed_h_green = fwht2(h_green);
transformed_h_blue = fwht2(h_blue);

% Apply SVD to the FWHT-transformed watermarked image
[U_w_red, S_w_red, V_w_red] = svd(transformed_w_red);
[U_w_green, S_w_green, V_w_green] = svd(transformed_w_green);
[U_w_blue, S_w_blue, V_w_blue] = svd(transformed_w_blue);

% Apply SVD to the FWHT-transformed watermarked image
[U_wm_red, S_wm_red, V_wm_red] = svd(transformed_wm_red);
[U_wm_green, S_wm_green, V_wm_green] = svd(transformed_wm_green);
[U_wm_blue, S_wm_blue, V_wm_blue] = svd(transformed_wm_blue);

% Apply SVD to the FWHT-transformed host image
[~, S_h_red, ~] = svd(transformed_h_red);
[~, S_h_green, ~] = svd(transformed_h_green);
[~, S_h_blue, ~] = svd(transformed_h_blue);

% Subtract the singular values of the host image from the singular values of the watermarked image
S_ext_red = (S_w_red – S_h_red)/alpha;
S_ext_green = (S_w_green – S_h_green)/alpha;
S_ext_blue = (S_w_blue – S_h_blue)/alpha;

% Apply the inverse SVD to acquire the extracted watermark image
extracted_red = U_wm_red * S_ext_red * V_wm_red’;
extracted_green = U_wm_green * S_ext_green * V_wm_green’;
extracted_blue = U_wm_blue * S_ext_blue * V_wm_blue’;

% Apply the inverse FWHT
extracted_red = ifwht2(extracted_red);
extracted_green = ifwht2(extracted_green);
extracted_blue = ifwht2(extracted_blue);

% Combine the RGB blocks to form the final extracted watermark image
extracted_watermark = cat(3, extracted_red, extracted_green, extracted_blue);

%%
% figure;imshow(uint8(extracted_watermark));
extracted_watermark = uint8(255 * mat2gray( (extracted_watermark)));
% extracted_watermark = uint8(extracted_watermark);
figure;imshow(extracted_watermark);

% Resize the extracted watermark image to 128 x 128
extracted_watermark = imresize(extracted_watermark, [128 128]);

% Display the host image, watermark image, watermarked image, and extracted image side by side
figure; % Open a new figure window
subplot(1, 4, 1);
imshow(host, ‘InitialMagnification’, ‘fit’);
title(‘Host Image’);

subplot(1, 4, 2);
imshow(wm, ‘InitialMagnification’, ‘fit’);
title(‘Watermark Image’);

watermarked = uint8(watermarked);
subplot(1, 4, 3);
imshow(watermarked, ‘InitialMagnification’, ‘fit’);
title(‘Watermarked Image’);

subplot(1, 4, 4);
imshow(extracted_watermark, ‘InitialMagnification’, ‘fit’);
title(‘Extracted Watermark Image’);

% Function to apply 2D FWHT
function transformed_image = fwht2(image)
% Apply the 2D FWHT
transformed_image = fwht(fwht(double(image)).’).’;
end

% Function to apply 2D inverse FWHT
function image = ifwht2(transformed_image)
% Apply the 2D inverse FWHT
image = ifwht(ifwht(double(transformed_image)).’).’;
end #imagewatermarking, #imageprocessing, #mathematics, #fwht, #svd MATLAB Answers — New Questions

​

I have two 5*5 matrices. I want to use a plot to show that they are close element-wise.
What plot is suitable?
I think the plot should be transparent.
It is of course doable if we reshape the matrices into vectors, and use a 2d plot. But that is not intuitive.I have two 5*5 matrices. I want to use a plot to show that they are close element-wise.
What plot is suitable?
I think the plot should be transparent.
It is of course doable if we reshape the matrices into vectors, and use a 2d plot. But that is not intuitive. I have two 5*5 matrices. I want to use a plot to show that they are close element-wise.
What plot is suitable?
I think the plot should be transparent.
It is of course doable if we reshape the matrices into vectors, and use a 2d plot. But that is not intuitive. plotting, surface MATLAB Answers — New Questions

​

This is the code to perform LOAM for the self driving car moving around the parking lot, now how to add kalman filter in this code given below:
% Load reference path % 60 vehicles are parked
data = load("parkingLotReferenceData.mat");

% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;

% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;

% Display the reference trajectory and the parked vehicle locations
sceneName = "LargeParkingLot";
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2,DisplayName="Reference Path");
scatter(parkedPoses(:,1),parkedPoses(:,2),[],"filled",DisplayName="Parked Vehicles");
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500]; % Resize figure
title("Large Parking Lot")
legend

modelName = ‘GenerateLidarDataOfParkingLot’;
open_system(modelName)
snapnow
helperAddParkedVehicles(modelName,parkedPoses)

close(hScene)

if ~ispc
error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end

% Run simulation
simOut = sim(modelName);

close_system(modelName,0)

ptCloudArr = helperGetPointClouds(simOut);
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);

ptCloud = ptCloudArr(1);
nextPtCloud = ptCloudArr(2);
gridStep = 0.5;
tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);
disp(tform)

egoRadius = 3.5;
cylinderRadius = 50;
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
selectedIdx = findPointsInCylinder(nextPtCloud,[egoRadius cylinderRadius]);
nextPtCloud = select(nextPtCloud,selectedIdx,OutputSize="full");

figure
hold on
title("Cylindrical Neighborhood")
pcshow(ptCloud)
view(2)

maxPlanarSurfacePoints = 8;
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);

figure
hold on
title("LOAM Points")
show(points,MarkerSize=12)

[~,rmseValue] = pcregisterloam(points,nextPoints);
disp([‘RMSE: ‘ num2str(rmseValue)])

points = downsampleLessPlanar(points,gridStep);

figure
hold on
title(‘LOAM Points After Downsampling the Less Planar Surface Points’)
show(points,’MarkerSize’,12)

absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetOdometry = pcviewset;

viewId = 1;
vSetOdometry = addView(vSetOdometry,viewId,absPose);

% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];

numSkip = 5;
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);

% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");

% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);

% Update the absolute pose and store it in the view set
absPose = rigidtform3d(absPose.A * relPose.A);
vSetOdometry = addView(vSetOdometry,viewId,absPose);

% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8);
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Odometry")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end

absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetMapping = pcviewset;

ptCloud = ptCloudArr(1);
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
points = detectLOAMFeatures(ptCloud,’MaxPlanarSurfacePoints’,maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

viewId = 1;
vSetMapping = addView(vSetMapping,viewId,absPose);

voxelSize = 0.1;
loamMap = pcmaploam(voxelSize);
addPoints(loamMap,points,absPose);

% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];

for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPtCloud = ptCloud;
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);

% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");

% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,MatchingMethod="one-to-one",InitialTransform=relPose);

% Find the refined absolute pose that aligns the points to the map
absPose = findPose(loamMap,points,relPose);

% Store the refined absolute pose in the view set
vSetMapping = addView(vSetMapping,viewId,absPose);

% Add the new points to the map
addPoints(loamMap,points,absPose);

% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8)
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Mapping")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end

% Get the ground truth trajectory
groundTruthTrajectory = vertcat(groundTruthPosesLidar.Translation);

% Get the estimated trajectories
odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);
mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);

% Plot the trajectories
figure
plot3(groundTruthTrajectory(:,1),groundTruthTrajectory(:,2),groundTruthTrajectory(:,3),LineWidth=2,DisplayName="Ground Truth")
hold on
plot3(odometryPositions(:,1),odometryPositions(:,2),odometryPositions(:,3),LineWidth=2,DisplayName="Odometry")
plot3(mappingPositions(:,1),mappingPositions(:,2),mappingPositions(:,3),LineWidth=2,DisplayName="Odometry and Mapping")
view(2)

legend
title("Compare Trajectories")

% Select the poses to compare
selectedGroundTruth = groundTruthTrajectory(1:numSkip:end,:);

% Compute the RMSE
rmseOdometry = rmse(selectedGroundTruth,odometryPositions,"all");
rmseWithMapping = rmse(selectedGroundTruth,mappingPositions,"all");
disp([‘RMSE of the trajectory estimated with Odometry: ‘ num2str(rmseOdometry)])

disp([‘RMSE of the trajectory estimated with Odometry and Mapping: ‘ num2str(rmseWithMapping)])

function ptCloudArr = helperGetPointClouds(simOut)

% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;

% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 1:size(ptCloudData,4)
ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n)); %#ok<AGROW>
end

end
function gTruth = helperGetLidarGroundTruth(simOut)

numFrames = size(simOut.lidarLocation.time,1);
gTruth = repmat(rigidtform3d,numFrames-1,1);

for i = 1:numFrames
gTruth(i).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
% Ignore the roll and the pitch rotations since the ground is flat
yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
gTruth(i).R = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
end
endThis is the code to perform LOAM for the self driving car moving around the parking lot, now how to add kalman filter in this code given below:
% Load reference path % 60 vehicles are parked
data = load("parkingLotReferenceData.mat");

% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;

% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;

% Display the reference trajectory and the parked vehicle locations
sceneName = "LargeParkingLot";
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2,DisplayName="Reference Path");
scatter(parkedPoses(:,1),parkedPoses(:,2),[],"filled",DisplayName="Parked Vehicles");
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500]; % Resize figure
title("Large Parking Lot")
legend

modelName = ‘GenerateLidarDataOfParkingLot’;
open_system(modelName)
snapnow
helperAddParkedVehicles(modelName,parkedPoses)

close(hScene)

if ~ispc
error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end

% Run simulation
simOut = sim(modelName);

close_system(modelName,0)

ptCloudArr = helperGetPointClouds(simOut);
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);

ptCloud = ptCloudArr(1);
nextPtCloud = ptCloudArr(2);
gridStep = 0.5;
tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);
disp(tform)

egoRadius = 3.5;
cylinderRadius = 50;
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
selectedIdx = findPointsInCylinder(nextPtCloud,[egoRadius cylinderRadius]);
nextPtCloud = select(nextPtCloud,selectedIdx,OutputSize="full");

figure
hold on
title("Cylindrical Neighborhood")
pcshow(ptCloud)
view(2)

maxPlanarSurfacePoints = 8;
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);

figure
hold on
title("LOAM Points")
show(points,MarkerSize=12)

[~,rmseValue] = pcregisterloam(points,nextPoints);
disp([‘RMSE: ‘ num2str(rmseValue)])

points = downsampleLessPlanar(points,gridStep);

figure
hold on
title(‘LOAM Points After Downsampling the Less Planar Surface Points’)
show(points,’MarkerSize’,12)

absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetOdometry = pcviewset;

viewId = 1;
vSetOdometry = addView(vSetOdometry,viewId,absPose);

% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];

numSkip = 5;
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);

% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");

% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);

% Update the absolute pose and store it in the view set
absPose = rigidtform3d(absPose.A * relPose.A);
vSetOdometry = addView(vSetOdometry,viewId,absPose);

% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8);
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Odometry")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end

absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetMapping = pcviewset;

ptCloud = ptCloudArr(1);
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
points = detectLOAMFeatures(ptCloud,’MaxPlanarSurfacePoints’,maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

viewId = 1;
vSetMapping = addView(vSetMapping,viewId,absPose);

voxelSize = 0.1;
loamMap = pcmaploam(voxelSize);
addPoints(loamMap,points,absPose);

% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];

for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPtCloud = ptCloud;
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);

% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");

% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,MatchingMethod="one-to-one",InitialTransform=relPose);

% Find the refined absolute pose that aligns the points to the map
absPose = findPose(loamMap,points,relPose);

% Store the refined absolute pose in the view set
vSetMapping = addView(vSetMapping,viewId,absPose);

% Add the new points to the map
addPoints(loamMap,points,absPose);

% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8)
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Mapping")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end

% Get the ground truth trajectory
groundTruthTrajectory = vertcat(groundTruthPosesLidar.Translation);

% Get the estimated trajectories
odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);
mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);

% Plot the trajectories
figure
plot3(groundTruthTrajectory(:,1),groundTruthTrajectory(:,2),groundTruthTrajectory(:,3),LineWidth=2,DisplayName="Ground Truth")
hold on
plot3(odometryPositions(:,1),odometryPositions(:,2),odometryPositions(:,3),LineWidth=2,DisplayName="Odometry")
plot3(mappingPositions(:,1),mappingPositions(:,2),mappingPositions(:,3),LineWidth=2,DisplayName="Odometry and Mapping")
view(2)

legend
title("Compare Trajectories")

% Select the poses to compare
selectedGroundTruth = groundTruthTrajectory(1:numSkip:end,:);

% Compute the RMSE
rmseOdometry = rmse(selectedGroundTruth,odometryPositions,"all");
rmseWithMapping = rmse(selectedGroundTruth,mappingPositions,"all");
disp([‘RMSE of the trajectory estimated with Odometry: ‘ num2str(rmseOdometry)])

disp([‘RMSE of the trajectory estimated with Odometry and Mapping: ‘ num2str(rmseWithMapping)])

function ptCloudArr = helperGetPointClouds(simOut)

% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;

% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 1:size(ptCloudData,4)
ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n)); %#ok<AGROW>
end

end
function gTruth = helperGetLidarGroundTruth(simOut)

numFrames = size(simOut.lidarLocation.time,1);
gTruth = repmat(rigidtform3d,numFrames-1,1);

for i = 1:numFrames
gTruth(i).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
% Ignore the roll and the pitch rotations since the ground is flat
yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
gTruth(i).R = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
end
end This is the code to perform LOAM for the self driving car moving around the parking lot, now how to add kalman filter in this code given below:
% Load reference path % 60 vehicles are parked
data = load("parkingLotReferenceData.mat");

% Set reference trajectory of the ego vehicle
refPosesX = data.refPosesX;
refPosesY = data.refPosesY;
refPosesT = data.refPosesT;

% Set poses of the parked vehicles
parkedPoses = data.parkedPoses;

% Display the reference trajectory and the parked vehicle locations
sceneName = "LargeParkingLot";
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2,DisplayName="Reference Path");
scatter(parkedPoses(:,1),parkedPoses(:,2),[],"filled",DisplayName="Parked Vehicles");
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500]; % Resize figure
title("Large Parking Lot")
legend

modelName = ‘GenerateLidarDataOfParkingLot’;
open_system(modelName)
snapnow
helperAddParkedVehicles(modelName,parkedPoses)

close(hScene)

if ~ispc
error("Unreal Engine Simulation is supported only on Microsoft" + char(174) + " Windows" + char(174) + ".");
end

% Run simulation
simOut = sim(modelName);

close_system(modelName,0)

ptCloudArr = helperGetPointClouds(simOut);
groundTruthPosesLidar = helperGetLidarGroundTruth(simOut);

ptCloud = ptCloudArr(1);
nextPtCloud = ptCloudArr(2);
gridStep = 0.5;
tform = pcregisterloam(ptCloud,nextPtCloud,gridStep);
disp(tform)

egoRadius = 3.5;
cylinderRadius = 50;
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
selectedIdx = findPointsInCylinder(nextPtCloud,[egoRadius cylinderRadius]);
nextPtCloud = select(nextPtCloud,selectedIdx,OutputSize="full");

figure
hold on
title("Cylindrical Neighborhood")
pcshow(ptCloud)
view(2)

maxPlanarSurfacePoints = 8;
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
nextPoints = detectLOAMFeatures(nextPtCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);

figure
hold on
title("LOAM Points")
show(points,MarkerSize=12)

[~,rmseValue] = pcregisterloam(points,nextPoints);
disp([‘RMSE: ‘ num2str(rmseValue)])

points = downsampleLessPlanar(points,gridStep);

figure
hold on
title(‘LOAM Points After Downsampling the Less Planar Surface Points’)
show(points,’MarkerSize’,12)

absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetOdometry = pcviewset;

viewId = 1;
vSetOdometry = addView(vSetOdometry,viewId,absPose);

% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];

numSkip = 5;
for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);

% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");

% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,InitialTransform=relPose);

% Update the absolute pose and store it in the view set
absPose = rigidtform3d(absPose.A * relPose.A);
vSetOdometry = addView(vSetOdometry,viewId,absPose);

% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8);
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Odometry")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end

absPose = groundTruthPosesLidar(1);
relPose = rigidtform3d;
vSetMapping = pcviewset;

ptCloud = ptCloudArr(1);
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");
points = detectLOAMFeatures(ptCloud,’MaxPlanarSurfacePoints’,maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

viewId = 1;
vSetMapping = addView(vSetMapping,viewId,absPose);

voxelSize = 0.1;
loamMap = pcmaploam(voxelSize);
addPoints(loamMap,points,absPose);

% Display the parking lot scene with the reference trajectory
hScene = figure(Name="Large Parking Lot",NumberTitle="off");
helperShowSceneImage(sceneName);
hold on
plot(refPosesX(:,2),refPosesY(:,2),LineWidth=2)
xlim([-60 40])
ylim([10 60])
hScene.Position = [100 100 1000 500];

for k = (numSkip+1):numSkip:numel(ptCloudArr)
prevPtCloud = ptCloud;
prevPoints = points;
viewId = viewId + 1;
ptCloud = ptCloudArr(k);

% Select a cylindrical neighborhood of interest.
selectedIdx = findPointsInCylinder(ptCloud,[egoRadius cylinderRadius]);
ptCloud = select(ptCloud,selectedIdx,OutputSize="full");

% Detect LOAM points and downsample the less planar surface points
points = detectLOAMFeatures(ptCloud,MaxPlanarSurfacePoints=maxPlanarSurfacePoints);
points = downsampleLessPlanar(points,gridStep);

% Register the points using the previous relative pose as an initial
% transformation
relPose = pcregisterloam(points,prevPoints,MatchingMethod="one-to-one",InitialTransform=relPose);

% Find the refined absolute pose that aligns the points to the map
absPose = findPose(loamMap,points,relPose);

% Store the refined absolute pose in the view set
vSetMapping = addView(vSetMapping,viewId,absPose);

% Add the new points to the map
addPoints(loamMap,points,absPose);

% Visualize the absolute pose in the parking lot scene
plot(absPose.Translation(1),absPose.Translation(2),Color="r",Marker=".",MarkerSize=8)
xlim([-60 40])
ylim([10 60])
title("Build a Map Using Lidar Mapping")
legend(["Ground Truth","Estimated Position"])
pause(0.001)
view(2)
end

% Get the ground truth trajectory
groundTruthTrajectory = vertcat(groundTruthPosesLidar.Translation);

% Get the estimated trajectories
odometryPositions = vertcat(vSetOdometry.Views.AbsolutePose.Translation);
mappingPositions = vertcat(vSetMapping.Views.AbsolutePose.Translation);

% Plot the trajectories
figure
plot3(groundTruthTrajectory(:,1),groundTruthTrajectory(:,2),groundTruthTrajectory(:,3),LineWidth=2,DisplayName="Ground Truth")
hold on
plot3(odometryPositions(:,1),odometryPositions(:,2),odometryPositions(:,3),LineWidth=2,DisplayName="Odometry")
plot3(mappingPositions(:,1),mappingPositions(:,2),mappingPositions(:,3),LineWidth=2,DisplayName="Odometry and Mapping")
view(2)

legend
title("Compare Trajectories")

% Select the poses to compare
selectedGroundTruth = groundTruthTrajectory(1:numSkip:end,:);

% Compute the RMSE
rmseOdometry = rmse(selectedGroundTruth,odometryPositions,"all");
rmseWithMapping = rmse(selectedGroundTruth,mappingPositions,"all");
disp([‘RMSE of the trajectory estimated with Odometry: ‘ num2str(rmseOdometry)])

disp([‘RMSE of the trajectory estimated with Odometry and Mapping: ‘ num2str(rmseWithMapping)])

function ptCloudArr = helperGetPointClouds(simOut)

% Extract signal
ptCloudData = simOut.ptCloudData.signals.values;

% Create a pointCloud array
ptCloudArr = pointCloud(ptCloudData(:,:,:,2)); % Ignore first frame
for n = 1:size(ptCloudData,4)
ptCloudArr(end+1) = pointCloud(ptCloudData(:,:,:,n)); %#ok<AGROW>
end

end
function gTruth = helperGetLidarGroundTruth(simOut)

numFrames = size(simOut.lidarLocation.time,1);
gTruth = repmat(rigidtform3d,numFrames-1,1);

for i = 1:numFrames
gTruth(i).Translation = squeeze(simOut.lidarLocation.signals.values(:,:,i));
% Ignore the roll and the pitch rotations since the ground is flat
yaw = double(simOut.lidarOrientation.signals.values(:,3,i));
gTruth(i).R = [cos(yaw) -sin(yaw) 0;
sin(yaw) cos(yaw) 0;
0 0 1];
end
end slam, loam, autonomous vehicle, kalman filter MATLAB Answers — New Questions

​

When i use matlab 2021a and unreal engine 4.23 for simulation,in unreal editor model ,the question is appear.
The details of the error are as follows:
MATLAB System block ‘SCRReferenceApplication/Simulation 3D Vehicle with Ground Following/Simulation 3D Vehicle’ error occurred when invoking ‘stepImpl’ method of ‘Simulation3DVehicleTerrainFb’. The error was thrown from ‘
‘D:Program FilesMATLABR2021atoolboxsharedsim3dblkssim3dblksSimulation3DVehicleTerrainFb.p’ at line 0
‘D:Program FilesMATLABR2021atoolboxsharedsim3dblkssim3dblksSimulation3DVehicleTerrainFb.p’ at line 0′.
The 3D vehicle no longer detects the ground. Review scene limits or initial position.When i use matlab 2021a and unreal engine 4.23 for simulation,in unreal editor model ,the question is appear.
The details of the error are as follows:
MATLAB System block ‘SCRReferenceApplication/Simulation 3D Vehicle with Ground Following/Simulation 3D Vehicle’ error occurred when invoking ‘stepImpl’ method of ‘Simulation3DVehicleTerrainFb’. The error was thrown from ‘
‘D:Program FilesMATLABR2021atoolboxsharedsim3dblkssim3dblksSimulation3DVehicleTerrainFb.p’ at line 0
‘D:Program FilesMATLABR2021atoolboxsharedsim3dblkssim3dblksSimulation3DVehicleTerrainFb.p’ at line 0′.
The 3D vehicle no longer detects the ground. Review scene limits or initial position. When i use matlab 2021a and unreal engine 4.23 for simulation,in unreal editor model ,the question is appear.
The details of the error are as follows:
MATLAB System block ‘SCRReferenceApplication/Simulation 3D Vehicle with Ground Following/Simulation 3D Vehicle’ error occurred when invoking ‘stepImpl’ method of ‘Simulation3DVehicleTerrainFb’. The error was thrown from ‘
‘D:Program FilesMATLABR2021atoolboxsharedsim3dblkssim3dblksSimulation3DVehicleTerrainFb.p’ at line 0
‘D:Program FilesMATLABR2021atoolboxsharedsim3dblkssim3dblksSimulation3DVehicleTerrainFb.p’ at line 0′.
The 3D vehicle no longer detects the ground. Review scene limits or initial position. unreal engine, simulink MATLAB Answers — New Questions

​