add support for second version of triangulatePoint3
- added undistort for cameras version of triangulatePoint3 - changed to 2 space indent - changed to calibration from shared calibrationrelease/4.3a0
Thomas Sayre-McCord
parent
ba32a0de85
commit
afc406162b
|
|
@ -99,102 +99,102 @@ TEST(triangulation, twoPoses) {
|
|||
//******************************************************************************
|
||||
// Simple test with a well-behaved two camera situation with Cal3S2 calibration.
|
||||
TEST(triangulation, twoPosesCal3S2) {
|
||||
static const boost::shared_ptr<Cal3DS2> sharedDistortedCal = //
|
||||
boost::make_shared<Cal3DS2>(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003);
|
||||
static const boost::shared_ptr<Cal3DS2> sharedDistortedCal = //
|
||||
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
|
||||
Point2 z1Distorted = camera1Distorted.project(landmark);
|
||||
Point2 z2Distorted = camera2Distorted.project(landmark);
|
||||
Point2 z1Distorted = camera1Distorted.project(landmark);
|
||||
Point2 z2Distorted = camera2Distorted.project(landmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += pose1, pose2;
|
||||
measurements += z1Distorted, z2Distorted;
|
||||
poses += pose1, pose2;
|
||||
measurements += z1Distorted, z2Distorted;
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual1, 1e-7));
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual1, 1e-7));
|
||||
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual2, 1e-7));
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual2, 1e-7));
|
||||
|
||||
// 3. Add some noise and try again: result should be ~ (4.995,
|
||||
// 0.499167, 1.19814)
|
||||
measurements.at(0) += Point2(0.1, 0.5);
|
||||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
|
||||
// 3. Add some noise and try again: result should be ~ (4.995,
|
||||
// 0.499167, 1.19814)
|
||||
measurements.at(0) += Point2(0.1, 0.5);
|
||||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
|
||||
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Cal3DS2>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
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.
|
||||
TEST(triangulation, twoPosesFisheye) {
|
||||
using Calibration = Cal3Fisheye;
|
||||
static const boost::shared_ptr<Calibration> sharedDistortedCal = //
|
||||
boost::make_shared<Calibration>(1500, 1200, .1, 640, 480, -.3, 0.1, 0.0001, -0.0003);
|
||||
using Calibration = Cal3Fisheye;
|
||||
static const boost::shared_ptr<Calibration> sharedDistortedCal = //
|
||||
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
|
||||
Point2 z1Distorted = camera1Distorted.project(landmark);
|
||||
Point2 z2Distorted = camera2Distorted.project(landmark);
|
||||
Point2 z1Distorted = camera1Distorted.project(landmark);
|
||||
Point2 z2Distorted = camera2Distorted.project(landmark);
|
||||
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
vector<Pose3> poses;
|
||||
Point2Vector measurements;
|
||||
|
||||
poses += pose1, pose2;
|
||||
measurements += z1Distorted, z2Distorted;
|
||||
poses += pose1, pose2;
|
||||
measurements += z1Distorted, z2Distorted;
|
||||
|
||||
double rank_tol = 1e-9;
|
||||
double rank_tol = 1e-9;
|
||||
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual1, 1e-7));
|
||||
// 1. Test simple DLT, perfect in no noise situation
|
||||
bool optimize = false;
|
||||
boost::optional<Point3> actual1 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual1, 1e-7));
|
||||
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual2, 1e-7));
|
||||
// 2. test with optimization on, same answer
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual2 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(landmark, *actual2, 1e-7));
|
||||
|
||||
// 3. Add some noise and try again: result should be ~ (4.995,
|
||||
// 0.499167, 1.19814)
|
||||
measurements.at(0) += Point2(0.1, 0.5);
|
||||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
|
||||
// 3. Add some noise and try again: result should be ~ (4.995,
|
||||
// 0.499167, 1.19814)
|
||||
measurements.at(0) += Point2(0.1, 0.5);
|
||||
measurements.at(1) += Point2(-0.2, 0.3);
|
||||
optimize = false;
|
||||
boost::optional<Point3> actual3 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3));
|
||||
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
|
||||
// 4. Now with optimization on
|
||||
optimize = true;
|
||||
boost::optional<Point3> actual4 = //
|
||||
triangulatePoint3<Calibration>(poses, sharedDistortedCal, measurements, rank_tol, optimize);
|
||||
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3));
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -439,6 +439,31 @@ TEST(triangulation, fourPoses_distinct_Ks) {
|
|||
#endif
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(triangulation, fourPoses_distinct_Ks_distortion) {
|
||||
Cal3DS2 K1(1500, 1200, 0, 640, 480, -.3, 0.1, 0.0001, -0.0003);
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
PinholeCamera<Cal3DS2> camera1(pose1, K1);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Cal3DS2 K2(1600, 1300, 0, 650, 440, -.2, 0.05, 0.0002, -0.0001);
|
||||
PinholeCamera<Cal3DS2> camera2(pose2, K2);
|
||||
|
||||
// 1. Project two landmarks into two cameras and triangulate
|
||||
Point2 z1 = camera1.project(landmark);
|
||||
Point2 z2 = camera2.project(landmark);
|
||||
|
||||
CameraSet<PinholeCamera<Cal3DS2>> cameras;
|
||||
Point2Vector measurements;
|
||||
|
||||
cameras += camera1, camera2;
|
||||
measurements += z1, z2;
|
||||
|
||||
boost::optional<Point3> actual = //
|
||||
triangulatePoint3<Cal3DS2>(cameras, measurements);
|
||||
EXPECT(assert_equal(landmark, *actual, 1e-2));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
TEST(triangulation, outliersAndFarLandmarks) {
|
||||
Cal3_S2 K1(1500, 1200, 0, 640, 480);
|
||||
|
|
|
|||
|
|
@ -227,35 +227,99 @@ std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFrom
|
|||
return projection_matrices;
|
||||
}
|
||||
|
||||
/** Create a pinhole calibration from a different Cal3 object, removing distortion.
|
||||
*
|
||||
* @tparam CALIBRATION Original calibration object.
|
||||
* @param cal Input calibration object.
|
||||
* @return Cal3_S2 with only the pinhole elements of cal.
|
||||
*/
|
||||
template <class CALIBRATION>
|
||||
Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
|
||||
const auto& K = cal.K();
|
||||
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 */
|
||||
template <class CALIBRATION, class MEASUREMENT>
|
||||
MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
|
||||
const MEASUREMENT& measurement,
|
||||
boost::optional<Cal3_S2> pinholeCal = boost::none) {
|
||||
if (!pinholeCal) {
|
||||
pinholeCal = createPinholeCalibration(cal);
|
||||
}
|
||||
return pinholeCal->uncalibrate(cal.calibrate(measurement));
|
||||
}
|
||||
|
||||
/** Remove distortion for measurements so as if the measurements came from a pinhole camera.
|
||||
*
|
||||
* Removes distortion but maintains the K matrix of the initial sharedCal. Operates by calibrating using
|
||||
* Removes distortion but maintains the K matrix of the initial cal. Operates by calibrating using
|
||||
* full calibration and uncalibrating with only the pinhole component of the calibration.
|
||||
* @tparam CALIBRATION Calibration type to use.
|
||||
* @param sharedCal Calibration with which measurements were taken.
|
||||
* @param cal Calibration with which measurements were taken.
|
||||
* @param measurements Vector of measurements to undistort.
|
||||
* @return measurements with the effect of the distortion of sharedCal removed.
|
||||
*/
|
||||
template <class CALIBRATION>
|
||||
Point2Vector undistortMeasurements(boost::shared_ptr<CALIBRATION> sharedCal, const Point2Vector& measurements) {
|
||||
const auto& K = sharedCal->K();
|
||||
Cal3_S2 pinholeCalibration(K(0,0), K(1,1), K(0,1), K(0,2), K(1,2));
|
||||
Point2Vector undistortedMeasurements;
|
||||
// Calibrate with sharedCal and uncalibrate with pinhole version of sharedCal so that measurements are undistorted.
|
||||
std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), [&sharedCal, &pinholeCalibration](auto& measurement) {
|
||||
return pinholeCalibration.uncalibrate(sharedCal->calibrate(measurement));
|
||||
});
|
||||
|
||||
return undistortedMeasurements;
|
||||
Point2Vector undistortMeasurements(const CALIBRATION& cal,
|
||||
const Point2Vector& measurements) {
|
||||
Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
|
||||
Point2Vector undistortedMeasurements;
|
||||
// Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted.
|
||||
std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements),
|
||||
[&cal, &pinholeCalibration](const Point2& measurement) {
|
||||
return undistortMeasurementInternal<CALIBRATION>(cal, measurement, pinholeCalibration);
|
||||
});
|
||||
return undistortedMeasurements;
|
||||
}
|
||||
|
||||
/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
|
||||
template <>
|
||||
Point2Vector undistortMeasurements(boost::shared_ptr<Cal3_S2> sharedCal, const Point2Vector& measurements) {
|
||||
inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
|
||||
const Point2Vector& measurements) {
|
||||
return measurements;
|
||||
}
|
||||
|
||||
|
||||
/** Remove distortion for measurements so as if the measurements came from a pinhole camera.
|
||||
*
|
||||
* Removes distortion but maintains the K matrix of the initial calibrations. Operates by calibrating using
|
||||
* full calibration and uncalibrating with only the pinhole component of the calibration.
|
||||
* @tparam CAMERA Camera type to use.
|
||||
* @param cameras Cameras corresponding to each measurement.
|
||||
* @param measurements Vector of measurements to undistort.
|
||||
* @return measurements with the effect of the distortion of the camera removed.
|
||||
*/
|
||||
template <class CAMERA>
|
||||
typename CAMERA::MeasurementVector undistortMeasurements(const CameraSet<CAMERA>& cameras,
|
||||
const typename CAMERA::MeasurementVector& measurements) {
|
||||
|
||||
const size_t num_meas = cameras.size();
|
||||
assert(num_meas == measurements.size());
|
||||
typename CAMERA::MeasurementVector undistortedMeasurements(num_meas);
|
||||
for (size_t ii = 0; ii < num_meas; ++ii) {
|
||||
// Calibrate with cal and uncalibrate with pinhole version of cal so that measurements are undistorted.
|
||||
undistortedMeasurements[ii] =
|
||||
undistortMeasurementInternal<typename CAMERA::CalibrationType>(cameras[ii].calibration(), measurements[ii]);
|
||||
}
|
||||
return undistortedMeasurements;
|
||||
}
|
||||
|
||||
/** Specialize for Cal3_S2 to do nothing. */
|
||||
template <class CAMERA = PinholeCamera<Cal3_S2>>
|
||||
inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
|
||||
const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
|
||||
const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
|
||||
return measurements;
|
||||
}
|
||||
|
||||
/** Specialize for SphericalCamera to do nothing. */
|
||||
template <class CAMERA = SphericalCamera>
|
||||
inline SphericalCamera::MeasurementVector undistortMeasurements(
|
||||
const CameraSet<SphericalCamera>& cameras,
|
||||
const SphericalCamera::MeasurementVector& measurements) {
|
||||
return measurements;
|
||||
}
|
||||
|
||||
/**
|
||||
* Function to triangulate 3D landmark point from an arbitrary number
|
||||
* of poses (at least 2) using the DLT. The function checks that the
|
||||
|
|
@ -283,7 +347,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
|
|||
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
|
||||
|
||||
// Undistort the measurements, leaving only the pinhole elements in effect.
|
||||
auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(sharedCal, measurements);
|
||||
auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
|
||||
|
||||
// Triangulate linearly
|
||||
Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
|
||||
|
|
@ -332,7 +396,11 @@ Point3 triangulatePoint3(
|
|||
|
||||
// construct projection matrices from poses & calibration
|
||||
auto projection_matrices = projectionMatricesFromCameras(cameras);
|
||||
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
|
||||
|
||||
// Undistort the measurements, leaving only the pinhole elements in effect.
|
||||
auto undistortedMeasurements = undistortMeasurements<CAMERA>(cameras, measurements);
|
||||
|
||||
Point3 point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
|
||||
|
||||
// The n refine using non-linear optimization
|
||||
if (optimize)
|
||||
|
|
|
|||
Loading…
Reference in New Issue