diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e9a18e9ec..3981e9a84 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -332,7 +332,7 @@ public: } // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). - std::vector Fblocks; + std::vector Fblocks; Matrix F, E; Vector b; computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 50d9bb8fb..061c67a08 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -38,12 +38,11 @@ namespace gtsam { * The calibration is known here. The factor only constraints poses (variable dimension is 6) * @addtogroup SLAM */ -template class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { protected: - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) + std::vector > 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 This; + typedef SmartStereoProjectionPoseFactor This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr 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 K_i) { + const boost::shared_ptr 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 measurements, std::vector poseKeys, std::vector noises, - std::vector > Ks) { + std::vector > 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 measurements, std::vector poseKeys, - const SharedNoiseModel noise, const boost::shared_ptr K) { + const SharedNoiseModel noise, const boost::shared_ptr 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& K, K_all_) + BOOST_FOREACH(const boost::shared_ptr& 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(&p); + const SmartStereoProjectionPoseFactor *e = + dynamic_cast(&p); return e && Base::equals(p, tol); } @@ -151,7 +151,7 @@ public: } /** return the calibration object */ - inline const std::vector > calibration() const { + inline const std::vector > 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(k); + const Pose3& pose = values.at(k); StereoCamera camera(pose, K_all_[i++]); cameras.push_back(camera); } @@ -185,9 +185,9 @@ private: }; // end of class declaration /// traits -template -struct traits > : public Testable< - SmartStereoProjectionPoseFactor > { +template<> +struct traits : public Testable< + SmartStereoProjectionPoseFactor> { }; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 097a9bddd..bef8d4048 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -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 SmartFactor; - vector 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 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 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 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;