GTSAM speed

[David O'Brien-Moller]

Hi All,

I am using GTSAM to do pose estimation of an underwater drone fitted with a stereo camera. I'm using the python wrapper for the library, and it's fantastic to use and working great! However, it is quite slow. I'm wondering what speeds can you expect? Is there any ways to speed up the GTSAM optimisation.

For context, I ran an optimisation consisting of ~40,000 factors and 26,000 Variables and it took about 8 hours to run. I have an i7 8th gen CPU. I am using a custom factor. Is this performance normal?

Thank you,

David

[Dellaert, Frank]

David

That is very slow. The wrapper is compiled with release mode, so that should be fine. In GTSFM - which uses the wrapper - we run much larger problems, in a fraction of the time. Two things to check:

- do you have a decent initialization? I’d plot the error over iteration and see what this looks like.

- is there some specific factor you use that is causing a slowdown? Regardless, I’d do some profiling and get insight.

Frank

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of David O'Brien-Moller <davi...@gmail.com>
Sent: Wednesday, September 11, 2024 2:51:17 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] GTSAM speed

 

You don't often get email from davi...@gmail.com. Learn why this is important

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/2cf58306-4df6-4f6e-92ab-dd404b1a2b8dn%40googlegroups.com.

[David O'Brien-Moller]

Hi Frank,

Thank you very much for your fast reply, I've done some investigation and I'm certain that the initial estimates are quite accurate, I can also see that the error decreases nicely, it is just that each iteration of the optimiser is extremely slow. I've attached a plot showing the error decrease over iterations for a smaller subset of data, here the optimiser would of course normally exit the optimisation once the error had stabilised, but 100 iterations were forced. 

Here is how my custom factor is looking, I was wondering if it looks reasonable to you? I can't think of any reasons why it would be especially slow but I am very new to GTSAM and SLAM in general! 

def evaluateError(measurement: np.ndarray, this: gtsam.CustomFactor,
values: gtsam.Values,
jacobians: Optional[List[np.ndarray]]) -> float:

pose_key = this.keys()[0]
pose = values.atPose3(pose_key) # Retrieve pose of robot

landmark_key = this.keys()[1]
landmark = values.atPoint3(landmark_key) # Get position of 3d point in local reference frame

# Transform the landmark to the pose's coordinate frame
predicted = pose.transformFrom(measurement)
error = predicted - landmark

if jacobians is not None:
if jacobians[0] is not None:
# Jacobian w.r.t transformation
R = pose.rotation().matrix()
skew_symmetric = np.cross(np.eye(3), measurement).reshape(3, 3)
dR_dtheta = R @ skew_symmetric
J_rot = -dR_dtheta

J_trans = np.eye(3)
jacobians[0] = np.hstack((J_rot,J_trans))
if jacobians[1] is not None:
# Compute Jacobian with respect to the landmark
jacobians[1] = -pose.rotation().matrix()
return error

[Dellaert, Frank]

Ah, yes, I’d not recommend using 40k python custom factors. That will be slow. My recommendation is to either use wrapped factors, or to use many fewer custom factors that batch up all landmarks to a particular pose (or vice versa).

PS it’s crucial you unit test the derivatives if you write your own. Joel Truher shared some numerical differentiation code a week or two ago in this forum.

Frank

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of David O'Brien-Moller <davi...@gmail.com>

Sent: Thursday, September 12, 2024 5:09:34 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] GTSAM speed

 

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/4a85001e-89d7-4d9e-a388-f4877370c1e6n%40googlegroups.com.

[Varun Agrawal]

We've received reports about the python wrapper being slow, though we haven't confirmed this via a load test.

We'd be very interested in a deep dive into this, as well as the effect of TBB on performance.

--

[Joel Truher]

hi everybody,

I'm able to get back to this work now, and actually I was going to ask about python performance.  My experiments show the python implementation of BetweenFactor and the C++ version to be about 20X slower than the C++ version -- both are logically very simple things, of course, the estimation function is literally x.logmap(y).  This is using the numeric differentation in python, so quite a few trips across the pybind boundary, I suppose.

My goal is to run at about twice real time (i.e. 50% slack) for a 50 Hz sample rate for each of a few measurements, using a few kinds of factors, in real time, for a smoother window of about a second, so maybe a graph of a few hundred variables and a few hundred factors.  It looks like maybe python isn't going to be able to do it.  It *can* run the framework fast enough, which is convenient, e.g. calling the C++ BetweenFactor goes plenty fast.

Unless anyone has any ideas for speeding up the python CustomFactor 20X, I know what I'll be doing this weekend.  :-)

[Dellaert, Frank]

Did you try making the factor a batch factor? The cool thing about the customfactor is that you can treat many measurements all at once if they involve the same unknowns. It could still be slow in python because of the multithreading issue, I forget whether we take advantage of TBB when calling from Python. I admit that if we do timing tests, we do it in C++ :-)

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Joel Truher <joel....@gmail.com>
Sent: Thursday, October 31, 2024 9:52:35 PM

To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] GTSAM speed

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/103d8afb-99eb-4d7b-b6a0-6a4a3c00571en%40googlegroups.com.

[Jiang, Fan]

Hi all,

TBB does not work with Custom Factors due to the Python Global Interpreter Lock (GIL).

Best,

Fan

Fan Jiang

5th year Robotics PhD
Georgia Institute of Technology

E-mail: fan....@gatech.edu

Try TeXmacs today!

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/SJ0PR07MB90746F8F22C074E1F4B89684CF562%40SJ0PR07MB9074.namprd07.prod.outlook.com.

[Joel Truher]

do you mean to give the whole smoother window to a single factor at once?

[Dellaert, Frank]

No. Actually, it might not be relevant in your application. I was referring to, for example, structure from motion where many measurements are seen the same image, or vice versa: one landmark is seen in several images.

For the latter case in SFM we actually have a custom C++ factor that is available in Python as well (look in slam.i).

F

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Joel Truher <joel....@gmail.com>

Sent: Friday, November 1, 2024 9:56:32 AM

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/b70ebd04-c419-4df6-8bba-4c86949ed035n%40googlegroups.com.

[Joel Truher]

oh, yeah, thanks!  that's true actually, there are multiple landmarks per image.  concatenating all the measurements into a single factor does indeed speed things up quite a bit.  this is high school robotics, so 4 apriltag corners = 4 points in the frame.  using one batch factor instead of 4 singe-point factors results in 60% improvement in latency.  yay!

another thing i've been wondering about for the smoothing task is the size of the window.  we never do loop-closing, we're just doing localization within a fixed map, using odometry, gyro, and vision.  the vision input is considerably delayed (~50 ms) compared to the other inputs, so i imagine my smoother window needs to be at least that big.  does a longer window make much difference for this kind of task?