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

Vector in template param: works! Also fix tests for recent changes...

release/4.3a0
Duy-Nguyen Ta 2016-09-19 12:39:41 -04:00
parent f5691804ed
commit bfb58143f5
2 changed files with 21 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cython/gtsam.h
View File

@ -2174,7 +2174,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
PriorFactor(const This& other);

37
cython/tests.py
View File

@ -12,8 +12,8 @@ v = np.array([.1, .1, .1])
print("v = ", v)
r4 = r3.retract(v)
print("r4 pitch:", r4.pitch())
r4._print(b'r4: ')
r3._print(b"r3: ")
r4.print_(b'r4: ')
r3.print_(b"r3: ")
v = r3.localCoordinates(r4)
print("localCoordinates:", v)
@ -23,46 +23,49 @@ Rmat = np.array([
[0.104218, 0.990074, -0.0942928],
[-0.0942928, 0.104218, 0.990074]
])
r5 = Rot3.ctor1(Rmat)
r5._print(b"r5: ")
r5 = Rot3.Rot3_1(Rmat)
r5.print_(b"r5: ")
l = Rot3.Logmap(r5)
print("l = ", l)
noise = noiseModel_Gaussian.Covariance(Rmat)
noise._print(b"noise:")
noise.print_(b"noise:")
D = np.array([1.,2.,3.])
diag = noiseModel_Diagonal.Variances(D)
print("diag:", diag)
diag._print(b"diag:")
diag.print_(b"diag:")
print("diag R:", diag.R())
p = Point3()
p._print("p:")
factor = BetweenFactorPoint3.ctor(1,2,p, noise)
factor._print(b"factor:")
p.print_("p:")
factor = BetweenFactorPoint3.BetweenFactorPoint3(1,2,p, noise)
factor.print_(b"factor:")
vv = VectorValues()
vv._print(b"vv:")
vv.print_(b"vv:")
vv.insert(1, np.array([1.,2.,3.]))
vv.insert(2, np.array([3.,4.]))
vv.insert(3, np.array([5.,6.,7.,8.]))
vv._print(b"vv:")
vv.print_(b"vv:")
vv2 = VectorValues.ctor1(vv)
vv2 = VectorValues.VectorValues_1(vv)
vv2.insert(4, np.array([4.,2.,1]))
vv2._print(b"vv2:")
vv._print(b"vv:")
vv2.print_(b"vv2:")
vv.print_(b"vv:")
vv.insert(4, np.array([1.,2.,4.]))
vv._print(b"vv:")
vv.print_(b"vv:")
vv3 = vv.add(vv2)
vv3._print(b"vv3:")
vv3.print_(b"vv3:")
values = Values()
values.insertPoint3(1, Point3())
values.insertRot3(2, Rot3())
values._print(b"values:")
values.print_(b"values:")
factor = PriorFactorVector.PriorFactorVector(1, np.array([1.,2.,3.]), diag)
factor.print_("Prior factor vector: ")