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

docs fixed and error threshold reduced

release/4.3a0
Sushmita 2020-11-28 15:49:08 -05:00
parent 02e94730a6
commit cc54b18fe5
2 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/geometry/triangulation.h
View File

@ -18,14 +18,14 @@
#pragma once #pragma once
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h> #include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam { namespace gtsam {
@ -496,6 +496,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
} }
} }
// Vector of Cameras - used by the Python/MATLAB wrapper
typedef CameraSet<PinholeCamera<Cal3Bundler>> CameraSetCal3Bundler; typedef CameraSet<PinholeCamera<Cal3Bundler>> CameraSetCal3Bundler;
typedef CameraSet<PinholeCamera<Cal3_S2>> CameraSetCal3_S2; typedef CameraSet<PinholeCamera<Cal3_S2>> CameraSetCal3_S2;

34
python/gtsam/tests/test_Triangulation.py
View File

@ -15,7 +15,9 @@ import numpy as np
import gtsam as g import gtsam as g
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \ from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3Bundler PinholeCameraCal3_S2, PinholeCameraCal3Bundler, Point3, \
Point2Vector, Pose3Vector, triangulatePoint3, \
CameraSetCal3_S2, CameraSetCal3Bundler
class TestVisualISAMExample(GtsamTestCase): class TestVisualISAMExample(GtsamTestCase):
def setUp(self): def setUp(self):
@ -80,62 +82,62 @@ class TestVisualISAMExample(GtsamTestCase):
# self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9) # self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
def test_distinct_Ks(self): def test_distinct_Ks(self):
# two cameras
K1 = Cal3_S2(1500, 1200, 0, 640, 480) K1 = Cal3_S2(1500, 1200, 0, 640, 480)
camera1 = PinholeCameraCal3_S2(self.pose1, K1) camera1 = PinholeCameraCal3_S2(self.pose1, K1)
K2 = Cal3_S2(1600, 1300, 0, 650, 440) K2 = Cal3_S2(1600, 1300, 0, 650, 440)
camera2 = PinholeCameraCal3_S2(self.pose2, K2) camera2 = PinholeCameraCal3_S2(self.pose2, K2)
cameras = CameraSetCal3_S2()
cameras.append(camera1)
cameras.append(camera2)
# landmark ~5 meters infront of camera # landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2) landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate # 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark) z1 = camera1.project(landmark)
z2 = camera2.project(landmark) z2 = camera2.project(landmark)
# two cameras
measurements = Point2Vector()
cameras = CameraSetCal3_S2()
measurements = Point2Vector()
measurements.append(z1) measurements.append(z1)
measurements.append(z2) measurements.append(z2)
cameras.append(camera1)
cameras.append(camera2)
optimize = True optimize = True
rank_tol = 1e-9 rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9)
def test_distinct_Ks_Bundler(self): def test_distinct_Ks_Bundler(self):
# two cameras
K1 = Cal3Bundler(1500, 0, 0, 640, 480) K1 = Cal3Bundler(1500, 0, 0, 640, 480)
camera1 = PinholeCameraCal3Bundler(self.pose1, K1) camera1 = PinholeCameraCal3Bundler(self.pose1, K1)
K2 = Cal3Bundler(1500, 0, 0, 640, 480) K2 = Cal3Bundler(1600, 0, 0, 650, 440)
camera2 = PinholeCameraCal3Bundler(self.pose2, K2) camera2 = PinholeCameraCal3Bundler(self.pose2, K2)
cameras = CameraSetCal3Bundler()
cameras.append(camera1)
cameras.append(camera2)
# landmark ~5 meters infront of camera # landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2) landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate # 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark) z1 = camera1.project(landmark)
z2 = camera2.project(landmark) z2 = camera2.project(landmark)
# two cameras
measurements = Point2Vector()
cameras = CameraSetCal3Bundler()
measurements = Point2Vector()
measurements.append(z1) measurements.append(z1)
measurements.append(z2) measurements.append(z2)
cameras.append(camera1)
cameras.append(camera2)
optimize = True optimize = True
rank_tol = 1e-9 rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize) triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2) self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-9)
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()