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

set new code to google style and fix doc 

- new code in triangulation and testTriangulation
- clean up doc number and typos
release/4.3a0
Thomas Sayre-McCord 2022-03-14 09:19:19 +01:00
parent afc406162b
commit bcf49e6243
2 changed files with 72 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -97,16 +97,17 @@ TEST(triangulation, twoPoses) {
} }
//****************************************************************************** //******************************************************************************
// Simple test with a well-behaved two camera situation with Cal3S2 calibration. // Simple test with a well-behaved two camera situation with Cal3DS2 calibration.
TEST(triangulation, twoPosesCal3S2) { TEST(triangulation, twoPosesCal3DS2) {
static const boost::shared_ptr<Cal3DS2> sharedDistortedCal = // static const boost::shared_ptr<Cal3DS2> sharedDistortedCal = //
boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003); boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001,
-0.0003);
PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal); PinholeCamera<Cal3DS2> camera1Distorted(pose1, *sharedDistortedCal);
PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal); PinholeCamera<Cal3DS2> camera2Distorted(pose2, *sharedDistortedCal);
// 1. Project two landmarks into two cameras and triangulate // 0. Project two landmarks into two cameras and triangulate
Point2 z1Distorted = camera1Distorted.project(landmark); Point2 z1Distorted = camera1Distorted.project(landmark);
Point2 z2Distorted = camera2Distorted.project(landmark); Point2 z2Distorted = camera2Distorted.project(landmark);
@ -121,13 +122,15 @@ TEST(triangulation, twoPosesCal3S2) {
// 1. Test simple DLT, perfect in no noise situation // 1. Test simple DLT, perfect in no noise situation
bool optimize = false; bool optimize = false;
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 2. test with optimization on, same answer // 2. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, // 3. Add some noise and try again: result should be ~ (4.995,
@ -136,28 +139,32 @@ TEST(triangulation, twoPosesCal3S2) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
optimize = false; optimize = false;
boost::optional<Point3> actual3 = // boost::optional<Point3> actual3 = //
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
// 4. Now with optimization on // 4. Now with optimization on
optimize = true; optimize = true;
boost::optional<Point3> actual4 = // boost::optional<Point3> actual4 = //
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
} }
//****************************************************************************** //******************************************************************************
// Simple test with a well-behaved two camera situation with Fisheye calibration. // Simple test with a well-behaved two camera situation with Fisheye
// calibration.
TEST(triangulation, twoPosesFisheye) { TEST(triangulation, twoPosesFisheye) {
using Calibration = Cal3Fisheye; using Calibration = Cal3Fisheye;
static const boost::shared_ptr<Calibration> sharedDistortedCal = // static const boost::shared_ptr<Calibration> sharedDistortedCal = //
boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003); boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1,
0.0001, -0.0003);
PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal); PinholeCamera<Calibration> camera1Distorted(pose1, *sharedDistortedCal);
PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal); PinholeCamera<Calibration> camera2Distorted(pose2, *sharedDistortedCal);
// 1. Project two landmarks into two cameras and triangulate // 0. Project two landmarks into two cameras and triangulate
Point2 z1Distorted = camera1Distorted.project(landmark); Point2 z1Distorted = camera1Distorted.project(landmark);
Point2 z2Distorted = camera2Distorted.project(landmark); Point2 z2Distorted = camera2Distorted.project(landmark);
@ -172,13 +179,15 @@ TEST(triangulation, twoPosesFisheye) {
// 1. Test simple DLT, perfect in no noise situation // 1. Test simple DLT, perfect in no noise situation
bool optimize = false; bool optimize = false;
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 2. test with optimization on, same answer // 2. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, // 3. Add some noise and try again: result should be ~ (4.995,
@ -187,17 +196,18 @@ TEST(triangulation, twoPosesFisheye) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
optimize = false; optimize = false;
boost::optional<Point3> actual3 = // boost::optional<Point3> actual3 = //
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
// 4. Now with optimization on // 4. Now with optimization on
optimize = true; optimize = true;
boost::optional<Point3> actual4 = // boost::optional<Point3> actual4 = //
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize); triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements,
rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
} }
//****************************************************************************** //******************************************************************************
// Similar, but now with Bundler calibration // Similar, but now with Bundler calibration
TEST(triangulation, twoPosesBundler) { TEST(triangulation, twoPosesBundler) {
@ -220,7 +230,8 @@ TEST(triangulation, twoPosesBundler) {
double rank_tol = 1e-9; double rank_tol = 1e-9;
boost::optional<Point3> actual = // boost::optional<Point3> actual = //
triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmark, *actual, 1e-7)); EXPECT(assert_equal(landmark, *actual, 1e-7));
// Add some noise and try again // Add some noise and try again
@ -228,7 +239,8 @@ TEST(triangulation, twoPosesBundler) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3));
} }

58
gtsam/geometry/triangulation.h
View File

@ -227,7 +227,8 @@ std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFrom
return projection_matrices; return projection_matrices;
} }
/** Create a pinhole calibration from a different Cal3 object, removing distortion. /** Create a pinhole calibration from a different Cal3 object, removing
* distortion.
* *
* @tparam CALIBRATION Original calibration object. * @tparam CALIBRATION Original calibration object.
* @param cal Input calibration object. * @param cal Input calibration object.
@ -239,10 +240,11 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)); return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
} }
/** Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements */ /** Internal undistortMeasurement to be used by undistortMeasurement and
* undistortMeasurements */
template <class CALIBRATION, class MEASUREMENT> template <class CALIBRATION, class MEASUREMENT>
MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, MEASUREMENT undistortMeasurementInternal(
const MEASUREMENT& measurement, const CALIBRATION& cal, const MEASUREMENT& measurement,
boost::optional<Cal3_S2> pinholeCal = boost::none) { boost::optional<Cal3_S2> pinholeCal = boost::none) {
if (!pinholeCal) { if (!pinholeCal) {
pinholeCal = createPinholeCalibration(cal); pinholeCal = createPinholeCalibration(cal);
@ -250,10 +252,12 @@ MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
return pinholeCal->uncalibrate(cal.calibrate(measurement)); return pinholeCal->uncalibrate(cal.calibrate(measurement));
} }
/** Remove distortion for measurements so as if the measurements came from a pinhole camera. /** Remove distortion for measurements so as if the measurements came from a
* pinhole camera.
* *
* Removes distortion but maintains the K matrix of the initial cal. Operates by calibrating using * Removes distortion but maintains the K matrix of the initial cal. Operates by
* full calibration and uncalibrating with only the pinhole component of the calibration. * calibrating using full calibration and uncalibrating with only the pinhole
* component of the calibration.
* @tparam CALIBRATION Calibration type to use. * @tparam CALIBRATION Calibration type to use.
* @param cal Calibration with which measurements were taken. * @param cal Calibration with which measurements were taken.
* @param measurements Vector of measurements to undistort. * @param measurements Vector of measurements to undistort.
@ -264,10 +268,13 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal,
const Point2Vector& measurements) { const Point2Vector& measurements) {
Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
Point2Vector undistortedMeasurements; Point2Vector undistortedMeasurements;
// Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. // Calibrate with cal and uncalibrate with pinhole version of cal so that
std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), // measurements are undistorted.
std::transform(measurements.begin(), measurements.end(),
std::back_inserter(undistortedMeasurements),
[&cal, &pinholeCalibration](const Point2& measurement) { [&cal, &pinholeCalibration](const Point2& measurement) {
return undistortMeasurementInternal<CALIBRATION>(cal, measurement, pinholeCalibration); return undistortMeasurementInternal<CALIBRATION>(
cal, measurement, pinholeCalibration);
}); });
return undistortedMeasurements; return undistortedMeasurements;
} }
@ -279,27 +286,30 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
return measurements; return measurements;
} }
/** Remove distortion for measurements so as if the measurements came from a
/** Remove distortion for measurements so as if the measurements came from a pinhole camera. * pinhole camera.
* *
* Removes distortion but maintains the K matrix of the initial calibrations. Operates by calibrating using * Removes distortion but maintains the K matrix of the initial calibrations.
* full calibration and uncalibrating with only the pinhole component of the calibration. * Operates by calibrating using full calibration and uncalibrating with only
* the pinhole component of the calibration.
* @tparam CAMERA Camera type to use. * @tparam CAMERA Camera type to use.
* @param cameras Cameras corresponding to each measurement. * @param cameras Cameras corresponding to each measurement.
* @param measurements Vector of measurements to undistort. * @param measurements Vector of measurements to undistort.
* @return measurements with the effect of the distortion of the camera removed. * @return measurements with the effect of the distortion of the camera removed.
*/ */
template <class CAMERA> template <class CAMERA>
typename CAMERA::MeasurementVector undistortMeasurements(const CameraSet<CAMERA>& cameras, typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements) { const typename CAMERA::MeasurementVector& measurements) {
const size_t num_meas = cameras.size(); const size_t num_meas = cameras.size();
assert(num_meas == measurements.size()); assert(num_meas == measurements.size());
typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
for (size_t ii = 0; ii < num_meas; ++ii) { for (size_t ii = 0; ii < num_meas; ++ii) {
// Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted. // Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted.
undistortedMeasurements[ii] = undistortedMeasurements[ii] =
undistortMeasurementInternal<typename CAMERA::CalibrationType>(cameras[ii].calibration(), measurements[ii]); undistortMeasurementInternal<typename CAMERA::CalibrationType>(
cameras[ii].calibration(), measurements[ii]);
} }
return undistortedMeasurements; return undistortedMeasurements;
} }
@ -347,10 +357,12 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
// Undistort the measurements, leaving only the pinhole elements in effect. // Undistort the measurements, leaving only the pinhole elements in effect.
auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(*sharedCal, measurements); auto undistortedMeasurements =
undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
// Triangulate linearly // Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); Point3 point =
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
// Then refine using non-linear optimization // Then refine using non-linear optimization
if (optimize) if (optimize)
@ -398,9 +410,11 @@ Point3 triangulatePoint3(
auto projection_matrices = projectionMatricesFromCameras(cameras); auto projection_matrices = projectionMatricesFromCameras(cameras);
// Undistort the measurements, leaving only the pinhole elements in effect. // Undistort the measurements, leaving only the pinhole elements in effect.
auto undistortedMeasurements = undistortMeasurements<CAMERA>(cameras, measurements); auto undistortedMeasurements =
undistortMeasurements<CAMERA>(cameras, measurements);
Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); Point3 point =
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization
if (optimize) if (optimize)