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

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.

release/4.3a0
dellaert 2015-02-26 13:55:16 +01:00
parent a132d959f5
commit a375e7b5be
10 changed files with 195 additions and 264 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
.cproject
View File

@ -811,18 +811,10 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SmartProjectionFactorExample_kitti_nonbatch.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="SmartStereoProjectionFactorExample.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SmartProjectionFactorExample_kitti_nonbatch.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SmartProjectionFactorExample_kitti.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SmartProjectionFactorExample_kitti.run</buildTarget>
<buildArguments>-j4</buildArguments>
<buildTarget>SmartStereoProjectionFactorExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -835,6 +827,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SmartProjectionFactorExample.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>SmartProjectionFactorExample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

15
examples/SFMExample_SmartFactor.cpp
View File

@ -34,6 +34,9 @@ using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> 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<Pose3>(0, poses[0], poseNoise));
graph.push_back(PriorFactor<Camera>(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<Pose3>(1, poses[1], poseNoise)); // add directly to graph
graph.push_back(PriorFactor<Camera>(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

15
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -30,6 +30,9 @@ using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> 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<Pose3>(0, poses[0], poseNoise));
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
graph.push_back(PriorFactor<Camera>(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.

20
gtsam/slam/SmartFactorBase.h
View File

@ -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<Z>& measurements, std::vector<Key>& poseKeys,
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
std::vector<SharedNoiseModel>& 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<Z>& measurements, std::vector<Key>& poseKeys,
void add(std::vector<Z>& measurements, std::vector<Key>& 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<CAMERA>(k));
return cameras;
}
/**
* print
* @param s optional string naming the factor

20
gtsam/slam/SmartProjectionCameraFactor.h
View File

@ -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<Camera>(k));
return cameras;
}
/// linearize and adds damping on the points
boost::shared_ptr<GaussianFactor> 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<RegularHessianFactor<Dim> > 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<RegularImplicitSchurFactor<Dim> > 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<JacobianFactorQ<Dim, 2> > 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;
}

3
gtsam/slam/SmartProjectionFactor.h
View File

@ -621,9 +621,6 @@ public:
}
}
/// Cameras are computed in derived class
virtual Cameras cameras(const Values& values) const = 0;
/** return the landmark */
boost::optional<Point3> point() const {
return point_;

70
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -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<CALIBRATION> 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<Point2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<CALIBRATION> > 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<Point2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> 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<Pose3>(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;
}

263
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -19,8 +19,8 @@
*/
#include "smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
@ -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<boost::shared_ptr<Cal3_S2> > Ks; ///< shared pointer to calibration object (one for each camera)
Ks.push_back(sharedK);
Ks.push_back(sharedK);
vector<Key> 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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -288,7 +282,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
views.push_back(x2);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>();
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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(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<Pose3>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(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<Pose3>(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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> 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<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues);
@ -1171,9 +1164,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
SmartFactorBundler factor(rankTol, linThreshold);
boost::shared_ptr<Cal3Bundler> 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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(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<Pose3>(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<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(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<Pose3>(x3).print("Smart: Pose3 before optimization: ");

30
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -26,16 +26,14 @@
* -makes monocular observations of many landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <string>
#include <fstream>
@ -46,6 +44,7 @@ using namespace gtsam;
int main(int argc, char** argv){
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> 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<Pose3>(Symbol('x',1));
Camera firstCamera = initial_estimate.at<Camera>(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<Pose3>(Symbol('x',1),first_pose));
graph.push_back(NonlinearEquality<Camera>(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<Pose3>();
Values pose_values = result.filter<Camera>();
pose_values.print("Final camera poses:\n");
return 0;

1
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -15,6 +15,7 @@
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
* @author Frank Dellaert
*/
#pragma once