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

vector of cameras and triangulation function wrapped

release/4.3a0
Sushmita 2020-11-27 00:14:52 -05:00
parent 627c015727
commit 02e94730a6
6 changed files with 116 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/triangulation.h
View File

@ -24,6 +24,8 @@
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam {
@ -494,5 +496,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
}
}
typedef CameraSet<PinholeCamera<Cal3Bundler>> CameraSetCal3Bundler;
typedef CameraSet<PinholeCamera<Cal3_S2>> CameraSetCal3_S2;
} // \namespace gtsam

34
gtsam/gtsam.i
View File

@ -1108,6 +1108,32 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
class CameraSetCal3Bundler {
CameraSetCal3Bundler();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::PinholeCameraCal3Bundler at(size_t i) const;
void push_back(gtsam::PinholeCameraCal3Bundler& cam) const;
};
class CameraSetCal3_S2 {
CameraSetCal3_S2();
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
gtsam::PinholeCameraCal3_S2 at(size_t i) const;
void push_back(gtsam::PinholeCameraCal3_S2& cam) const;
};
#include <gtsam/geometry/StereoCamera.h>
class StereoCamera {
// Standard Constructors and Named Constructors
@ -1149,7 +1175,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);
//*************************************************************************
// Symbolic
//*************************************************************************

2
python/CMakeLists.txt
View File

@ -38,6 +38,8 @@ set(ignore
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::BinaryMeasurementsUnit3
gtsam::CameraSetCal3Bundler
gtsam::CameraSetCal3_S2
gtsam::KeyPairDoubleMap)
pybind_wrap(gtsam_py # target

2
python/gtsam/preamble.h
View File

@ -10,3 +10,5 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> >);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> >);

2
python/gtsam/specializations.h
View File

@ -13,3 +13,5 @@ py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "Bina
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");

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

@ -15,21 +15,24 @@ import numpy as np
import gtsam as g
from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3, CameraSetCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3Bundler
class TestVisualISAMExample(GtsamTestCase):
def test_TriangulationExample(self):
# Some common constants
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
def setUp(self):
# Set up two camera poses
# Looking along X-axis, 1 meter above ground plane (x-y)
upright = Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2)
pose1 = Pose3(upright, Point3(0, 0, 1))
camera1 = PinholeCameraCal3_S2(pose1, sharedCal)
self.pose1 = Pose3(upright, Point3(0, 0, 1))
# create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
camera2 = PinholeCameraCal3_S2(pose2, sharedCal)
self.pose2 = self.pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
def test_TriangulationExample(self):
# Some common constants
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
camera1 = PinholeCameraCal3_S2(self.pose1, sharedCal)
camera2 = PinholeCameraCal3_S2(self.pose2, sharedCal)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
@ -42,8 +45,8 @@ class TestVisualISAMExample(GtsamTestCase):
poses = Pose3Vector()
measurements = Point2Vector()
poses.append(pose1)
poses.append(pose2)
poses.append(self.pose1)
poses.append(self.pose2)
measurements.append(z1)
measurements.append(z2)
@ -76,5 +79,63 @@ class TestVisualISAMExample(GtsamTestCase):
# triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
# self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
def test_distinct_Ks(self):
K1 = Cal3_S2(1500, 1200, 0, 640, 480)
camera1 = PinholeCameraCal3_S2(self.pose1, K1)
K2 = Cal3_S2(1600, 1300, 0, 650, 440)
camera2 = PinholeCameraCal3_S2(self.pose2, K2)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark)
z2 = camera2.project(landmark)
# two cameras
measurements = Point2Vector()
cameras = CameraSetCal3_S2()
measurements.append(z1)
measurements.append(z2)
cameras.append(camera1)
cameras.append(camera2)
optimize = True
rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2)
def test_distinct_Ks_Bundler(self):
K1 = Cal3Bundler(1500, 0, 0, 640, 480)
camera1 = PinholeCameraCal3Bundler(self.pose1, K1)
K2 = Cal3Bundler(1500, 0, 0, 640, 480)
camera2 = PinholeCameraCal3Bundler(self.pose2, K2)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark)
z2 = camera2.project(landmark)
# two cameras
measurements = Point2Vector()
cameras = CameraSetCal3Bundler()
measurements.append(z1)
measurements.append(z2)
cameras.append(camera1)
cameras.append(camera2)
optimize = True
rank_tol = 1e-9
triangulated_landmark = triangulatePoint3(cameras, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark, 1e-2)
if __name__ == "__main__":
unittest.main()