search

borglab / gtsam

GTSAM is a library of C++ classes that implement smoothing and mapping (SAM) in robotics and vision, using factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices.
http://gtsam.org
Other
2.63k stars 767 forks source link

Factor between two landmarks #1708

Closed pie-leroy closed 9 months ago

pie-leroy commented 10 months ago

Hello, I am looking to develop a SLAM using the gtsam Python API. In my project, I am doing image segmentation which allows me to extract a landmark for each object of interest. I can make assumptions about the distances between two landmarks, i.e., a minimum distance. My landmarks are 3D Points.

I feel like I could use "ReferenceFrameFactor" but I haven't succeeded in Python. Is it possible to insert a factor between two landmarks? If yes, how? Thank you in advance.

ProfFan commented 10 months ago

Hi, is BetweenFactor good for this? Is there anything special?

pie-leroy commented 9 months ago

Thank you very much, indeed it works, here is the code used

noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.0001) distance_factor = gtsam.BetweenFactorPoint3(symbol('l', 0), symbol('l', 24), np.array([0.0, 0.0, 553]), noise_model) graph.add(distance_factor) result = optimize(graph, initial_estimate)