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

Changed API for triangulation to use vectors of pinhole cameras, or a vector of poses and a single calibration

release/4.3a0
Frank Dellaert 2013-11-06 06:31:46 +00:00
parent 9e0893d807
commit 199505db5f
2 changed files with 243 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
gtsam_unstable/geometry/tests/testTriangulation.cpp
View File

@ -28,22 +28,25 @@
#include <boost/assign.hpp> #include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/make_shared.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign; using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( triangulation, twoPosesBundler) { TEST( triangulation, twoPosesBundler) {
Cal3Bundler K(1500, 0, 0, 640, 480); boost::shared_ptr<Cal3Bundler> sharedCal = //
boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
gtsam::Point3(0, 0, 1)); gtsam::Point3(0, 0, 1));
PinholeCamera<Cal3Bundler> level_camera(level_pose, K); PinholeCamera<Cal3Bundler> level_camera(level_pose, *sharedCal);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
PinholeCamera<Cal3Bundler> level_camera_right(level_pose_right, K); PinholeCamera<Cal3Bundler> level_camera_right(level_pose_right, *sharedCal);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2); Point3 landmark(5, 0.5, 1.2);
@ -52,7 +55,7 @@ TEST( triangulation, twoPosesBundler) {
Point2 level_uv = level_camera.project(landmark); Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark);
vector<Pose3> poses; vector < Pose3 > poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose_right; poses += level_pose, level_pose_right;
@ -62,7 +65,7 @@ TEST( triangulation, twoPosesBundler) {
double rank_tol = 1e-9; double rank_tol = 1e-9;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
measurements, K, rank_tol, optimize); sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
@ -70,21 +73,23 @@ TEST( triangulation, twoPosesBundler) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
measurements, K, rank_tol, optimize); sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( triangulation, fourPoses) { TEST( triangulation, fourPoses) {
Cal3_S2 K(1500, 1200, 0, 640, 480); boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
gtsam::Point3(0, 0, 1)); gtsam::Point3(0, 0, 1));
SimpleCamera level_camera(level_pose, K); SimpleCamera level_camera(level_pose, *sharedCal);
// create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
SimpleCamera level_camera_right(level_pose_right, K); SimpleCamera level_camera_right(level_pose_right, *sharedCal);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2); Point3 landmark(5, 0.5, 1.2);
@ -93,14 +98,14 @@ TEST( triangulation, fourPoses) {
Point2 level_uv = level_camera.project(landmark); Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark);
vector<Pose3> poses; vector < Pose3 > poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose_right; poses += level_pose, level_pose_right;
measurements += level_uv, level_uv_right; measurements += level_uv, level_uv_right;
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses, boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
measurements, K); sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
@ -108,43 +113,43 @@ TEST( triangulation, fourPoses) {
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses,
measurements, K); sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 3. Add a slightly rotated third camera above, again with measurement noise
Pose3 pose_top = level_pose Pose3 pose_top = level_pose
* Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); * Pose3(Rot3::ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
SimpleCamera camera_top(pose_top, K); SimpleCamera camera_top(pose_top, *sharedCal);
Point2 top_uv = camera_top.project(landmark); Point2 top_uv = camera_top.project(landmark);
poses += pose_top; poses += pose_top;
measurements += top_uv + Point2(0.1, -0.1); measurements += top_uv + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses,
measurements, K); sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses,
measurements, K, 1e-9, true); sharedCal, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 4. Test failure: Add a 4th camera facing the wrong way
Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2), Pose3 level_pose180 = Pose3(Rot3::ypr(M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1)); Point3(0, 0, 1));
SimpleCamera camera_180(level_pose180, K); SimpleCamera camera_180(level_pose180, *sharedCal);
CHECK_EXCEPTION(camera_180.project(landmark) CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException);
;, CheiralityException);
poses += level_pose180; poses += level_pose180;
measurements += Point2(400, 400); measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationCheiralityException); TriangulationCheiralityException);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( triangulation, fourPoses_distinct_Ks) { TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
@ -164,26 +169,22 @@ TEST( triangulation, fourPoses_distinct_Ks) {
Point2 level_uv = level_camera.project(landmark); Point2 level_uv = level_camera.project(landmark);
Point2 level_uv_right = level_camera_right.project(landmark); Point2 level_uv_right = level_camera_right.project(landmark);
vector<Pose3> poses; vector<SimpleCamera> cameras;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose_right; cameras += level_camera, level_camera_right;
measurements += level_uv, level_uv_right; measurements += level_uv, level_uv_right;
std::vector<boost::shared_ptr<Cal3_S2> > Ks; boost::optional<Point3> triangulated_landmark = triangulatePoint3(cameras,
Ks.push_back(boost::make_shared<Cal3_S2>(K1)); measurements);
Ks.push_back(boost::make_shared<Cal3_S2>(K2));
boost::optional<Point3> triangulated_landmark = triangulatePoint3(poses,
measurements, Ks);
EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. 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(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> triangulated_landmark_noise = triangulatePoint3(poses, boost::optional<Point3> triangulated_landmark_noise = //
measurements, Ks); triangulatePoint3(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_landmark_noise, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 3. Add a slightly rotated third camera above, again with measurement noise
@ -193,17 +194,16 @@ TEST( triangulation, fourPoses_distinct_Ks) {
SimpleCamera camera_top(pose_top, K3); SimpleCamera camera_top(pose_top, K3);
Point2 top_uv = camera_top.project(landmark); Point2 top_uv = camera_top.project(landmark);
poses += pose_top; cameras += camera_top;
measurements += top_uv + Point2(0.1, -0.1); measurements += top_uv + Point2(0.1, -0.1);
Ks.push_back(boost::make_shared<Cal3_S2>(K3));
boost::optional<Point3> triangulated_3cameras = triangulatePoint3(poses, boost::optional<Point3> triangulated_3cameras = triangulatePoint3(cameras,
measurements, Ks); measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras,
measurements, Ks, 1e-9, true); measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 4. Test failure: Add a 4th camera facing the wrong way
@ -212,24 +212,23 @@ TEST( triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K4(700, 500, 0, 640, 480); Cal3_S2 K4(700, 500, 0, 640, 480);
SimpleCamera camera_180(level_pose180, K4); SimpleCamera camera_180(level_pose180, K4);
CHECK_EXCEPTION(camera_180.project(landmark) CHECK_EXCEPTION(camera_180.project(landmark) ;, CheiralityException);
;, CheiralityException);
poses += level_pose180; cameras += camera_180;
measurements += Point2(400, 400); measurements += Point2(400, 400);
Ks.push_back(boost::make_shared<Cal3_S2>(K4)); CHECK_EXCEPTION(triangulatePoint3(cameras, measurements),
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, Ks),
TriangulationCheiralityException); TriangulationCheiralityException);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( triangulation, twoIdenticalPoses) { TEST( triangulation, twoIdenticalPoses) {
Cal3_S2 K(1500, 1200, 0, 640, 480); boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
gtsam::Point3(0, 0, 1)); gtsam::Point3(0, 0, 1));
SimpleCamera level_camera(level_pose, K); SimpleCamera level_camera(level_pose, *sharedCal);
// landmark ~5 meters infront of camera // landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2); Point3 landmark(5, 0.5, 1.2);
@ -237,34 +236,35 @@ TEST( triangulation, twoIdenticalPoses) {
// 1. Project two landmarks into two cameras and triangulate // 1. Project two landmarks into two cameras and triangulate
Point2 level_uv = level_camera.project(landmark); Point2 level_uv = level_camera.project(landmark);
vector<Pose3> poses; vector < Pose3 > poses;
vector<Point2> measurements; vector<Point2> measurements;
poses += level_pose, level_pose; poses += level_pose, level_pose;
measurements += level_uv, level_uv; measurements += level_uv, level_uv;
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);
} }
/* ************************************************************************* */ /* ************************************************************************* *
TEST( triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation
Cal3_S2 K(1500, 1200, 0, 640, 480); TEST( triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation
vector<Pose3> poses; Cal3_S2 *sharedCal(1500, 1200, 0, 640, 480);
vector<Point2> measurements;
poses += Pose3(); vector<Pose3> poses;
measurements += Point2(); vector<Point2> measurements;
CHECK_EXCEPTION(triangulatePoint3(poses, measurements, K), poses += Pose3();
TriangulationUnderconstrainedException); measurements += Point2();
}
/* ************************************************************************* */ CHECK_EXCEPTION(triangulatePoint3(poses, measurements, *sharedCal),
TriangulationUnderconstrainedException);
}
/* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);

292
gtsam_unstable/geometry/triangulation.h
View File

@ -18,21 +18,21 @@
#pragma once #pragma once
#include <vector>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam_unstable/base/dllexport.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign.hpp> #include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/slam/PriorFactor.h> #include <vector>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/ProjectionFactor.h>
namespace gtsam { namespace gtsam {
@ -53,24 +53,15 @@ public:
} }
}; };
/* ************************************************************************* */
// See Hartley and Zisserman, 2nd Ed., page 312
/** /**
* * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
* @param poses Camera poses
* @param projection_matrices Projection matrices (K*P^-1) * @param projection_matrices Projection matrices (K*P^-1)
* @param measurements 2D measurements * @param measurements 2D measurements
* @param Ks vector of calibrations
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
* @param Flag to turn on nonlinear refinement of triangulation
* @return Triangulated Point3 * @return Triangulated Point3
*/ */
template<class CALIBRATION> Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
Point3 triangulateDLT(const std::vector<Pose3>& poses, const std::vector<Point2>& measurements, double rank_tol) {
const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements,
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol,
bool optimize) {
// number of cameras // number of cameras
size_t m = projection_matrices.size(); size_t m = projection_matrices.size();
@ -97,55 +88,143 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// Create 3D point from eigenvector // Create 3D point from eigenvector
Point3 point = Point3(sub((v / v(3)), 0, 3)); return Point3(sub((v / v(3)), 0, 3));
}
if (optimize) { // Frank says: putting priors on poses and then optimizing is a terrible idea: we turn a 3dof problem into a much more difficult problem
// Create a factor graph // We should have a projectionfactor that knows pose is fixed
NonlinearFactorGraph graph;
gtsam::Values values;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
// Initial landmark value ///
Key landmarkKey = Symbol('p', 0); /**
values.insert(landmarkKey, point); * Create a factor graph with projection factors from poses and one calibration
* @param poses Camera poses
// Create all projection factors, as well as priors on poses * @param sharedCal shared pointer to single calibration object
Key i = 0; * @param measurements 2D measurements
BOOST_FOREACH(const Point2 &z_i, measurements) { * @param landmarkKey to refer to landmark
// Factor for pose i * @param initialEstimate
typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> ProjectionFactor; * @return graph and initial values
ProjectionFactor projectionFactor(z_i, unit2, i, landmarkKey, Ks[i]); */
graph.push_back(projectionFactor); template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
// Prior on pose const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
// Frank says: this is a terrible idea: we turn a 3dof problem into a much more difficult problem const std::vector<Point2>& measurements, Key landmarkKey,
typedef PriorFactor<Pose3> Pose3Prior; const Point3& initialEstimate) {
graph.push_back(Pose3Prior(i, poses[i], prior_model)); Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
// Initial pose values NonlinearFactorGraph graph;
values.insert(i, poses[i]); static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
i++; static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
} for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i];
// Optimize graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> //
LevenbergMarquardtParams params; (measurements[i], unit2, i, landmarkKey, sharedCal));
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model));
params.verbosity = NonlinearOptimizerParams::ERROR; values.insert(i, pose_i);
params.lambdaInitial = 1;
params.lambdaFactor = 10;
params.maxIterations = 100;
params.absoluteErrorTol = 1.0;
params.verbosityLM = LevenbergMarquardtParams::SILENT;
params.verbosity = NonlinearOptimizerParams::SILENT;
params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
Values result = optimizer.optimize();
point = result.at<Point3>(landmarkKey);
} }
return std::make_pair(graph, values);
}
return point; /**
* Create a factor graph with projection factors from pinhole cameras
* (each camera has a pose and calibration)
* @param cameras pinhole cameras
* @param measurements 2D measurements
* @param landmarkKey to refer to landmark
* @param initialEstimate
* @return graph and initial values
*/
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey,
const Point3& initialEstimate) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) {
const PinholeCamera<CALIBRATION>& camera_i = cameras[i];
boost::shared_ptr<CALIBRATION> // Seems wasteful to create new object
sharedCal(new CALIBRATION(camera_i.calibration()));
graph.push_back(GenericProjectionFactor<Pose3, Point3, CALIBRATION> //
(measurements[i], unit2, i, landmarkKey, sharedCal));
const Pose3& pose_i = camera_i.pose();
graph.push_back(PriorFactor<Pose3>(i, pose_i, prior_model));
values.insert(i, pose_i);
}
return std::make_pair(graph, values);
}
///
/**
* Optimize for triangulation
* @param graph nonlinear factors for projection
* @param values initial values
* @param landmarkKey to refer to landmark
* @return refined Point3
*/
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
Key landmarkKey) {
// Maybe we should consider Gauss-Newton?
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
params.lambdaInitial = 1;
params.lambdaFactor = 10;
params.maxIterations = 100;
params.absoluteErrorTol = 1.0;
params.verbosityLM = LevenbergMarquardtParams::SILENT;
params.verbosity = NonlinearOptimizerParams::SILENT;
params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
Values result = optimizer.optimize();
return result.at<Point3>(landmarkKey);
}
/**
* Given an initial estimate , refine a point using measurements in several cameras
* @param poses Camera poses
* @param sharedCal shared pointer to single calibration object
* @param measurements 2D measurements
* @param initialEstimate
* @return refined Point3
*/
template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements,
Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0));
}
/**
* Given an initial estimate , refine a point using measurements in several cameras
* @param cameras pinhole cameras
* @param measurements 2D measurements
* @param initialEstimate
* @return refined Point3
*/
template<class CALIBRATION>
Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values
Values values;
NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph(cameras, measurements,
Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0));
} }
/** /**
@ -154,49 +233,46 @@ Point3 triangulateDLT(const std::vector<Pose3>& poses,
* resulting point lies in front of all cameras, but has no other checks * resulting point lies in front of all cameras, but has no other checks
* to verify the quality of the triangulation. * to verify the quality of the triangulation.
* @param poses A vector of camera poses * @param poses A vector of camera poses
* @param sharedCal shared pointer to single calibration object
* @param measurements A vector of camera measurements * @param measurements A vector of camera measurements
* @param K The camera calibration (Same for all cameras involved)
* @param rank tolerance, default 1e-9 * @param rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation * @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3 on success, boost::none otherwise. * @return Returns a Point3
*/ */
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, const CALIBRATION& K, boost::shared_ptr<CALIBRATION> sharedCal,
double rank_tol = 1e-9, bool optimize = false) { const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) {
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
if (poses.size() < 2) if (poses.size() < 2)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
std::vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix> projection_matrices;
BOOST_FOREACH(const Pose3& pose, poses) { BOOST_FOREACH(const Pose3& pose, poses) {
projection_matrices.push_back( projection_matrices.push_back(
K.K() * sub(pose.inverse().matrix(), 0, 3, 0, 4)); sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4));
// std::cout << "Calibration i \n" << K.K() << std::endl;
// std::cout << "rank_tol i \n" << rank_tol << std::endl;
} }
// create vector with shared pointer to calibration (all the same in this case) // Triangulate linearly
boost::shared_ptr<CALIBRATION> sharedK = boost::make_shared<CALIBRATION>(K); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
std::vector<boost::shared_ptr<CALIBRATION> > Ks(poses.size(), sharedK);
Point3 triangulated_point = triangulateDLT(poses, projection_matrices, // The n refine using non-linear optimization
measurements, Ks, rank_tol, optimize); if (optimize)
point = triangulateNonlinear(poses, sharedCal, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies infront of all cameras // verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) { BOOST_FOREACH(const Pose3& pose, poses) {
const Point3& p_local = pose.transform_to(triangulated_point); const Point3& p_local = pose.transform_to(point);
if(p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
} }
#endif #endif
return triangulated_point; return point;
} }
/** /**
@ -205,46 +281,48 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
* above, except that each camera has its own calibration. The function * above, except that each camera has its own calibration. The function
* checks that the resulting point lies in front of all cameras, but has * checks that the resulting point lies in front of all cameras, but has
* no other checks to verify the quality of the triangulation. * no other checks to verify the quality of the triangulation.
* @param poses A vector of camera poses * @param cameras pinhole cameras
* @param measurements A vector of camera measurements * @param measurements A vector of camera measurements
* @param Ks Vector of camera calibrations
* @param rank tolerance, default 1e-9 * @param rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation * @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3 on success, boost::none otherwise. * @return Returns a Point3
*/ */
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, Point3 triangulatePoint3(
const std::vector<Point2>& measurements, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<boost::shared_ptr<CALIBRATION> >& Ks, double rank_tol = const std::vector<Point2>& measurements, double rank_tol = 1e-9,
1e-9, bool optimize = false) { bool optimize = false) {
assert(poses.size() == measurements.size()); size_t m = cameras.size();
assert(poses.size() == Ks.size()); assert(measurements.size()==m);
if (poses.size() < 2) if (m < 2)
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
std::vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
for (size_t i = 0; i < poses.size(); i++) { typedef PinholeCamera<CALIBRATION> Camera;
std::vector<Matrix> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras)
projection_matrices.push_back( projection_matrices.push_back(
Ks.at(i)->K() * sub(poses.at(i).inverse().matrix(), 0, 3, 0, 4)); camera.calibration().K()
// std::cout << "2Calibration i \n" << Ks.at(i)->K() << std::endl; * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4));
// std::cout << "2rank_tol i \n" << rank_tol << std::endl;
}
Point3 triangulated_point = triangulateDLT(poses, projection_matrices, Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
measurements, Ks, rank_tol, optimize);
// The n refine using non-linear optimization
if (optimize)
point = triangulateNonlinear(cameras, measurements, point);
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies infront of all cameras // verify that the triangulated point lies infront of all cameras
BOOST_FOREACH(const Pose3& pose, poses) { BOOST_FOREACH(const Camera& camera, cameras) {
const Point3& p_local = pose.transform_to(triangulated_point); const Point3& p_local = camera.pose().transform_to(point);
if (p_local.z() <= 0) if (p_local.z() <= 0)
throw(TriangulationCheiralityException()); throw(TriangulationCheiralityException());
} }
#endif
return triangulated_point; return point;
} }
} // \namespace gtsam } // \namespace gtsam