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

refactoring: step 1

release/4.3a0
cbeall3 2015-07-15 12:58:03 -04:00
parent ecdd4d5787
commit fb4dd81c4d
3 changed files with 21 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -33,6 +33,12 @@
namespace gtsam {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/**
* SmartStereoProjectionFactor: triangulates point
*/
@ -82,11 +88,8 @@ public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a StereoCamera // TODO: Get rid of this?
typedef StereoCamera Camera;
/// Vector of cameras
typedef CameraSet<Camera> Cameras;
typedef CameraSet<StereoCamera> Cameras;
/**
* Constructor
@ -195,7 +198,7 @@ public:
}
std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
BOOST_FOREACH(const Camera& camera, cameras) {
BOOST_FOREACH(const StereoCamera& camera, cameras) {
const Pose3& pose = camera.pose();
const Cal3_S2& K = camera.calibration()->calibration();
mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
@ -215,7 +218,7 @@ public:
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
BOOST_FOREACH(const StereoCamera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
@ -578,9 +581,6 @@ public:
}
}
/// Cameras are computed in derived class
virtual Cameras cameras(const Values& values) const = 0;
/** return the landmark */
boost::optional<Point3> point() const {
return point_;

7
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -42,10 +42,7 @@ template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
public:
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
protected:
@ -163,7 +160,7 @@ public:
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
typename Base::Camera camera(pose, K_all_[i++]);
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;

20
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -326,15 +326,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
new SmartFactor(1, -1, false, false, JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -394,17 +394,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
@ -472,22 +472,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);