Matlab Wrapper function to extract Vectors from a Values object

[gchenfc]

@mattking-smith and I added a function in utilities.h, which is primarily used for the matlab wrapper, to extract all the vectors from a Values object.

Similar to extractPose3 and extractPose2, this just makes a lot of matlab code significantly faster than for-looping.

[dellaert]

I think an API that makes more sense is to give a character, say 'x`' that will be used to select all values with a particular Symbol, e.g. x4, x5, x6, and that will assume that they are all of the same dimension. Use case is then:

result = extractVectors(values, 'x')

and if there are 200 such vectors, and they are 5D, the result is a 200x5 Matrix (not 5x200, we want to be consistent with other methods in this file). You can thrown an exception if those values have different dimensions.

[dellaert]

Also, merge in develop and resolve conflicts (carefully!)

[dellaert]

I love that we are reviving this PR. Just change the docs etc to read mxn not ‘NxM’, because:

• matrices are typically mxn
• N is a better constant for dimensionality IMHO

[mattking-smith]

I love that we are reviving this PR. Just change the docs etc to read mxn not ‘NxM’, because:

• matrices are typically mxn
• N is a better constant for dimensionality IMHO

I am going to verify this update on my computer now.

[mattking-smith]

@gchenfc I just switched to this branch, compiled and tried running the testUtilities.m and immediately ran into:

>> testUtilities
Error using gtsam_wrapper
Exception from gtsam:
eliminateMultifrontal expects 1 arguments, not 0


Error in gtsam.utilities.extractVector (line 3)
        varargout{1} = gtsam_wrapper(2379, varargin{:});

Error in testUtilities (line 54)
actual = utilities.extractVector(values);

I am guessing that this may be related to #954 (comment) and to your current wrapper issue.

[gchenfc]

Oops I had a typo in unit test, can you try again with updated .m file?

No clue why EliminateMultifrontal is involved - but if it's still having issues after updated .m file we can chat to resolve.

[mattking-smith]

Oops I had a typo in unit test, can you try again with updated .m file?

I looked at the file changes you made and implemented them on my system so I could quickly test your fix without recompiling and I found the following error:

>> actual = utilities.extractVectors(values);

CHECK('extractVector', all(actual == expected, 'all'));
Error using gtsam.utilities.extractVectors (line 5)
Arguments do not match any overload of function extractVectors

Looking at the output of the wrapper definition I see the following:

function varargout = extractVectors(varargin)
      if length(varargin) == 2 && isa(varargin{1},'gtsam.Values') && isa(varargin{2},'char')
        varargout{1} = gtsam_wrapper(89, varargin{:});
      else
        error('Arguments do not match any overload of function extractVectors');
      end

So I tried the following tests:

>> actual = utilities.extractVectors(values,'7')

actual =

     []

>> actual = utilities.extractVectors(values,'x1')
Error using gtsam_wrapper
Exception from gtsam:
The error message identifier is invalid.


Error in gtsam.utilities.extractVectors (line 3)
        varargout{1} = gtsam_wrapper(89, varargin{:});
 
>> actual = utilities.extractVectors(values,'x')

actual =

     []

Am I inputting the symbol correctly?

[gchenfc]

Omg I'm so careless - it should be the last one:

utilities.extractVectors(values,'x')

Actually the test should be like this:

% test extractVectors
values = Values();
values.insert(symbol('x', 0), (1:6)');
values.insert(symbol('x', 1), (7:12)');
values.insert(symbol('x', 2), (13:18)');
values.insert(symbol('x', 7), Pose3());
actual = utilities.extractVectors(values, 'x');
expected = reshape(1:18, 6, 3)';
CHECK('extractVectors', all(actual == expected, 'all'));

Sorry for the confusion
Since pushing causes CI to take a long time and slows down everyone else, could you try this before I push these changes?

[mattking-smith]

% test extractVectors
values = Values();
values.insert(symbol('x', 0), (1:6)');
values.insert(symbol('x', 1), (7:12)');
values.insert(symbol('x', 2), (13:18)');
values.insert(symbol('x', 7), Pose3());
actual = utilities.extractVectors(values, 'x');
expected = reshape(1:18, 6, 3)';
CHECK('extractVectors', all(actual == expected, 'all'));

Running this snippet of code resulted in:

>> % test extractVectors
values = Values();
values.insert(symbol('x', 0), (1:6)');
values.insert(symbol('x', 1), (7:12)');
values.insert(symbol('x', 2), (13:18)');
values.insert(symbol('x', 7), Pose3());
actual = utilities.extractVectors(values, 'x')
expected = reshape(1:18, 6, 3)';
CHECK('extractVectors', all(actual == expected, 'all'));

actual =

     1     2     3     4     5     6
     7     8     9    10    11    12
    13    14    15    16    17    18

This is the correct format and executed flawlessly.

[mattking-smith]

Sorry for the confusion Since pushing causes CI to take a long time and slows down everyone else, could you try this before I push these changes?

Can you push these changes so I can pull and then just test the .m files real quick?

[gchenfc]

pushed - thanks so much for testing for me!!!

[mattking-smith]

pushed - thanks so much for testing for me!!!

I have pulled and verified this fix.
>> testUtilities

Thank you for doing this @gchenfc

[mattking-smith]

Since pushing causes CI to take a long time and slows down everyone else, could you try this before I push these changes?

Once the CI tests are successfully completed, we should be able to wrap up this PR @dellaert. Nice work @gchenfc.