About SLAM initial Pose data

I am the following site

https://jp.mathworks.com/help/vision/ug/stereo-visual-simultaneous-localization-mapping.html

I would like to run SLAM on my stereo camera as a reference. I was able to record the left and right video with my stereo camera to inspect the first pair of images and get "stereo Params" from exporting the parameters of the stereo camera calibration app.

However, in the example of the above site, the initial camera pose is loaded from "'initialPose.mat'" and used as the function "initialPose". What should I prepare to do these on my camera device?

Answer 1

[Qu Cao]

The initial pose data is provided by the dataset. It's used to convert the 3-D reconstruction into the world coordinate system. If you cannot measure the inital pose of your camera, you can assume that the camera is at the origin with zero rotations:

initialPose = rigid3d(eye(3), [0 0 0])

Answer 2

[圭介 川邉]

As described above

initialPose = rigid3d (eye (3), [0 0 0])

I changed the code and ran it, but I get the following error:

Error:  * 
The dimensions for matrix multiplication are incorrect. The number of columns in the first matrix matches the number of rows in the second matrix
Please make sure that. Use'. *'To perform elemental multiplication.

Error: helperReconstructFromStereo (line 26)
xyzPoints  = xyzPoints * currPose.Rotation + currPose.Translation;

Error: stereoslam (line 37)
[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft,
currIRight ...

I'm using my code and properties like this, but I'm not sure why this error occurs so please let me know.

code:

Leftreader = VideoReader('leftframe.avi')
Rightreader = VideoReader('rightframe.avi')


currFrameIdx =1;
currILeft = read(Leftreader,currFrameIdx);
currIRight = read(Rightreader,currFrameIdx);

imshowpair(currILeft,currIRight, 'montage')

rng(0);

initialPoseData = load('initialPose.mat');
initialPose     = initialPoseData.initialPose;

focalLength     = [387.777 387.777];     
principalPoint  = [257.446 197.718];     
baseline        = 0.239965;              
intrinsicMatrix = [focalLength(1), 0, 0; ...
    0, focalLength(2), 0; ...
    principalPoint(1), principalPoint(2), 1];
imageSize       = size(currILeft,[1,2]);
cameraParam     = cameraParameters('IntrinsicMatrix', intrinsicMatrix, 'ImageSize', imageSize);
intrinsics      = cameraParam.Intrinsics;


[currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams, 'OutputView','full');

scaleFactor = 1.2;
numLevels   = 8;
[currFeaturesLeft,  currPointsLeft]   = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels)
[currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels)

maxDisparity = 48;  
[xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight   ...
    currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, stereoParams, initialPose, maxDisparity);

properties:

stereoParams =

   properties of stereoParameters:

    Parameters of Two Cameras
         CameraParameters1: [1x1 cameraParameters]
         CameraParameters2: [1x1 cameraParameters]

    Inter-camera Geometry
         RotationOfCamera2: [3x3 double]
      TranslationOfCamera2: [-138.2614 0.1132 -1.9609]
         FundamentalMatrix: [3x3 double]
           EssentialMatrix: [3x3 double]

    Accuracy of Estimation
     MeanReprojectionError: 0.7208

    Calibration Settings
               NumPatterns: 13
               WorldPoints: [54x2 double]
                WorldUnits:'millimeters'


estimationErrors =

   properties of stereoCalibrationErrors:

       Camera1IntrinsicsErrors: [1x1 intrinsicsEstimationErrors]
       Camera1ExtrinsicsErrors: [1x1 extrinsicsEstimationErrors]
       Camera2IntrinsicsErrors: [1x1 intrinsicsEstimationErrors]
        RotationOfCamera2Error: [0.0012 0.0021 2.9278e-04]
     TranslationOfCamera2Error: [0.1338 0.1060 0.4320]

Leftreader =

  VideoReader Properties:

   General properties:
            Name:'leftframe.avi'
            Path:'C: \ Users \ 18044 \ Documents \ gitHub \ ZED SDK \ zed-matlab \ matlab'
        Duration: 5.6667
     CurrentTime: 0
       NumFrames: 170

   Video Properties:
           Width: 1920
          Height: 1080
       FrameRate: 30
    BitsPerPixel: 24
     VideoFormat:'RGB24'


Rightreader =

  VideoReader Properties:

   General properties:
            Name:'rightframe.avi'
            Path:'C: \ Users \ 18044 \ Documents \ gitHub \ ZED SDK \ zed-matlab \ matlab'
        Duration: 5.6667
     CurrentTime: 0
       NumFrames: 170

   Video Properties:
           Width: 1920
          Height: 1080
       FrameRate: 30
    BitsPerPixel: 24
     VideoFormat:'RGB24'


currFeaturesLeft =

  properties of binaryFeatures:

       Features: [1000x32 uint8]
        NumBits: 256
    NumFeatures: 1000


currPointsLeft =

  1000x1 ORBPoints Array Properties:

       Location: [1000x2 single]
         Metric: [1000x1 single]
          Count: 1000
          Scale: [1000x1 single]
    Orientation: [1000x1 single]


currFeaturesRight =

  properties of binaryFeatures:

       Features: [1000x32 uint8]
        NumBits: 256
    NumFeatures: 1000


currPointsRight =

  1000x1 ORBPoints Array Properties:

       Location: [1000x2 single]
         Metric: [1000x1 single]
          Count: 1000
          Scale: [1000x1 single]
    Orientation: [1000x1 single]

[Qu Cao]

I'd do some debugging here by checking the values of xyzPoints.

[圭介 川邉]

How do I check the value of xyzPoint?

I actually ran "helperFindValidFeaturePairs", but the following error occurs as above.

Error:  * 
The dimensions for matrix multiplication are incorrect. The number of columns in the first matrix matches the number of rows in the second matrix
Please make sure that. Use'. *'To perform elemental multiplication.

Error: helperReconstructFromStereo (line 26)
xyzPoints  = xyzPoints * currPose.Rotation + currPose.Translation

[圭介 川邉]

When I examined the dimensions of "currPose.Rotation" and "currPose.Translation" to examine the dimension of "xyzPoint", "currPose.Rotation" is a 3x3 matrix and "currPose.Translation" is a 1x3 matrix. Therefore, it was found that the dimension of "xyzPoint" needs to be 1x3 in order to establish the given formula.

Therefore, the error could be eliminated by changing the previously given "xyzPoint = []" to "xyzPoint = zeros (1,3)". Thank you for your reply.

[Sheraz Shamim]

I am having a same error. xyzPoints is empty and got same error.

[Fardad]

And I cann't fint the "helperReconstructFromStereo" in the folder of "StereoVisualSimultaneousLocalizationAndMappingExample"!