Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Imu Examples Refactor #872

Merged
merged 6 commits into from
Oct 29, 2021
Merged

Imu Examples Refactor #872

merged 6 commits into from
Oct 29, 2021

Conversation

varunagrawal
Copy link
Collaborator

  1. Custom preintegration params support.
  2. Make plotting into its own function.
  3. Minor formatting.

@varunagrawal varunagrawal self-assigned this Sep 10, 2021
@varunagrawal varunagrawal added the cleanup Help clean up old/obsolete aspects of GTSAM label Sep 10, 2021
@varunagrawal
Copy link
Collaborator Author

Gentle reminder

@varunagrawal
Copy link
Collaborator Author

@johnwlambert gentle reminder

parser.add_argument("--twist_scenario",
default="sick_twist",
choices=("zero_twist", "forward_twist", "loop_twist",
"sick_twist"))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi Varun, maybe you could define "sick_twist" in the "help" section for this argparse argument?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This should be documented in the ScenarioRunner file.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Makes sense. Might be nice to include a few words about it here, just to be self-contained.

state = self.scenario.navState(i)
graph.push_back(
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
graph.push_back(
gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))

def optimize(self, graph, initial):
def optimize(self, graph: gtsam.NonlinearFactorGraph,
initial: gtsam.Values):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i'm curious how using the prior factor to a value at each time step affects performance, rather than just initializing LM with those values

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just providing it as initial values means that there is no constraint on thayt value. The prior says that it should not deviate too much from this initial value.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Makes sense, have you found this constraint to have a big impact on performance in your experience?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For my use cases, yes. Something as simple as fixing the origin via a prior can be pretty powerful.


print("Bias Values", result.atConstantBias(BIAS_KEY))
print("Bias Values", values.atConstantBias(BIAS_KEY))
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

might be nice to put in the brief docstring at the top of the file what the goal of this IMU example is and the significance of the "bias" values computed.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This already exists right? The assumption here is that the person reading this file is already familiar with IMU Preintegration and is essentially trying to figure out how to use it.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see. I haven't worked with IMU pre-integration before, though, so would be beneficial for a gtsam user like me. I think there are some users that might just peruse the examples out of interest in learning more.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

For my use cases, yes. Something as simple as fixing the origin via a prior can be pretty powerful.

plt.show()

def run(self,
T: int = 12,
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

might be nice in the docstring to explain what "T" represents

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Args:
    T: number of seconds to perform state estimation for
    compute_covariances: ...

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That's the general convention where T is the total time for the trajectory. I'll add in the docstring.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

i think this one didn't get updated still

ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)
ax.set_zlim3d(-self.maxDim, self.maxDim)

plt.pause(time_interval)

def run(self, T=12):
# simulate the loop
def run(self, T: int = 12):
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nit: we can put a return type here:

def run(self, T: int = 12) -> None

Copy link
Collaborator Author

@varunagrawal varunagrawal Oct 28, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you do a full sweep of the type reviews rather than in bits please? It's mildly frustrating to have to make multiple of these updates especially since they are tangential to the main aspect of this PR.

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

And especially since the None return type doesn't really give us any information...

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure, I missed this one earlier

Copy link
Contributor

@johnwlambert johnwlambert left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM modulo last 3 review comments

@varunagrawal varunagrawal merged commit 41dc3f8 into develop Oct 29, 2021
@varunagrawal varunagrawal deleted the fix/imu-examples branch October 29, 2021 13:34
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
cleanup Help clean up old/obsolete aspects of GTSAM
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants