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

Final cleanup

release/4.3a0
Travis Driver 2023-06-06 10:01:46 -07:00
parent 8d134fd120
commit bba4b8731f
1 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/geometry/triangulation.cpp
View File

@ -25,8 +25,8 @@
namespace gtsam { namespace gtsam {
Vector4 triangulateHomogeneousDLT( Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
projection_matrices, projection_matrices,
const Point2Vector& measurements, double rank_tol) { const Point2Vector& measurements, double rank_tol) {
// number of cameras // number of cameras
size_t m = projection_matrices.size(); size_t m = projection_matrices.size();
@ -56,8 +56,8 @@ Vector4 triangulateHomogeneousDLT(
} }
Vector4 triangulateHomogeneousDLT( Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
projection_matrices, projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol) { const std::vector<Unit3>& measurements, double rank_tol) {
// number of cameras // number of cameras
size_t m = projection_matrices.size(); size_t m = projection_matrices.size();
@ -68,7 +68,7 @@ Vector4 triangulateHomogeneousDLT(
for (size_t i = 0; i < m; i++) { for (size_t i = 0; i < m; i++) {
size_t row = i * 2; size_t row = i * 2;
const Matrix34& projection = projection_matrices.at(i); const Matrix34& projection = projection_matrices.at(i);
const Point3& p = const Point3& p =
measurements.at(i) measurements.at(i)
.point3(); // to get access to x,y,z of the bearing vector .point3(); // to get access to x,y,z of the bearing vector
@ -130,7 +130,7 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
// Note: Setting q_i = 1.0 gives same results as DLT. // Note: Setting q_i = 1.0 gives same results as DLT.
const double q_i = num_i / (measurementNoise->sigma() * den_i); const double q_i = num_i / (measurementNoise->sigma() * den_i);
const Matrix23 coefficientMat = const Matrix23 coefficientMat =
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
wTi.rotation().matrix().transpose(); wTi.rotation().matrix().transpose();
@ -147,8 +147,8 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
} }
Point3 triangulateDLT( Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
projection_matrices, projection_matrices,
const Point2Vector& measurements, double rank_tol) { const Point2Vector& measurements, double rank_tol) {
Vector4 v = Vector4 v =
triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
@ -161,7 +161,7 @@ Point3 triangulateDLT(
projection_matrices, projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol) { const std::vector<Unit3>& measurements, double rank_tol) {
// contrary to previous triangulateDLT, this is now taking Unit3 inputs // contrary to previous triangulateDLT, this is now taking Unit3 inputs
Vector4 v = Vector4 v =
triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
// Create 3D point from homogeneous coordinates // Create 3D point from homogeneous coordinates
return Point3(v.head<3>() / v[3]); return Point3(v.head<3>() / v[3]);
@ -174,7 +174,7 @@ Point3 triangulateDLT(
* @param landmarkKey to refer to landmark * @param landmarkKey to refer to landmark
* @return refined Point3 * @return refined Point3
*/ */
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
Key landmarkKey) { Key landmarkKey) {
// Maybe we should consider Gauss-Newton? // Maybe we should consider Gauss-Newton?
LevenbergMarquardtParams params; LevenbergMarquardtParams params;