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

nice cleanup to triangulation, moved get camera matrix to cameras, to generalize to other cameras

release/4.3a0
lcarlone 2021-08-27 15:57:27 -04:00
parent e1c5b50934
commit 01c0b281b6
5 changed files with 60 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/PinholeCamera.h
View File

@ -312,6 +312,12 @@ public:
return range(camera.pose(), Dcamera, Dother); return range(camera.pose(), Dcamera, Dother);
} }
/// for Linear Triangulation
Matrix34 getCameraProjectionMatrix() const {
Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
return K_.K() * P;
}
private: private:
/** Serialization function */ /** Serialization function */

7
gtsam/geometry/PinholePose.h
View File

@ -166,7 +166,6 @@ public:
return result; return result;
} }
/// backproject a 2-dimensional point to a 3-dimensional point at infinity /// backproject a 2-dimensional point to a 3-dimensional point at infinity
Unit3 backprojectPointAtInfinity(const Point2& p) const { Unit3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p); const Point2 pn = calibration().calibrate(p);
@ -417,6 +416,12 @@ public:
return PinholePose(); // assumes that the default constructor is valid return PinholePose(); // assumes that the default constructor is valid
} }
/// for Linear Triangulation
Matrix34 getCameraProjectionMatrix() const {
Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
return K_->K() * P;
}
/// @} /// @}
private: private:

5
gtsam/geometry/SphericalCamera.h
View File

@ -210,6 +210,11 @@ class GTSAM_EXPORT SphericalCamera {
return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid return SphericalCamera(Pose3::identity()); // assumes that the default constructor is valid
} }
/// for Linear Triangulation
Matrix34 getCameraProjectionMatrix() const {
return Matrix::Zero(3,4);
}
private: private:
/** Serialization function */ /** Serialization function */

49
gtsam/geometry/triangulation.h
View File

@ -180,26 +180,26 @@ Point3 triangulateNonlinear(
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
/** template<class CAMERA>
* Create a 3*4 camera projection matrix from calibration and pose. std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMatrices(const CameraSet<CAMERA>& cameras) {
* Functor for partial application on calibration std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
* @param pose The camera pose for(const CAMERA& camera: cameras){
* @param cal The calibration projection_matrices.push_back(camera.getCameraProjectionMatrix());
* @return Returns a Matrix34 }
*/ return projection_matrices;
}
// overload, assuming pinholePose
template<class CALIBRATION> template<class CALIBRATION>
struct CameraProjectionMatrix { std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMatrices(
CameraProjectionMatrix(const CALIBRATION& calibration) : const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal) {
K_(calibration.K()) { std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (size_t i = 0; i < poses.size(); i++) {
PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
projection_matrices.push_back(camera.getCameraProjectionMatrix());
} }
Matrix34 operator()(const Pose3& pose) const { return projection_matrices;
return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); }
}
private:
const Matrix3 K_;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/** /**
* Function to triangulate 3D landmark point from an arbitrary number * Function to triangulate 3D landmark point from an arbitrary number
@ -224,10 +224,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices =
CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply getCameraProjectionMatrices< CALIBRATION >(poses, sharedCal);
for(const Pose3& pose: poses)
projection_matrices.push_back(createP(pose));
// Triangulate linearly // Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -274,11 +272,8 @@ Point3 triangulatePoint3(
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices =
for(const CAMERA& camera: cameras) getCameraProjectionMatrices<CAMERA>(cameras);
projection_matrices.push_back(
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
camera.pose()));
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization

42
gtsam/slam/tests/testSmartProjectionFactorP.cpp
View File

@ -1089,27 +1089,27 @@ TEST( SmartProjectionFactorP, timing ) {
/* *************************************************************************/ /* *************************************************************************/
TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) { TEST( SmartProjectionFactorP, optimization_3poses_sphericalCamera ) {
using namespace sphericalCamera; // using namespace sphericalCamera;
Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3;
//
// Project three landmarks into three cameras // // Project three landmarks into three cameras
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1); // projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2); // projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3); // projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
//
// create inputs // // create inputs
std::vector<Key> keys; // std::vector<Key> keys;
keys.push_back(x1); // keys.push_back(x1);
keys.push_back(x2); // keys.push_back(x2);
keys.push_back(x3); // keys.push_back(x3);
//
std::vector<EmptyCal::shared_ptr> emptyKs; // std::vector<EmptyCal::shared_ptr> emptyKs;
emptyKs.push_back(emptyK); // emptyKs.push_back(emptyK);
emptyKs.push_back(emptyK); // emptyKs.push_back(emptyK);
emptyKs.push_back(emptyK); // emptyKs.push_back(emptyK);
//
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); // SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
smartFactor1->add(measurements_lmk1, keys, emptyKs); // smartFactor1->add(measurements_lmk1, keys, emptyKs);
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); // SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model));
// smartFactor2->add(measurements_lmk2, keys, sharedKs); // smartFactor2->add(measurements_lmk2, keys, sharedKs);