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