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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

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

30
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

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

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