VectorValues::dot Erratically occurs in code

[Dan Nguyen]

Hi all,

I have an implementation for a SLAM pipeline using GTSAM's iSAM2 model that encounters the VectorValues::dot error erratically. 

What is happening in my code:

The SLAM pipeline is used by a robot on cones. Robot poses are represented as Pose2 and landmarks/cones are represented using the Point2 data type. 

To update the factor graph, I define a BetweenFactor between the Pose2 at time t-1 and the Pose2 at time t (current time), and then add the BetweenFactor to the factor graph of the iSAM2 model. I define a BearingRangeFactor between the Pose2 at time t and a landmark observed at time t. 

isam2 is the variable representing our iSAM2 model. I call isam2.update(graph, values) a.) after every time that I construct the BetweenFactor and b.) after every time I create the BearingRangeFactors for all observed landmarks.

What's the issue:

I identified that the VectorValues dot product occurring at the beginning of ComputeBlend is the cause of the error. Specifically, when x_u is multiplied to x_n. x_u and x_n differs in size by 1 and this is what is causing the error. 

ComputeBlend gets called through the call to updateDelta in ISAM2.cpp when the model needs to relinearize.

Can I ask for input on this issue? Any advice on this issue would be greatly appreciated.

[Dellaert, Frank]

Hi Dan

 

Did you attach code?

Note it might be easiest if you have a very minimal example of the failure – or even better: a failing unit test 😊 Although: I realize it only happens erratically, so maybe that’s not easy to do.

 

FD

 

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Dan Nguyen <dngu...@andrew.cmu.edu>
Date: Wednesday, December 4, 2024 at 5:17 PM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] VectorValues::dot Erratically occurs in code

You don't often get email from dngu...@andrew.cmu.edu. 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 visit https://groups.google.com/d/msgid/gtsam-users/b5bc7f6d-c987-4d3d-9894-34e3bfa83f85n%40googlegroups.com.

[Dan Nguyen]

Thank you for your response!

The most recent development that has occurred for our SLAM pipeline has been happening in the slam_refactor branch: https://github.com/carnegiemellonracing/iSAM2_SLAM/tree/slam_refactor 
The relevant code is in the directory: iSAM2_SLAM/workspace/src/isam2/
isam2.cpp is the file with the code where we define our isam2 model

isam2Node.cpp is the file where we construct our ROS2 node to subscribe to LiDAR and odometry information.
ros_utils.cpp is where we define utility functions.

The VectorValue error frequently occurs at the beginning when the iSAM2 node is first run. We will generally get the error within the first 100 poses. During this time, the robot is stationary and a majority of the time, we will run into the VectorValues error. When the robot is moving and observes more landmarks, we do not get the error as often. Here is a picture of logging x_u and x_n from inside ComputeBlend when the error occurs:

Thank you! Let me know if you have any questions.

[Dan Nguyen]

As for tests and unit tests, I have been testing the code using rosbag data. But I have noticed that the error is encountered when the iSAM2 model is running but the car is not moving. GTSAM builds with all unit tests passed. The values being fed to the iSAM2 model are defined and not all-0 element Pose2 or Point2 types. 

[Dellaert, Frank]

My advice is to try and get a minimal example working that deterministically fails. If it is truly a problem in GTSAM, then that can be developed into a unit test and we can help diagnose and correct the issue.

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Dan Nguyen <dngu...@andrew.cmu.edu>

Sent: Thursday, December 5, 2024 3:52:51 PM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] VectorValues::dot Erratically occurs in code

 

To view this discussion visit https://groups.google.com/d/msgid/gtsam-users/37e10870-fb73-4181-b8bd-ca4703532389n%40googlegroups.com.

[Dan Nguyen]

I have created an example where the code consistently fails. From one of our rosbags, I logged the input parameters for the function updating the isam2 model called step starting from the beginning (timestep 0). At time step 43, the 43rd iteration of calling step, the code encounters the VectorValues error. 

Here are instructions to set up and reproduce the error:

Setup:

1.) git clone https://github.com/carnegiemellonracing/iSAM2_SLAM

- All of the necessary code is in the main branch

2.) The following packages need to be built: interfaces, isam2, and test_ws

- run: "colcon build --packages-up-to test_ws"

- interfaces is the package where our ros messages are defined

- isam2 is the package where our SLAM node is defined and where the step function is defined

- test_ws is the package where the test case is defined

3.) After sourcing the setup.bash file, run the test case.

- run the following:

- "source install/setup.bash" from the "workspace" directory 

- "ros2 run test_ws test_run"

Please let me know if there are any questions. 

Thank you,

Dan