remove calibration template from SmartStereoProjectionPoseFactor

release/4.3a0
cbeall3 2015-07-28 15:12:02 -04:00
parent 748877ff7e
commit fd1e41a9e6
3 changed files with 54 additions and 56 deletions

View File

@ -332,7 +332,7 @@ public:
}
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<typename Base::MatrixZD> Fblocks;
std::vector<Base::MatrixZD> Fblocks;
Matrix F, E;
Vector b;
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);

View File

@ -38,12 +38,11 @@ namespace gtsam {
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
protected:
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
@ -53,7 +52,7 @@ public:
typedef SmartStereoProjectionFactor Base;
/// shorthand for this class
typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
typedef SmartStereoProjectionPoseFactor This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
@ -83,7 +82,7 @@ public:
*/
void add(const StereoPoint2 measured_i, const Key poseKey_i,
const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
const boost::shared_ptr<Cal3_S2Stereo> K_i) {
Base::add(measured_i, poseKey_i, noise_i);
K_all_.push_back(K_i);
}
@ -97,7 +96,7 @@ public:
*/
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys, noises);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
@ -112,7 +111,7 @@ public:
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
const SharedNoiseModel noise, const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i), noise);
K_all_.push_back(K);
@ -127,14 +126,15 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
BOOST_FOREACH(const boost::shared_ptr<Cal3_S2Stereo>& K, K_all_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
const SmartStereoProjectionPoseFactor *e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
@ -151,7 +151,7 @@ public:
}
/** return the calibration object */
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
return K_all_;
}
@ -161,11 +161,11 @@ public:
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
Base::Cameras cameras(const Values& values) const {
Base::Cameras cameras;
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
const Pose3& pose = values.at<Pose3>(k);
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
@ -185,9 +185,9 @@ private:
}; // end of class declaration
/// traits
template<class CALIBRATION>
struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionPoseFactor<CALIBRATION> > {
template<>
struct traits<SmartStereoProjectionPoseFactor> : public Testable<
SmartStereoProjectionPoseFactor> {
};
} // \ namespace gtsam

View File

@ -62,8 +62,6 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -83,32 +81,32 @@ LevenbergMarquardtParams lm_params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor2) {
SmartFactor factor1(params);
SmartStereoProjectionPoseFactor factor1(params);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor3) {
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
factor1->add(measurement1, poseKey1, model, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor4) {
SmartFactor factor1(params);
SmartStereoProjectionPoseFactor factor1(params);
factor1.add(measurement1, poseKey1, model, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Equals ) {
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
factor1->add(measurement1, poseKey1, model, K);
SmartFactor::shared_ptr factor2(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor());
factor2->add(measurement1, poseKey1, model, K);
CHECK(assert_equal(*factor1, *factor2));
@ -136,7 +134,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
values.insert(x1, level_pose);
values.insert(x2, level_pose_right);
SmartFactor factor1;
SmartStereoProjectionPoseFactor factor1;
factor1.add(level_uv, x1, model, K2);
factor1.add(level_uv_right, x2, model, K2);
@ -144,7 +142,7 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
double expectedError = 0.0;
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
SmartFactor::Cameras cameras = factor1.cameras(values);
SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
double actualError2 = factor1.totalReprojectionError(cameras);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
@ -178,13 +176,13 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
Point3(0.5, 0.1, 0.3));
values.insert(x2, level_pose_right.compose(noise_pose));
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor());
factor1->add(level_uv, x1, model, K);
factor1->add(level_uv_right, x2, model, K);
double actualError1 = factor1->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor());
vector<StereoPoint2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
@ -240,13 +238,13 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
smartFactor1->add(measurements_cam1, views, model, K2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor());
smartFactor2->add(measurements_cam2, views, model, K2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor());
smartFactor3->add(measurements_cam3, views, model, K2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -327,13 +325,13 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
SmartFactor::shared_ptr smartFactor1( new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(params));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -396,13 +394,13 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
params.setLinearizationMode(JACOBIAN_SVD);
params.setLandmarkDistanceThreshold(2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -470,20 +468,20 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
params.setDynamicOutlierRejectionThreshold(1);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(params));
smartFactor4->add(measurements_cam4, views, model, K);
// same as factor 4, but dynamic outlier rejection is off
SmartFactor::shared_ptr smartFactor4b(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor());
smartFactor4b->add(measurements_cam4, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -556,13 +554,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -683,13 +681,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
SmartStereoProjectionParams params;
params.setRankTolerance(10);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(params));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(params));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(params));
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(params));
smartFactor3->add(measurements_cam3, views, model, K);
// Create graph to optimize
@ -766,10 +764,10 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
//
// double rankTol = 50;
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor1->add(measurements_cam1, views, model, K2);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor2->add(measurements_cam2, views, model, K2);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -835,13 +833,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// double rankTol = 10;
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -900,7 +898,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// measurements_cam1.push_back(cam1_uv1);
// measurements_cam1.push_back(cam2_uv1);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor());
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
// smartFactor1->add(measurements_cam1,views, model, K2);
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
@ -941,7 +939,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor());
smartFactorInstance->add(measurements_cam1, views, model, K);
Values values;
@ -1009,7 +1007,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactor(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor());
smartFactor->add(measurements_cam1, views, model, K2);
Values values;