From a375e7b5be1b43d254a7d295e413ef16f22aaa4d Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 26 Feb 2015 13:55:16 +0100 Subject: [PATCH] RADICAL2: The SmartProjectionPoseFactor (soon to be renamed SmartPinholePoseFactor, if it survives at all) now no longer stores shared calibrations. Values expect to contain PinholePoses not Pose3s now. The current state of affairs was simply a bug: one pose could be optimized for several different calibrations. It relied on the user to make sure all measurements for a specific pose to optimize were all given the same shared calibration, which was then stored *millions of times* in the pose factors. Instead, there is now *one* shared calibration per PinholePose unknown. --- .cproject | 22 +- examples/SFMExample_SmartFactor.cpp | 15 +- examples/SFMExample_SmartFactorPCG.cpp | 15 +- gtsam/slam/SmartFactorBase.h | 20 +- gtsam/slam/SmartProjectionCameraFactor.h | 20 +- gtsam/slam/SmartProjectionFactor.h | 3 - gtsam/slam/SmartProjectionPoseFactor.h | 70 +---- .../tests/testSmartProjectionPoseFactor.cpp | 263 +++++++++--------- .../examples/SmartProjectionFactorExample.cpp | 30 +- .../slam/SmartStereoProjectionPoseFactor.h | 1 + 10 files changed, 195 insertions(+), 264 deletions(-) diff --git a/.cproject b/.cproject index 5d3733e28..e190d8f65 100644 --- a/.cproject +++ b/.cproject @@ -811,18 +811,10 @@ false true - + make - -j5 - SmartProjectionFactorExample_kitti_nonbatch.run - true - true - true - - - make - -j5 - SmartProjectionFactorExample_kitti.run + -j4 + SmartStereoProjectionFactorExample.run true true true @@ -835,6 +827,14 @@ true true + + make + -j4 + SmartProjectionFactorExample.run + true + true + true + make -j2 diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index fce046a59..cd6024e6c 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -34,6 +34,9 @@ using namespace gtsam; // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; +// create a typedef to the camera type +typedef PinholePose Camera; + /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -60,7 +63,7 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[i], *K); + Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: @@ -68,7 +71,7 @@ int main(int argc, char* argv[]) { // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, i, measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise); } // insert the smart factor in the graph @@ -77,15 +80,15 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], poseNoise)); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); // Because the structure-from-motion problem has a scale ambiguity, the problem is // still under-constrained. Here we add a prior on the second pose x1, so this will // fix the scale by indicating the distance between x0 and x1. // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph + graph.push_back(PriorFactor(1, Camera(poses[1],K), noise)); // add directly to graph graph.print("Factor Graph:\n"); @@ -94,7 +97,7 @@ int main(int argc, char* argv[]) { Values initialEstimate; Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.insert(i, Camera(poses[i].compose(delta), K)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index 49482292f..27ef7b9cc 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/examples/SFMExample_SmartFactorPCG.cpp @@ -30,6 +30,9 @@ using namespace gtsam; // Make the typename short so it looks much cleaner typedef SmartProjectionPoseFactor SmartFactor; +// create a typedef to the camera type +typedef PinholePose Camera; + /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -56,11 +59,11 @@ int main(int argc, char* argv[]) { for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[i], *K); + Camera camera(poses[i], K); Point2 measurement = camera.project(points[j]); // call add() function to add measurement into a single factor, here we need to add: - smartfactor->add(measurement, i, measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise); } // insert the smart factor in the graph @@ -69,18 +72,18 @@ int main(int argc, char* argv[]) { // Add a prior on pose x0. This indirectly specifies where the origin is. // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); - graph.push_back(PriorFactor(0, poses[0], poseNoise)); + graph.push_back(PriorFactor(0, Camera(poses[0],K), noise)); // Fix the scale ambiguity by adding a prior - graph.push_back(PriorFactor(1, poses[1], poseNoise)); + graph.push_back(PriorFactor(1, Camera(poses[0],K), noise)); // Create the initial estimate to the solution Values initialEstimate; Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.insert(i, Camera(poses[i].compose(delta),K)); // We will use LM in the outer optimization loop, but by specifying "Iterative" below // We indicate that an iterative linear solver should be used. diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 56c214264..ba0d27530 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -116,21 +116,21 @@ public: * @param poseKey is the index corresponding to the camera observing the landmark * @param sharedNoiseModel is the measurement noise */ - void add(const Z& measured_i, const Key& poseKey_i, + void add(const Z& measured_i, const Key& cameraKey_i, const SharedNoiseModel& sharedNoiseModel) { this->measured_.push_back(measured_i); - this->keys_.push_back(poseKey_i); + this->keys_.push_back(cameraKey_i); maybeSetNoiseModel(sharedNoiseModel); } /** * Add a bunch of measurements, together with the camera keys and noises */ - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& cameraKeys, std::vector& noises) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); + this->keys_.push_back(cameraKeys.at(i)); maybeSetNoiseModel(noises.at(i)); } } @@ -138,11 +138,11 @@ public: /** * Add a bunch of measurements and uses the same noise model for all of them */ - void add(std::vector& measurements, std::vector& poseKeys, + void add(std::vector& measurements, std::vector& cameraKeys, const SharedNoiseModel& noise) { for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); - this->keys_.push_back(poseKeys.at(i)); + this->keys_.push_back(cameraKeys.at(i)); maybeSetNoiseModel(noise); } } @@ -170,6 +170,14 @@ public: return measured_; } + /// Collect all cameras: important that in key order + Cameras cameras(const Values& values) const { + Cameras cameras; + BOOST_FOREACH(const Key& k, this->keys_) + cameras.push_back(values.at(k)); + return cameras; + } + /** * print * @param s optional string naming the factor diff --git a/gtsam/slam/SmartProjectionCameraFactor.h b/gtsam/slam/SmartProjectionCameraFactor.h index 7ca8a4e1d..f10681ab8 100644 --- a/gtsam/slam/SmartProjectionCameraFactor.h +++ b/gtsam/slam/SmartProjectionCameraFactor.h @@ -95,39 +95,31 @@ public: return Dim * this->keys_.size(); // 6 for the pose and 3 for the calibration } - /// Collect all cameras: important that in key order - Cameras cameras(const Values& values) const { - Cameras cameras; - BOOST_FOREACH(const Key& k, this->keys_) - cameras.push_back(values.at(k)); - return cameras; - } - /// linearize and adds damping on the points boost::shared_ptr linearizeDamped(const Values& values, const double lambda=0.0) const { if (!isImplicit_) - return Base::createHessianFactor(cameras(values), lambda); + return Base::createHessianFactor(Base::cameras(values), lambda); else - return Base::createRegularImplicitSchurFactor(cameras(values)); + return Base::createRegularImplicitSchurFactor(Base::cameras(values)); } /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr > linearizeToHessian( const Values& values, double lambda=0.0) const { - return Base::createHessianFactor(cameras(values),lambda); + return Base::createHessianFactor(Base::cameras(values),lambda); } /// linearize returns a Hessianfactor that is an approximation of error(p) virtual boost::shared_ptr > linearizeToImplicit( const Values& values, double lambda=0.0) const { - return Base::createRegularImplicitSchurFactor(cameras(values),lambda); + return Base::createRegularImplicitSchurFactor(Base::cameras(values),lambda); } /// linearize returns a Jacobianfactor that is an approximation of error(p) virtual boost::shared_ptr > linearizeToJacobian( const Values& values, double lambda=0.0) const { - return Base::createJacobianQFactor(cameras(values),lambda); + return Base::createJacobianQFactor(Base::cameras(values),lambda); } /// linearize returns a Hessianfactor that is an approximation of error(p) @@ -148,7 +140,7 @@ public: /// Calculare total reprojection error virtual double error(const Values& values) const { if (this->active(values)) { - return Base::totalReprojectionError(cameras(values)); + return Base::totalReprojectionError(Base::cameras(values)); } else { // else of active flag return 0.0; } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index f99ce7085..5b061f612 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -621,9 +621,6 @@ public: } } - /// Cameras are computed in derived class - virtual Cameras cameras(const Values& values) const = 0; - /** return the landmark */ boost::optional point() const { return point_; diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 2ee807d9d..48fbd937f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -78,51 +78,6 @@ public: /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} - /** - * add a new measurement and pose key - * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKey is key corresponding to the camera observing the same landmark - * @param noise_i is the measurement noise - * @param K_i is the (known) camera calibration - */ - void add(const Point2 measured_i, const Key poseKey_i, - const SharedNoiseModel noise_i, - const boost::shared_ptr K_i) { - Base::add(measured_i, poseKey_i, noise_i); - sharedKs_.push_back(K_i); - } - - /** - * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noises vector of measurement noises - * @param Ks vector of calibration objects - */ - void add(std::vector measurements, std::vector poseKeys, - std::vector noises, - std::vector > Ks) { - Base::add(measurements, poseKeys, noises); - for (size_t i = 0; i < measurements.size(); i++) { - sharedKs_.push_back(Ks.at(i)); - } - } - - /** - * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark - * @param noise measurement noise (same for all measurements) - * @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) { - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i), noise); - sharedKs_.push_back(K); - } - } - /** * print * @param s optional string naming the factor @@ -143,23 +98,6 @@ public: return e && Base::equals(p, tol); } - /** - * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses corresponding - * to keys involved in this factor - * @return vector of Values - */ - typename Base::Cameras cameras(const Values& values) const { - typename Base::Cameras cameras; - size_t i = 0; - BOOST_FOREACH(const Key& k, this->keys_) { - Pose3 pose = values.at(k); - Camera camera(pose, sharedKs_[i++]); - cameras.push_back(camera); - } - return cameras; - } - /** * Linearize to Gaussian Factor * @param values Values structure which must contain camera poses for this factor @@ -170,13 +108,13 @@ public: // depending on flag set on construction we may linearize to different linear factors switch(linearizeTo_){ case JACOBIAN_SVD : - return this->createJacobianSVDFactor(cameras(values), 0.0); + return this->createJacobianSVDFactor(Base::cameras(values), 0.0); break; case JACOBIAN_Q : - return this->createJacobianQFactor(cameras(values), 0.0); + return this->createJacobianQFactor(Base::cameras(values), 0.0); break; default: - return this->createHessianFactor(cameras(values)); + return this->createHessianFactor(Base::cameras(values)); break; } } @@ -186,7 +124,7 @@ public: */ virtual double error(const Values& values) const { if (this->active(values)) { - return this->totalReprojectionError(cameras(values)); + return this->totalReprojectionError(Base::cameras(values)); } else { // else of active flag return 0.0; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 9345b5f45..b235d8e2b 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -19,8 +19,8 @@ */ #include "smartFactorScenarios.h" -#include #include +#include #include #include #include @@ -67,24 +67,24 @@ TEST( SmartProjectionPoseFactor, Constructor2) { TEST( SmartProjectionPoseFactor, Constructor3) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, x1, model, sharedK); + factor1->add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; SmartFactor factor1(rankTol, linThreshold); - factor1.add(measurement1, x1, model, sharedK); + factor1.add(measurement1, x1, model); } /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor()); - factor1->add(measurement1, x1, model, sharedK); + factor1->add(measurement1, x1, model); SmartFactor::shared_ptr factor2(new SmartFactor()); - factor2->add(measurement1, x1, model, sharedK); + factor2->add(measurement1, x1, model); CHECK(assert_equal(*factor1, *factor2)); } @@ -100,12 +100,12 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) { Point2 level_uv_right = level_camera_right.project(landmark1); SmartFactor factor; - factor.add(level_uv, x1, model, sharedK); - factor.add(level_uv_right, x2, model, sharedK); + factor.add(level_uv, x1, model); + factor.add(level_uv_right, x2, model); Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam1); + values.insert(x2, cam2); double actualError = factor.error(values); double expectedError = 0.0; @@ -159,14 +159,14 @@ TEST( SmartProjectionPoseFactor, noisy ) { Point2 level_uv_right = level_camera_right.project(landmark1); Values values; - values.insert(x1, level_pose); + values.insert(x1, cam2); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); - values.insert(x2, pose_right.compose(noise_pose)); + values.insert(x2, Camera(pose_right.compose(noise_pose), sharedK)); SmartFactor::shared_ptr factor(new SmartFactor()); - factor->add(level_uv, x1, model, sharedK); - factor->add(level_uv_right, x2, model, sharedK); + factor->add(level_uv, x1, model); + factor->add(level_uv_right, x2, model); double actualError1 = factor->error(values); @@ -179,18 +179,12 @@ TEST( SmartProjectionPoseFactor, noisy ) { noises.push_back(model); noises.push_back(model); - vector > Ks; ///< shared pointer to calibration object (one for each camera) - Ks.push_back(sharedK); - Ks.push_back(sharedK); - vector views; views.push_back(x1); views.push_back(x2); - factor2->add(measurements, views, noises, Ks); - + factor2->add(measurements, views, noises); double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } @@ -212,13 +206,13 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { views.push_back(x3); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, sharedK2); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, sharedK2); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, sharedK2); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -226,17 +220,17 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose, sharedK2)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -288,7 +282,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { views.push_back(x2); SmartFactor::shared_ptr smartFactor1 = boost::make_shared(); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::Cameras cameras; cameras.push_back(cam1); @@ -408,13 +402,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2(new SmartFactor()); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3(new SmartFactor()); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -422,17 +416,17 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -476,15 +470,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -492,16 +486,16 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); LevenbergMarquardtParams params; Values result; @@ -532,17 +526,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -550,16 +544,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; @@ -598,22 +592,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); SmartFactor::shared_ptr smartFactor4( new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); - smartFactor4->add(measurements_cam4, views, model, sharedK); + smartFactor4->add(measurements_cam4, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -622,15 +616,15 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { graph.push_back(smartFactor2); graph.push_back(smartFactor3); graph.push_back(smartFactor4); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); // All factors are disabled and pose should remain where it is LevenbergMarquardtParams params; @@ -659,15 +653,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { SmartFactor::shared_ptr smartFactor1( new SmartFactor(1, -1, false, false, JACOBIAN_Q)); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( new SmartFactor(1, -1, false, false, JACOBIAN_Q)); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( new SmartFactor(1, -1, false, false, JACOBIAN_Q)); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -675,16 +669,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); - // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); LevenbergMarquardtParams params; Values result; @@ -730,15 +723,15 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above * noise_pose); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, Camera(pose_above * noise_pose, sharedK2)); values.insert(L(1), landmark1); values.insert(L(2), landmark2); values.insert(L(3), landmark3); @@ -786,13 +779,13 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); NonlinearFactorGraph graph; graph.push_back(smartFactor1); @@ -803,10 +796,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose, sharedK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -867,11 +860,11 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac double rankTol = 50; SmartFactor::shared_ptr smartFactor1( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, sharedK2); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, sharedK2); + smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -881,7 +874,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); graph.push_back( PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( @@ -890,10 +883,10 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right * noise_pose); + values.insert(x1, cam2); + values.insert(x2, Camera(pose_right * noise_pose, sharedK2)); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose * noise_pose, sharedK2)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -951,15 +944,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) SmartFactor::shared_ptr smartFactor1( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, sharedK); + smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, sharedK); + smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( new SmartFactor(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, sharedK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -970,7 +963,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); graph.push_back( PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( @@ -980,10 +973,10 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose,sharedK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -1029,13 +1022,13 @@ TEST( SmartProjectionPoseFactor, Hessian ) { measurements_cam1.push_back(cam2_uv1); SmartFactor::shared_ptr smartFactor1(new SmartFactor()); - smartFactor1->add(measurements_cam1, views, model, sharedK2); + smartFactor1->add(measurements_cam1, views, model); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); boost::shared_ptr hessianFactor = smartFactor1->linearize( values); @@ -1064,12 +1057,12 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); - smartFactorInstance->add(measurements_cam1, views, model, sharedK); + smartFactorInstance->add(measurements_cam1, views, model); Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, cam3); boost::shared_ptr hessianFactor = smartFactorInstance->linearize(values); @@ -1077,9 +1070,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(pose_right)); - rotValues.insert(x3, poseDrift.compose(pose_above)); + rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK)); + rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK)); + rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK)); boost::shared_ptr hessianFactorRot = smartFactorInstance->linearize(rotValues); @@ -1093,9 +1086,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) { Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(pose_right)); - tranValues.insert(x3, poseDrift2.compose(pose_above)); + tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK)); + tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK)); + tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK)); boost::shared_ptr hessianFactorRotTran = smartFactorInstance->linearize(tranValues); @@ -1122,12 +1115,12 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactor(new SmartFactor()); - smartFactor->add(measurements_cam1, views, model, sharedK2); + smartFactor->add(measurements_cam1, views, model); Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); + values.insert(x1, cam2); + values.insert(x2, cam2); + values.insert(x3, cam3); boost::shared_ptr hessianFactor = smartFactor->linearize( values); @@ -1137,9 +1130,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(pose_right)); - rotValues.insert(x3, poseDrift.compose(pose_above)); + rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK2)); + rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK2)); + rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK2)); boost::shared_ptr hessianFactorRot = smartFactor->linearize( rotValues); @@ -1155,9 +1148,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { Point3(10, -4, 5)); Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(pose_right)); - tranValues.insert(x3, poseDrift2.compose(pose_above)); + tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK2)); + tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK2)); + tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK2)); boost::shared_ptr hessianFactorRotTran = smartFactor->linearize(tranValues); @@ -1171,9 +1164,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { SmartFactorBundler factor(rankTol, linThreshold); - boost::shared_ptr sharedBundlerK( - new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); - factor.add(measurement1, x1, model, sharedBundlerK); + factor.add(measurement1, x1, model); } /* *************************************************************************/ @@ -1215,13 +1206,13 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { views.push_back(x3); SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); - smartFactor1->add(measurements_cam1, views, model, sharedBundlerK); + smartFactor1->add(measurements_cam1, views, model); SmartFactorBundler::shared_ptr smartFactor2(new SmartFactorBundler()); - smartFactor2->add(measurements_cam2, views, model, sharedBundlerK); + smartFactor2->add(measurements_cam2, views, model); SmartFactorBundler::shared_ptr smartFactor3(new SmartFactorBundler()); - smartFactor3->add(measurements_cam3, views, model, sharedBundlerK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1229,17 +1220,17 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); - graph.push_back(PriorFactor(x2, pose_right, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); + graph.push_back(PriorFactor(x2, cam2, noisePrior)); // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); @@ -1313,15 +1304,15 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { SmartFactorBundler::shared_ptr smartFactor1( new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor1->add(measurements_cam1, views, model, sharedBundlerK); + smartFactor1->add(measurements_cam1, views, model); SmartFactorBundler::shared_ptr smartFactor2( new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor2->add(measurements_cam2, views, model, sharedBundlerK); + smartFactor2->add(measurements_cam2, views, model); SmartFactorBundler::shared_ptr smartFactor3( new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); - smartFactor3->add(measurements_cam3, views, model, sharedBundlerK); + smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, @@ -1332,7 +1323,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { graph.push_back(smartFactor1); graph.push_back(smartFactor2); graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, level_pose, noisePrior)); + graph.push_back(PriorFactor(x1, cam1, noisePrior)); graph.push_back( PoseTranslationPrior(x2, positionPrior, noisePriorTranslation)); graph.push_back( @@ -1342,10 +1333,10 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise Values values; - values.insert(x1, level_pose); - values.insert(x2, pose_right); + values.insert(x1, cam2); + values.insert(x2, cam2); // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, pose_above * noise_pose); + values.insert(x3, Camera(pose_above * noise_pose,sharedBundlerK)); if (isDebugTest) values.at(x3).print("Smart: Pose3 before optimization: "); diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index dc921a7da..9e8523ebb 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -26,16 +26,14 @@ * -makes monocular observations of many landmarks */ -#include +#include +#include #include #include #include #include #include #include -#include - -#include #include #include @@ -46,6 +44,7 @@ using namespace gtsam; int main(int argc, char** argv){ + typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; Values initial_estimate; @@ -68,18 +67,17 @@ int main(int argc, char** argv){ cout << "Reading camera poses" << endl; ifstream pose_file(pose_loc.c_str()); - int pose_id; + int i; MatrixRowMajor m(4,4); //read camera pose parameters and use to make initial estimates of camera poses - while (pose_file >> pose_id) { - for (int i = 0; i < 16; i++) { + while (pose_file >> i) { + for (int i = 0; i < 16; i++) pose_file >> m.data()[i]; - } - initial_estimate.insert(Symbol('x', pose_id), Pose3(m)); + initial_estimate.insert(i, Camera(Pose3(m),K)); } - // camera and landmark keys - size_t x, l; + // landmark keys + size_t l; // pixel coordinates uL, uR, v (same for left/right images due to rectification) // landmark coordinates X, Y, Z in camera frame, resulting from triangulation @@ -92,21 +90,21 @@ int main(int argc, char** argv){ SmartFactor::shared_ptr factor(new SmartFactor()); size_t current_l = 3; // hardcoded landmark ID from first measurement - while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) { + while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) { if(current_l != l) { graph.push_back(factor); factor = SmartFactor::shared_ptr(new SmartFactor()); current_l = l; } - factor->add(Point2(uL,v), Symbol('x',x), model, K); + factor->add(Point2(uL,v), i, model); } - Pose3 first_pose = initial_estimate.at(Symbol('x',1)); + Camera firstCamera = initial_estimate.at(1); //constrain the first pose such that it cannot change from its original value during optimization // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky // QR is much slower than Cholesky, but numerically more stable - graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + graph.push_back(NonlinearEquality(1,firstCamera)); LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; @@ -118,7 +116,7 @@ int main(int argc, char** argv){ Values result = optimizer.optimize(); cout << "Final result sample:" << endl; - Values pose_values = result.filter(); + Values pose_values = result.filter(); pose_values.print("Final camera poses:\n"); return 0; diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 2f4677835..8fe8bea69 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -15,6 +15,7 @@ * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once