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

Merge pull request #615 from borglab/feature/wrap_camVector 

vector of cameras and triangulation function wrapped
release/4.3a0
Sushmita Warrier 2020-12-05 21:51:36 -05:00 committed by GitHub
parent 0527a83674 3da2885831
commit e411568ccf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 126 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/geometry/triangulation.h
View File

@ -18,8 +18,10 @@
#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/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -494,5 +496,9 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
}
}
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
} // \namespace gtsam

17
gtsam/gtsam.i
View File

@ -1067,6 +1067,15 @@ typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
template<T>
class CameraSet {
CameraSet();
// structure specific methods
T at(size_t i) const;
void push_back(const T& cam);
};
#include <gtsam/geometry/StereoCamera.h>
class StereoCamera {
// Standard Constructors and Named Constructors
@ -1098,7 +1107,7 @@ class StereoCamera {
#include <gtsam/geometry/triangulation.h>
// Templates appear not yet supported for free functions
// Templates appear not yet supported for free functions - issue raised at borglab/wrap#14 to add support
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
@ -1108,6 +1117,12 @@ 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

@ -82,6 +82,8 @@ set(ignore
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsUnit3
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::KeyPairDoubleMap)
pybind_wrap(gtsam_unstable_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");

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

@ -12,69 +12,123 @@ import unittest
import numpy as np
import gtsam as g
import gtsam
from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
CameraSetCal3Bundler, PinholeCameraCal3_S2,
PinholeCameraCal3Bundler, Point2Vector, Point3, Pose3,
Pose3Vector, Rot3)
from gtsam.utils.test_case import GtsamTestCase
from gtsam import Cal3_S2, Cal3Bundler, Rot3, Pose3, \
PinholeCameraCal3_S2, Point3, Point2Vector, Pose3Vector, triangulatePoint3
class TestVisualISAMExample(GtsamTestCase):
def test_TriangulationExample(self):
# Some common constants
sharedCal = Cal3_S2(1500, 1200, 0, 640, 480)
""" Tests for triangulation with shared and individual calibrations """
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)
# 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)
# twoPoses
self.poses = Pose3Vector()
self.poses.append(pose1)
self.poses.append(pose2)
# landmark ~5 meters infront of camera
landmark = Point3(5, 0.5, 1.2)
self.landmark = Point3(5, 0.5, 1.2)
# 1. Project two landmarks into two cameras and triangulate
z1 = camera1.project(landmark)
z2 = camera2.project(landmark)
def generate_measurements(self, calibration, camera_model, cal_params, camera_set=None):
"""
Generate vector of measurements for given calibration and camera model.
# twoPoses
poses = Pose3Vector()
Args:
calibration: Camera calibration e.g. Cal3_S2
camera_model: Camera model e.g. PinholeCameraCal3_S2
cal_params: Iterable of camera parameters for `calibration` e.g. [K1, K2]
camera_set: Cameraset object (for individual calibrations)
Returns:
list of measurements and list/CameraSet object for cameras
"""
if camera_set is not None:
cameras = camera_set()
else:
cameras = []
measurements = Point2Vector()
poses.append(pose1)
poses.append(pose2)
measurements.append(z1)
measurements.append(z2)
for k, pose in zip(cal_params, self.poses):
K = calibration(*k)
camera = camera_model(pose, K)
cameras.append(camera)
z = camera.project(self.landmark)
measurements.append(z)
optimize = True
rank_tol = 1e-9
return measurements, cameras
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
def test_TriangulationExample(self):
""" Tests triangulation with shared Cal3_S2 calibration"""
# Some common constants
sharedCal = (1500, 1200, 0, 640, 480)
# 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements = Point2Vector()
measurements.append(z1 - np.array([0.1, 0.5]))
measurements.append(z2 - np.array([-0.2, 0.3]))
measurements, _ = self.generate_measurements(Cal3_S2,
PinholeCameraCal3_S2,
(sharedCal, sharedCal))
triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal),
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
# Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements_noisy = Point2Vector()
measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
triangulated_landmark = gtsam.triangulatePoint3(self.poses,
Cal3_S2(sharedCal),
measurements_noisy,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
def test_distinct_Ks(self):
""" Tests triangulation with individual Cal3_S2 calibrations """
# two camera parameters
K1 = (1500, 1200, 0, 640, 480)
K2 = (1600, 1300, 0, 650, 440)
measurements, cameras = self.generate_measurements(Cal3_S2,
PinholeCameraCal3_S2,
(K1, K2),
camera_set=CameraSetCal3_S2)
triangulated_landmark = gtsam.triangulatePoint3(cameras,
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
def test_distinct_Ks_Bundler(self):
""" Tests triangulation with individual Cal3Bundler calibrations"""
# two camera parameters
K1 = (1500, 0, 0, 640, 480)
K2 = (1600, 0, 0, 650, 440)
measurements, cameras = self.generate_measurements(Cal3Bundler,
PinholeCameraCal3Bundler,
(K1, K2),
camera_set=CameraSetCal3Bundler)
triangulated_landmark = gtsam.triangulatePoint3(cameras,
measurements,
rank_tol=1e-9,
optimize=True)
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
triangulated_landmark = triangulatePoint3(poses,sharedCal, measurements, rank_tol, optimize)
self.gtsamAssertEquals(landmark, triangulated_landmark,1e-2)
#
# # two Poses with Bundler Calibration
# bundlerCal = Cal3Bundler(1500, 0, 0, 640, 480)
# camera1 = PinholeCameraCal3Bundler(pose1, bundlerCal)
# camera2 = PinholeCameraCal3Bundler(pose2, bundlerCal)
#
# z1 = camera1.project(landmark)
# z2 = camera2.project(landmark)
#
# measurements = Point2Vector()
# measurements.append(z1)
# measurements.append(z2)
#
# triangulated_landmark = triangulatePoint3(poses,bundlerCal, measurements, rank_tol, optimize)
# self.gtsamAssertEquals(landmark, triangulated_landmark,1e-9)
if __name__ == "__main__":
unittest.main()