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

fixed DLT to constant sized matrices

release/4.3a0
Natesh Srinivasan 2014-12-03 18:23:08 -05:00
parent 30b77d73e7
commit cabf17f294
2 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/triangulation.cpp
View File

@ -30,7 +30,7 @@ namespace gtsam {
* @param rank_tol SVD rank tolerance
* @return Triangulated Point3
*/
Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) {
// number of cameras
@ -41,7 +41,7 @@ Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
for (size_t i = 0; i < m; i++) {
size_t row = i * 2;
const Matrix& projection = projection_matrices.at(i);
const Matrix34& projection = projection_matrices.at(i);
const Point2& p = measurements.at(i);
// build system of equations

18
gtsam/geometry/triangulation.h
View File

@ -53,7 +53,7 @@ public:
* @return Triangulated Point3
*/
GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix>& projection_matrices, // TODO: Use the fact that projection matrices sizes are known at compile time
const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol);
///
@ -189,12 +189,11 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration
std::vector<Matrix> projection_matrices;
std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const Pose3& pose, poses) {
projection_matrices.push_back(
sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4));
sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0));
}
// Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -240,12 +239,11 @@ Point3 triangulatePoint3(
// construct projection matrices from poses & calibration
typedef PinholeCamera<CALIBRATION> Camera;
std::vector<Matrix> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras)
projection_matrices.push_back(
camera.calibration().K()
* sub(camera.pose().inverse().matrix(), 0, 3, 0, 4));
std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras) {
Matrix P_ = (camera.pose().inverse().matrix());
projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) );
}
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization