-
assigned issue to
- edited description
bug about matlab toolbox
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?
Comments (25)
-
reporter -
I don't regularly use matlab anymore, someone else can maybe help? Is it feasible to submit pull request?
-
- removed responsible
-
I also get a lot of errors and crash with the examples, any fix in the near future?
-
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....
-
reporter Thank you! I will try it.
发自网易邮箱大师
-
Any luck?
-
reporter 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?
-
@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 meantImuFactorPreintegratedMeasurements
andPreintegratedImuMeasurements
). The interface forgtsam::Values
has also changed, which might be why he tried to use.predict
to get a value for, e.g.,currentPoseGlobal
. I think usingpredict
is not what he wanted to do, and he should have instead calledcalculateEstimate()
(with no parameters) on theisam
object, and then called the newatPose3
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 breaksplot3DTrajectory
. On my machine, calling.atPose3(key)
for akey
that does not point to an object of typePose3
causes a straight-up crash (of the MATLAB environment -- not just the .m file that is running), even though it's executed in atry
/catch
block. I clumsily worked around it by sending in a collection of keys I knew to all point toPose3
objects.Anyway, with those two changes (and the modifications that update calling
ImuFactorPreintegratedMeasurements
to callingPreintegratedImuMeasurements
), theIMUKittiExampleGPS.m
example runs on my machine. I'll submit a PR when I get a chance (probably within a week or so). -
reporter @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?
-
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 (inplot3DTrajectory.m
), lines 21, 27, and 48, where you have a statement likex = values.atPose3(key);
are the issue. Whenkey
is the key for a type that is not of typePose3
, 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 lineplot3DTrajectoryPosesOnly(isam.calculateEstimate, PoseKeys, 'g-');
- After
- 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 toplot3DTrajectory.m
.
- Copy-and-paste the contents of
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.
- Modify
-
reporter @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?
-
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 ofcurrentPoseGlobal
andcurrentVelocityGlobal
. 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. -
reporter @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?
-
@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.
-
reporter @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.
-
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.
-
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 lastend
statement -- this is the last word in the file (I'm talking, specifically, aboutplot3DTrajectoryPosesOnly.m
, but this is also true forplot3DTrajectory.m
from which it was copied). Move thatend
to before the comment%Draw final pose
After you do that, the
%Draw final pose
andif ~holdstate
blocks will be outside the scope of the mainfor
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 ofplot3DTrajectory.m
in the 3.2.1 toolbox (the extraend
being legal but unusual MATLAB syntax for a function) and got confused about the appropriate scope of thefor
loop when he or she modifiedplot3DTrajectory.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
andplot3DTrajectory.m
(instead of creating a new plotting file with a different signature), hopefully this weekend. -
reporter @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?
-
@Davis2017 I'm afraid I can't answer those questions.
@dellaert Frank, I made a PR to address the issues addressed in this thread.
-
reporter @Mike Sheffler Anyway, thank you very much!
-
reporter @Mike Sheffler, Hi, Can I add your contact information?
-
reporter @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?
-
-
assigned issue to
-
assigned issue to
-
- changed status to resolved
Solved by PR #311
- Log in to comment