bug about matlab toolbox

[haifadai]

Hi! when running ' IMUKittiExampleGPS.m', I met with following error:

Undefined variable "gtsam" or class "gtsam.ImuFactorPreintegratedMeasurements".

Error in IMUKittiExampleGPS (line 82)
    currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...

I find that the examples in toolbox of version 4.0.0 are entirely same with the one of version 3.2.1. I wonder whether the examples be updated with the new version. Can you help me solve that issue?

[Frank Dellaert]

I don't regularly use matlab anymore, someone else can maybe help? Is it feasible to submit pull request?

[impyer]

I also get a lot of errors and crash with the examples, any fix in the near future?

[Frank Dellaert]

I understand this is not the answer you want, but I think we’re relying on people like you to assess the problem, try to fix it, and submit a PR. The reason is not many of the core developers use Marian anymore....

[haifadai]

Thank you! I will try it.

发自网易邮箱大师

[Frank Dellaert]

Any luck?

[haifadai]

I found that the class "gtsam.ImuFactorPreintegratedMeasurements" was replaced by "gtsam.ImuFactorPreintegratedMeasurements " in version of 4.0.0 and it should be initilized as following:

preintegrationParams = PreintegrationParams(g);
preintegrationParams.setAccelerometerCovariance(IMU_metadata.AccelerometerSigma.^2 * eye(3)) ;
%PreintegrationParams.setBodyPSensor(Pose3 pose) ;
preintegrationParams.setGyroscopeCovariance(IMU_metadata.GyroscopeSigma.^2 * eye(3)) ;
preintegrationParams.setIntegrationCovariance(IMU_metadata.IntegrationSigma.^2 * eye(3)) ;
preintegrationParams.setOmegaCoriolis(w_coriolis);

In addition, the constrction of class "ImuFactor" was modified by:

ImuFactor( ...
      currentPoseKey-1, currentVelKey-1, ...
      currentPoseKey, currentVelKey, ...
      currentBiasKey, currentSummarizedMeasurement, g, w_coriolis)

Furthermore, i use the predict function of currentSummarizedMeasurement to predict the navigation state which was as the initial value for the optimization.

    %Predict the navstate
    currentState = currentSummarizedMeasurement.predict(currentState,currentBias);
    currentPoseGlobal = currentState.pose() ;
    currentVelocityGlobal = currentState.velocity() ; 

However, after above modify, i meet these problem:

```

I do not know much about C++ programming，anyone who can help me?

[Mike Sheffler]

@dellaert, I'm finally updating my codebase from 3.2.1 to 4.0 and I took a look at IMUKittiExampleGPS.m to make sure everything would run okay before I tried to update the MATLAB code I wrote for my own projects.

Like @Davis2017, I saw that the version that is committed is busted. He (?) is right that the interface for the pre-integrated measurements has changed (he typed ImuFactorPreintegratedMeasurements twice, but I think he meant ImuFactorPreintegratedMeasurements and PreintegratedImuMeasurements). The interface for gtsam::Values has also changed, which might be why he tried to use .predict to get a value for, e.g., currentPoseGlobal. I think using predict is not what he wanted to do, and he should have instead called calculateEstimate() (with no parameters) on the isam object, and then called the new atPose3 accessor with the appropriate key.

So

currentPoseGlobal = isam.calculateEstimate(currentPoseKey);
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);

probably should have been modified to

result = isam.calculateEstimate();

currentPoseGlobal = result.atPose3(currentPoseKey);
currentVelocityGlobal = result.atVector(currentVelKey);
currentBiasGlobal = result.atConstantBias(currentBiasKey);

The change in the way Values works also breaks plot3DTrajectory. On my machine, calling .atPose3(key) for a key that does not point to an object of type Pose3 causes a straight-up crash (of the MATLAB environment -- not just the .m file that is running), even though it's executed in a try / catch block. I clumsily worked around it by sending in a collection of keys I knew to all point to Pose3 objects.

Anyway, with those two changes (and the modifications that update calling ImuFactorPreintegratedMeasurements to calling PreintegratedImuMeasurements), the IMUKittiExampleGPS.m example runs on my machine. I'll submit a PR when I get a chance (probably within a week or so).

[haifadai]

@Mike Sheffler, Thank you for you help! You are right that I mistakenly typed ImuFactorPreintegratedMeasurements twice. As for .predict, I use it for the reason that I think the IMU predicted value is more suitable for the initial value for the optimization. The problem is no longer appear after I modified my code as you suggest：

result = isam.calculateEstimate();

currentPoseGlobal = result.atPose3(currentPoseKey);
currentVelocityGlobal = result.atVector(currentVelKey);
currentBiasGlobal = result.atConstantBias(currentBiasKey);

However, it crush after awhile. It may be occured in plot3DTrajectory as you mentioned, but I cannot understand how you fix it. Can you explain it in detail?

[Mike Sheffler]

As for .predict, I use it for the reason that I think the IMU predicted value is more suitable for the initial value for the optimization

That is true. I meant more from the perspective of not changing the way that the original example worked.

It may be occured in plot3DTrajectory as you mentioned, but I cannot understand how you fix it. Can you explain it in detail?

Sure. The first thing you can try, of course, is just commenting out the drawing code (everything in the if rem(measurementIndex,10)==0 % plot every 10 time steps block) to see if everything keeps running.

If it turns out that plot3DTrajectory really is the problem, it's likely that (in plot3DTrajectory.m), lines 21, 27, and 48, where you have a statement like x = values.atPose3(key); are the issue. When key is the key for a type that is not of type Pose3, I, at least, get a crash. I haven't actually followed it in the debugger, but my guess is that a cast is going to be unsuccessful or unsafe.

So, we just need to make sure that we only pass in keys for Pose3 objects. To do this,

• Modify IMUKittiExampleGPS.m:
  • After newValues = Values;, add a new line: PoseKeys = KeyVector;
  • After currentPoseKey = symbol('x',measurementIndex);, add a new line: PoseKeys.push_back(currentPoseKey);
  • Comment out the line plot3DTrajectory(isam.calculateEstimate, 'g-'); and add a new line plot3DTrajectoryPosesOnly(isam.calculateEstimate, PoseKeys, 'g-');
• Create plot3DTrajectoryPosesOnly:
  • Copy-and-paste the contents of plot3DTrajectory.m into a new file
  • Change the first line to read function plot3DTrajectoryPosesOnly(values, keys,linespec,frames,scale,marginals)
  • Comment out the line keys = KeyVector(values.keys);
  • Save the file as plot3DTrajectoryPosesOnly.m. I put mine right next to plot3DTrajectory.m.

If you have the same problem I had, I think this will work. It could be a little cleaner, but this is the fewest number of edits I can think of.

[haifadai]

@Mike Sheffler I commented out the drawing code,

 % if rem(measurementIndex,10)==0 % plot every 10 time steps
     %   cla;
     %  plot3DTrajectory(isam.calculateEstimate, 'g-');
     % title('Estimated trajectory using ISAM2 (IMU+GPS)')
       ...
     % drawnow;
      %end

but it still crush. why?

[Mike Sheffler]

but it still crush. why?

I don't know. Are you still trying

%Predict the navstate
    currentState = currentSummarizedMeasurement.predict(currentState,currentBias);
    currentPoseGlobal = currentState.pose() ;
    currentVelocityGlobal = currentState.velocity() ; 

?

I strongly doubt currentState = currentSummarizedMeasurement.predict(currentState,currentBias); is the syntax you want. At least predict to a new variable (PredictedCurrentState = currentSummarizedMeasurement.predict(currentState,currentBias); or something) and figure out if you're getting what you expect and figure out how to copy it from there.

First try not doing the .predict to get the value of currentPoseGlobal and currentVelocityGlobal. If that works, then try adding back .predict, but be careful about how you use the results. Be wary of using = to make the copy, as most GTSAM objects in MATLAB are 'handle objects' and you might only be getting a reference to something.

[haifadai]

@mike I deprecated .predict and modified theIMUKittiExampleGPS.m and plot3DTrajectory.m. After then, It works, however, the trajectory it drawn was obviously different from the one of version 3.2.1. Have you encountered this situation?

[Mike Sheffler]

@Davis2017 I don't believe so. I just ran the 4.0 example and the 3.2.1 example and the plots looked the same to me. I don't believe I made any unusual choices in CMake, so I think my build is pretty much the regular setup. Best of luck.

[haifadai]

@Mike Sheffler I found it just drawn the line between current Pose and last pose rather than all the pose. I don't know how to fix it. I am wondering whether you could send me the code you modified. In addition, I found the reason why the prediction function went wrong. This is because the NavState is not updated after the factor graph is updated. As a result, the data conflicts and the optimization process crashes. So, I add a new line

: currentNavState = NavState(currentPoseGlobal, currentVelocityGlobal);

before the last end; What's more, I find that the optimization of the source code is not very good. I doubt it is because it add GPSFactor every 10 GPS data. So, I modified the line:

if mod(measurementIndex, GPSskip) ==0
newFactors.add (PriorFactorPose3(currentPoseKey,...
end

as:

newFactors.add (PriorFactorPose3(currentPoseKey,...

After above modifidation, it works well.

[Frank Dellaert]

Excellent sleuthing! Note there is a way to get all poses from a Values instance, it's wrapped as utilities::allPose3s. Maybe that's a better solution to avoid a crash in plot3DTrajectory? Anyway, I'll be happy to review a PR.

[Mike Sheffler]

I found it just drawn the line between current Pose and last pose rather than all the pose. I don't know how to fix it.

@Davis2017 You know what's weird? I started having this problem too! I'm not sure why yet, but I think the logic for hold is not quite right. It looks like there is a typo moving from 3.2.1 to 4.0. Find the last end statement -- this is the last word in the file (I'm talking, specifically, about plot3DTrajectoryPosesOnly.m, but this is also true for plot3DTrajectory.m from which it was copied). Move that end to before the comment

%Draw final pose

After you do that, the %Draw final pose and if ~holdstate blocks will be outside the scope of the main for loop, and that is what you want.

I'm 99% sure that what happened is that someone saw the extra end statement at the end of plot3DTrajectory.m in the 3.2.1 toolbox (the extra end being legal but unusual MATLAB syntax for a function) and got confused about the appropriate scope of the for loop when he or she modified plot3DTrajectory.m for the 4.0 toolbox.

Note there is a way to get all poses from a Values instance, it's wrapped as utilities::allPose3s.

@dellaert I did not notice that! That is a better solution! I checked, and it works. I'll make a PR that incorporates fixes to IMUKittiExampleGPS.m and plot3DTrajectory.m (instead of creating a new plotting file with a different signature), hopefully this weekend.

[haifadai]

@Mike Sheffler You are right, it works. Thank you very much for you help to solve this probelem, which confused me long time. But I still have some other doubts: (1) about nosiseModelGPS

noiseModelGPS = noiseModel.Diagonal.Precisions([ [0;0;0]; 1.0/0.07 *[1;1;1] ]);

why the part of position equal to 1.0/0.07*[1;1;1]? I doubt '1.0' is the Standard deviation of GPS position, but what's the meaning divided by 0.07？ What's more, why it return 'Sigma [[inf; inf; inf; 0.2645; 0.2645; 0.2645]'?

(2) I found the estimate Bias of IMU is not accurate as decribed by the paper, why?

(3) the initial velocity is set as zero, however the GPS trajectory shows that this is not the case. I notice that the initia velocity sigma is set as '1000.0'. Does this indicate that the speed is very uncertain?

[Mike Sheffler]

@Davis2017 I'm afraid I can't answer those questions.

@dellaert Frank, I made a PR to address the issues addressed in this thread.

[haifadai]

@Mike Sheffler Anyway, thank you very much!

[haifadai]

@Mike Sheffler, Hi, Can I add your contact information?

[haifadai]

@Frank Dellaert, I am trying to use this toolbox to fuse IMU measurement with the velocity measurement measured by Doppler log. However, the error of estimated position is big. In my opinion, maybe this is because I did not consider the Coriolis effect. That is, the w_coriolis is set to be zero. How should I set this value?

[Frank Dellaert]

Solved by PR #311