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

translation recovery unit tests pass

release/4.3a0
Akshay Krishnan 2020-09-06 23:54:27 +00:00
parent 0fb5c0d228
commit 556531f8b7
2 changed files with 27 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/gtsam.i
View File

@ -2918,7 +2918,7 @@ class BinaryMeasurementsUnit3 {
BinaryMeasurementsUnit3(); BinaryMeasurementsUnit3();
size_t size() const; size_t size() const;
gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const; gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3> measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
}; };
#include <gtsam/sfm/ShonanAveraging.h> #include <gtsam/sfm/ShonanAveraging.h>

42
python/gtsam/tests/test_TranslationRecovery.py
View File

@ -5,28 +5,30 @@ import unittest
import gtsam import gtsam
def SimulateMeasurements(gt_poses, graph_edges): """ Returns example pose values of 3 points A, B and C in the world frame """
measurements = gtsam.BinaryMeasurementsUnit3()
for edge in graph_edges:
Ta = gt_poses.atPose3(edge[0]).translation()
Tb = gt_poses.atPose3(edge[1]).translation()
measurements.append(BinaryMeasurementUnit3( \
edge[0], edge[1], gtsam.Unit3(Tb - Ta), \
gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
return measurements
# Hard-coded values from dubrovnik-3-7-pre.txt
def ExampleValues(): def ExampleValues():
T = [] T = []
T.append(gtsam.Point3(np.array([7.3030e-01, -2.6490e-01, -1.7127e+00]))) T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
data = gtsam.Values() data = gtsam.Values()
for i in range(len(T)): for i in range(len(T)):
data.insert(i, gtsam.Pose3(R[i], T[i])) data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i]))
return data return data
""" Returns binary measurements for the points in the given edges."""
def SimulateMeasurements(gt_poses, graph_edges):
measurements = gtsam.BinaryMeasurementsUnit3()
for edge in graph_edges:
Ta = gt_poses.atPose3(edge[0]).translation()
Tb = gt_poses.atPose3(edge[1]).translation()
measurements.append(gtsam.BinaryMeasurementUnit3( \
edge[0], edge[1], gtsam.Unit3(Tb - Ta), \
gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
return measurements
""" Tests for the translation recovery class """
class TestTranslationRecovery(unittest.TestCase): class TestTranslationRecovery(unittest.TestCase):
"""Test selected Translation Recovery methods.""" """Test selected Translation Recovery methods."""
@ -39,9 +41,17 @@ class TestTranslationRecovery(unittest.TestCase):
gt_poses = ExampleValues() gt_poses = ExampleValues()
measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
algorithm = gtsam.TranslationRecovery(measurements) algorithm = gtsam.TranslationRecovery(measurements)
result = algorithm.run(2.0) scale = 2.0
for i in range(3): result = algorithm.run(scale)
self.gtsamAssertEquals(result.atPoint3(i), 2*gt_poses.atPose3(i).translation(), 1e-6)
w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation()
w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation()
w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb)
w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb)
np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.")
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()