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);
}
/// for Linear Triangulation
Matrix34 getCameraProjectionMatrix() const {
Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
return K_.K() * P;
}
private:
/** Serialization function */

7
gtsam/geometry/PinholePose.h
View File

@ -166,7 +166,6 @@ public:
return result;
}
/// backproject a 2-dimensional point to a 3-dimensional point at infinity
Unit3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p);
@ -417,6 +416,12 @@ public:
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:

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
}
/// for Linear Triangulation
Matrix34 getCameraProjectionMatrix() const {
return Matrix::Zero(3,4);
}
private:
/** Serialization function */

49
gtsam/geometry/triangulation.h
View File

@ -180,26 +180,26 @@ Point3 triangulateNonlinear(
return optimize(graph, values, Symbol('p', 0));
}
/**
* Create a 3*4 camera projection matrix from calibration and pose.
* Functor for partial application on calibration
* @param pose The camera pose
* @param cal The calibration
* @return Returns a Matrix34
*/
template<class CAMERA>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMatrices(const CameraSet<CAMERA>& cameras) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for(const CAMERA& camera: cameras){
projection_matrices.push_back(camera.getCameraProjectionMatrix());
}
return projection_matrices;
}
// overload, assuming pinholePose
template<class CALIBRATION>
struct CameraProjectionMatrix {
CameraProjectionMatrix(const CALIBRATION& calibration) :
K_(calibration.K()) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> getCameraProjectionMatrices(
const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal) {
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 K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
}
private:
const Matrix3 K_;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
return projection_matrices;
}
/**
* Function to triangulate 3D landmark point from an arbitrary number
@ -224,10 +224,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
for(const Pose3& pose: poses)
projection_matrices.push_back(createP(pose));
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices =
getCameraProjectionMatrices< CALIBRATION >(poses, sharedCal);
// Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -274,11 +272,8 @@ Point3 triangulatePoint3(
throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for(const CAMERA& camera: cameras)
projection_matrices.push_back(
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
camera.pose()));
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices =
getCameraProjectionMatrices<CAMERA>(cameras);
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// 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 ) {
using namespace sphericalCamera;
Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3;
// Project three landmarks into three cameras
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
// create inputs
std::vector<Key> keys;
keys.push_back(x1);
keys.push_back(x2);
keys.push_back(x3);
std::vector<EmptyCal::shared_ptr> emptyKs;
emptyKs.push_back(emptyK);
emptyKs.push_back(emptyK);
emptyKs.push_back(emptyK);
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
smartFactor1->add(measurements_lmk1, keys, emptyKs);
// using namespace sphericalCamera;
// Camera::MeasurementVector measurements_lmk1, measurements_lmk2, measurements_lmk3;
//
// // Project three landmarks into three cameras
// projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark1, measurements_lmk1);
// projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark2, measurements_lmk2);
// projectToMultipleCameras<Camera>(cam1, cam2, cam3, landmark3, measurements_lmk3);
//
// // create inputs
// std::vector<Key> keys;
// keys.push_back(x1);
// keys.push_back(x2);
// keys.push_back(x3);
//
// std::vector<EmptyCal::shared_ptr> emptyKs;
// emptyKs.push_back(emptyK);
// emptyKs.push_back(emptyK);
// emptyKs.push_back(emptyK);
//
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
// smartFactor1->add(measurements_lmk1, keys, emptyKs);
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model));
// smartFactor2->add(measurements_lmk2, keys, sharedKs);