12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2067 from mhalber/features/similarity_python_bindings 

Python bindings: Enable use of Similarity2/Similarity3 in nonlinear optimization
release/4.3a0
Varun Agrawal 2025-03-22 13:46:37 -04:00 committed by GitHub
parent b8dbcba743 1575968571
commit fae071d603
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 97 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Similarity2.h
View File

@ -200,6 +200,19 @@ class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
/// Dimensionality of tangent space = 4 DOF
inline size_t dim() const { return 4; }
private:
#if GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(s_);
}
#endif
/// @}
};

16
gtsam/nonlinear/nonlinear.i
View File

@ -17,6 +17,8 @@ namespace gtsam {
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
@ -74,6 +76,8 @@ class NonlinearFactorGraph {
gtsam::Rot3,
gtsam::Pose2,
gtsam::Pose3,
gtsam::Similarity2,
gtsam::Similarity3,
gtsam::Cal3_S2,
gtsam::Cal3f,
gtsam::Cal3Bundler,
@ -544,7 +548,7 @@ class ISAM2 {
bool valueExists(gtsam::Key key) const;
gtsam::Values calculateEstimate() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3f, gtsam::Cal3Bundler,
gtsam::EssentialMatrix, gtsam::FundamentalMatrix, gtsam::SimpleFundamentalMatrix,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
@ -609,6 +613,8 @@ template <T = {double,
gtsam::Rot3,
gtsam::Pose2,
gtsam::Pose3,
gtsam::Similarity2,
gtsam::Similarity3,
gtsam::Unit3,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
@ -633,7 +639,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h>
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@ -651,7 +657,7 @@ virtual class NonlinearEquality : gtsam::NoiseModelFactor {
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@ -720,7 +726,7 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
gtsam::NonlinearFactorGraph getFactors() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Rot3, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Vector, gtsam::Matrix}>
VALUE calculateEstimate(size_t key) const;
};
@ -732,6 +738,8 @@ template <T = {gtsam::Point2,
gtsam::Rot3,
gtsam::Pose2,
gtsam::Pose3,
gtsam::Similarity2,
gtsam::Similarity3,
gtsam::NavState,
gtsam::imuBias::ConstantBias}>
virtual class ExtendedKalmanFilter {

10
gtsam/nonlinear/values.i
View File

@ -17,6 +17,8 @@ namespace gtsam {
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/geometry/Similarity3.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/SO3.h>
@ -80,6 +82,8 @@ class Values {
void insert(size_t j, const gtsam::SOn& P);
void insert(size_t j, const gtsam::Rot3& rot3);
void insert(size_t j, const gtsam::Pose3& pose3);
void insert(size_t j, const gtsam::Similarity2& similarity2);
void insert(size_t j, const gtsam::Similarity3& similarity3);
void insert(size_t j, const gtsam::Unit3& unit3);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::Cal3f& cal3f);
@ -122,6 +126,8 @@ class Values {
void update(size_t j, const gtsam::SOn& P);
void update(size_t j, const gtsam::Rot3& rot3);
void update(size_t j, const gtsam::Pose3& pose3);
void update(size_t j, const gtsam::Similarity2& similarity2);
void update(size_t j, const gtsam::Similarity3& similarity3);
void update(size_t j, const gtsam::Unit3& unit3);
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::Cal3f& cal3f);
@ -161,6 +167,8 @@ class Values {
void insert_or_assign(size_t j, const gtsam::SOn& P);
void insert_or_assign(size_t j, const gtsam::Rot3& rot3);
void insert_or_assign(size_t j, const gtsam::Pose3& pose3);
void insert_or_assign(size_t j, const gtsam::Similarity2& similarity2);
void insert_or_assign(size_t j, const gtsam::Similarity3& similarity3);
void insert_or_assign(size_t j, const gtsam::Unit3& unit3);
void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert_or_assign(size_t j, const gtsam::Cal3f& cal3f);
@ -196,6 +204,8 @@ class Values {
gtsam::SOn,
gtsam::Rot3,
gtsam::Pose3,
gtsam::Similarity2,
gtsam::Similarity3,
gtsam::Unit3,
gtsam::Cal3Bundler,
gtsam::Cal3f,

3
gtsam/slam/slam.i
View File

@ -7,13 +7,14 @@ namespace gtsam {
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/SO4.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/geometry/Similarity3.h>
// ######
#include <gtsam/slam/BetweenFactor.h>
template <T = {double, gtsam::Vector, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::SO3,
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity3,
gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Similarity2, gtsam::Similarity3,
gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose,

61
python/gtsam/tests/test_Sim3.py
View File

@ -16,7 +16,7 @@ import numpy as np
from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import Point3, Pose3, Rot3, Similarity3
from gtsam import Point3, Pose3, Rot3, Similarity3, BetweenFactorSimilarity3, NonlinearFactorGraph, Values, LevenbergMarquardtOptimizer, LevenbergMarquardtParams
class TestSim3(GtsamTestCase):
@ -130,6 +130,65 @@ class TestSim3(GtsamTestCase):
for aTi, bTi in zip(aTi_list, bTi_list):
self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
def test_sim3_optimization(self)->None:
# Create a PriorFactor with a Sim3 prior
prior = Similarity3(Rot3.Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4)
model = gtsam.noiseModel.Isotropic.Sigma(7, 1)
# Create graph
graph = NonlinearFactorGraph()
graph.addPriorSimilarity3(1, prior, model)
# Create initial estimate with Identity transform
initial = Values()
initial.insert(1, Similarity3())
# Optimize
params = LevenbergMarquardtParams()
params.setVerbosityLM("TRYCONFIG")
result = LevenbergMarquardtOptimizer(graph, initial).optimize()
# After optimization, result should be prior
self.gtsamAssertEquals(prior, result.atSimilarity3(1), 1e-4)
def test_sim3_optimization2(self) -> None:
prior = Similarity3()
m1 = Similarity3(Rot3.Ypr(np.pi / 4.0, 0, 0), Point3(2.0, 0, 0), 1.0)
m2 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(np.sqrt(8) * 0.9, 0, 0), 1.0)
m3 = Similarity3(Rot3.Ypr(3 * np.pi / 4.0, 0, 0), Point3(np.sqrt(32) * 0.8, 0, 0), 1.0)
m4 = Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(6 * 0.7, 0, 0), 1.0)
loop = Similarity3(1.42)
priorNoise = gtsam.noiseModel.Isotropic.Sigma(7, 0.01)
betweenNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10]))
betweenNoise2 = gtsam.noiseModel.Diagonal.Sigmas(np.array([ 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0]))
b1 = BetweenFactorSimilarity3(1, 2, m1, betweenNoise)
b2 = BetweenFactorSimilarity3(2, 3, m2, betweenNoise)
b3 = BetweenFactorSimilarity3(3, 4, m3, betweenNoise)
b4 = BetweenFactorSimilarity3(4, 5, m4, betweenNoise)
lc = BetweenFactorSimilarity3(5, 1, loop, betweenNoise2)
# Create graph
graph = NonlinearFactorGraph()
graph.addPriorSimilarity3(1, prior, priorNoise)
graph.add(b1)
graph.add(b2)
graph.add(b3)
graph.add(b4)
graph.add(lc)
# graph.print("Full Graph\n");
initial=Values()
initial.insert(1, prior)
initial.insert(2, Similarity3(Rot3.Ypr(np.pi / 2.0, 0, 0), Point3(1, 0, 0), 1.1))
initial.insert(3, Similarity3(Rot3.Ypr(2.0 * np.pi / 2.0, 0, 0), Point3(0.9, 1.1, 0), 1.2))
initial.insert(4, Similarity3(Rot3.Ypr(3.0 * np.pi / 2.0, 0, 0), Point3(0, 1, 0), 1.3))
initial.insert(5, Similarity3(Rot3.Ypr(4.0 * np.pi / 2.0, 0, 0), Point3(0, 0, 0), 1.0))
result = LevenbergMarquardtOptimizer(graph, initial).optimizeSafely()
self.gtsamAssertEquals(Similarity3(0.7), result.atSimilarity3(5), 0.4)
def test_align_via_Sim3_to_poses_skydio32(self) -> None:
"""Ensure scale estimate of Sim(3) object is non-negative.