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> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j4</buildArguments>
<buildTarget>SmartProjectionFactorExample_kitti_nonbatch.run</buildTarget> <buildTarget>SmartStereoProjectionFactorExample.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>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -835,6 +827,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <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 // Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */ /* ************************************************************************* */
int main(int argc, char* argv[]) { 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) { for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement // generate the 2D measurement
SimpleCamera camera(poses[i], *K); Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add: // 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 // 2. the corresponding camera's key
// 3. camera noise model // 3. camera noise model
// 4. camera calibration // 4. camera calibration
smartfactor->add(measurement, i, measurementNoise, K); smartfactor->add(measurement, i, measurementNoise);
} }
// insert the smart factor in the graph // 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. // 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 // 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()); (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 // 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 // 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. // 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. // 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"); graph.print("Factor Graph:\n");
@ -94,7 +97,7 @@ int main(int argc, char* argv[]) {
Values initialEstimate; Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); 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) 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"); initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results // 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 // Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */ /* ************************************************************************* */
int main(int argc, char* argv[]) { 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) { for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement // generate the 2D measurement
SimpleCamera camera(poses[i], *K); Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add: // 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 // 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. // 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 // 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()); (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 // 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 // Create the initial estimate to the solution
Values initialEstimate; Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); 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) 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 will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used. // 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 poseKey is the index corresponding to the camera observing the landmark
* @param sharedNoiseModel is the measurement noise * @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) { const SharedNoiseModel& sharedNoiseModel) {
this->measured_.push_back(measured_i); this->measured_.push_back(measured_i);
this->keys_.push_back(poseKey_i); this->keys_.push_back(cameraKey_i);
maybeSetNoiseModel(sharedNoiseModel); maybeSetNoiseModel(sharedNoiseModel);
} }
/** /**
* Add a bunch of measurements, together with the camera keys and noises * 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) { std::vector<SharedNoiseModel>& noises) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(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)); maybeSetNoiseModel(noises.at(i));
} }
} }
@ -138,11 +138,11 @@ public:
/** /**
* Add a bunch of measurements and uses the same noise model for all of them * 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) { const SharedNoiseModel& noise) {
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i)); this->measured_.push_back(measurements.at(i));
this->keys_.push_back(poseKeys.at(i)); this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noise); maybeSetNoiseModel(noise);
} }
} }
@ -170,6 +170,14 @@ public:
return measured_; 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 * print
* @param s optional string naming the factor * @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 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 /// linearize and adds damping on the points
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values, boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda=0.0) const { const double lambda=0.0) const {
if (!isImplicit_) if (!isImplicit_)
return Base::createHessianFactor(cameras(values), lambda); return Base::createHessianFactor(Base::cameras(values), lambda);
else else
return Base::createRegularImplicitSchurFactor(cameras(values)); return Base::createRegularImplicitSchurFactor(Base::cameras(values));
} }
/// linearize returns a Hessianfactor that is an approximation of error(p) /// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<RegularHessianFactor<Dim> > linearizeToHessian( virtual boost::shared_ptr<RegularHessianFactor<Dim> > linearizeToHessian(
const Values& values, double lambda=0.0) const { 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) /// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<RegularImplicitSchurFactor<Dim> > linearizeToImplicit( virtual boost::shared_ptr<RegularImplicitSchurFactor<Dim> > linearizeToImplicit(
const Values& values, double lambda=0.0) const { 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) /// linearize returns a Jacobianfactor that is an approximation of error(p)
virtual boost::shared_ptr<JacobianFactorQ<Dim, 2> > linearizeToJacobian( virtual boost::shared_ptr<JacobianFactorQ<Dim, 2> > linearizeToJacobian(
const Values& values, double lambda=0.0) const { 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) /// linearize returns a Hessianfactor that is an approximation of error(p)
@ -148,7 +140,7 @@ public:
/// Calculare total reprojection error /// Calculare total reprojection error
virtual double error(const Values& values) const { virtual double error(const Values& values) const {
if (this->active(values)) { if (this->active(values)) {
return Base::totalReprojectionError(cameras(values)); return Base::totalReprojectionError(Base::cameras(values));
} else { // else of active flag } else { // else of active flag
return 0.0; 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 */ /** return the landmark */
boost::optional<Point3> point() const { boost::optional<Point3> point() const {
return point_; return point_;

70
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -78,51 +78,6 @@ public:
/** Virtual destructor */ /** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {} 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 * print
* @param s optional string naming the factor * @param s optional string naming the factor
@ -143,23 +98,6 @@ public:
return e && Base::equals(p, tol); 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 * Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this 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 // depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){ switch(linearizeTo_){
case JACOBIAN_SVD : case JACOBIAN_SVD :
return this->createJacobianSVDFactor(cameras(values), 0.0); return this->createJacobianSVDFactor(Base::cameras(values), 0.0);
break; break;
case JACOBIAN_Q : case JACOBIAN_Q :
return this->createJacobianQFactor(cameras(values), 0.0); return this->createJacobianQFactor(Base::cameras(values), 0.0);
break; break;
default: default:
return this->createHessianFactor(cameras(values)); return this->createHessianFactor(Base::cameras(values));
break; break;
} }
} }
@ -186,7 +124,7 @@ public:
*/ */
virtual double error(const Values& values) const { virtual double error(const Values& values) const {
if (this->active(values)) { if (this->active(values)) {
return this->totalReprojectionError(cameras(values)); return this->totalReprojectionError(Base::cameras(values));
} else { // else of active flag } else { // else of active flag
return 0.0; return 0.0;
} }

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

@ -19,8 +19,8 @@
*/ */
#include "smartFactorScenarios.h" #include "smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h> #include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
@ -67,24 +67,24 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
TEST( SmartProjectionPoseFactor, Constructor3) { TEST( SmartProjectionPoseFactor, Constructor3) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor()); SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, model, sharedK); factor1->add(measurement1, x1, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor4) { TEST( SmartProjectionPoseFactor, Constructor4) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor factor1(rankTol, linThreshold); SmartFactor factor1(rankTol, linThreshold);
factor1.add(measurement1, x1, model, sharedK); factor1.add(measurement1, x1, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Equals ) { TEST( SmartProjectionPoseFactor, Equals ) {
using namespace vanillaPose; using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor()); SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, model, sharedK); factor1->add(measurement1, x1, model);
SmartFactor::shared_ptr factor2(new SmartFactor()); SmartFactor::shared_ptr factor2(new SmartFactor());
factor2->add(measurement1, x1, model, sharedK); factor2->add(measurement1, x1, model);
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
} }
@ -100,12 +100,12 @@ TEST_UNSAFE( SmartProjectionPoseFactor, noiseless ) {
Point2 level_uv_right = level_camera_right.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1);
SmartFactor factor; SmartFactor factor;
factor.add(level_uv, x1, model, sharedK); factor.add(level_uv, x1, model);
factor.add(level_uv_right, x2, model, sharedK); factor.add(level_uv_right, x2, model);
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam1);
values.insert(x2, pose_right); values.insert(x2, cam2);
double actualError = factor.error(values); double actualError = factor.error(values);
double expectedError = 0.0; double expectedError = 0.0;
@ -159,14 +159,14 @@ TEST( SmartProjectionPoseFactor, noisy ) {
Point2 level_uv_right = level_camera_right.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1);
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3)); 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()); SmartFactor::shared_ptr factor(new SmartFactor());
factor->add(level_uv, x1, model, sharedK); factor->add(level_uv, x1, model);
factor->add(level_uv_right, x2, model, sharedK); factor->add(level_uv_right, x2, model);
double actualError1 = factor->error(values); double actualError1 = factor->error(values);
@ -179,18 +179,12 @@ TEST( SmartProjectionPoseFactor, noisy ) {
noises.push_back(model); noises.push_back(model);
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; vector<Key> views;
views.push_back(x1); views.push_back(x1);
views.push_back(x2); views.push_back(x2);
factor2->add(measurements, views, noises, Ks); factor2->add(measurements, views, noises);
double actualError2 = factor2->error(values); double actualError2 = factor2->error(values);
DOUBLES_EQUAL(actualError1, actualError2, 1e-7); DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
} }
@ -212,13 +206,13 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x3); views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor()); 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()); 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()); 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); 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(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, 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/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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -288,7 +282,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
views.push_back(x2); views.push_back(x2);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(); SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>();
smartFactor1->add(measurements_cam1, views, model, sharedK); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::Cameras cameras; SmartFactor::Cameras cameras;
cameras.push_back(cam1); cameras.push_back(cam1);
@ -408,13 +402,13 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor()); 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()); 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()); 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); 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(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, 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/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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -476,15 +470,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); 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( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); 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( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); 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); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -492,16 +486,16 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, 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/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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above * noise_pose); values.insert(x3, Camera(pose_above * noise_pose, sharedK));
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result; Values result;
@ -532,17 +526,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist)); excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, sharedK); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist)); excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, sharedK); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist)); excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, sharedK); smartFactor3->add(measurements_cam3, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -550,16 +544,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, 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/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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above * noise_pose); values.insert(x3, Camera(pose_above * noise_pose, sharedK));
// All factors are disabled and pose should remain where it is // All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
@ -598,22 +592,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, sharedK); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, sharedK); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, sharedK); smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor4( SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, JACOBIAN_SVD, new SmartFactor(1, -1, false, false, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, sharedK); smartFactor4->add(measurements_cam4, views, model);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -622,15 +616,15 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(smartFactor4); graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior)); graph.push_back(PriorFactor<Camera>(x2, cam2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above); values.insert(x3, Camera(pose_above * noise_pose, sharedK));
// All factors are disabled and pose should remain where it is // All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
@ -659,15 +653,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, JACOBIAN_Q)); 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( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, JACOBIAN_Q)); 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( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, JACOBIAN_Q)); 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); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -675,16 +669,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, 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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above * noise_pose); values.insert(x3, Camera(pose_above * noise_pose, sharedK));
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result; Values result;
@ -730,15 +723,15 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2)); ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2));
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior)); graph.push_back(PriorFactor<Camera>(x2, cam2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3)); Point3(0.5, 0.1, 0.3));
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above * noise_pose); values.insert(x3, Camera(pose_above * noise_pose, sharedK2));
values.insert(L(1), landmark1); values.insert(L(1), landmark1);
values.insert(L(2), landmark2); values.insert(L(2), landmark2);
values.insert(L(3), landmark3); values.insert(L(3), landmark3);
@ -786,13 +779,13 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
double rankTol = 10; double rankTol = 10;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); 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)); 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)); SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
smartFactor3->add(measurements_cam3, views, model, sharedK); smartFactor3->add(measurements_cam3, views, model);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
@ -803,10 +796,10 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); 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; double rankTol = 50;
SmartFactor::shared_ptr smartFactor1( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(rankTol, linThreshold, manageDegeneracy)); new SmartFactor(rankTol, linThreshold, manageDegeneracy));
smartFactor1->add(measurements_cam1, views, model, sharedK2); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(rankTol, linThreshold, manageDegeneracy)); 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 noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
@ -881,7 +874,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back( graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back( 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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right * noise_pose); 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 // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); 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( SmartFactor::shared_ptr smartFactor1(
new SmartFactor(rankTol, linThreshold, manageDegeneracy)); new SmartFactor(rankTol, linThreshold, manageDegeneracy));
smartFactor1->add(measurements_cam1, views, model, sharedK); smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor2( SmartFactor::shared_ptr smartFactor2(
new SmartFactor(rankTol, linThreshold, manageDegeneracy)); new SmartFactor(rankTol, linThreshold, manageDegeneracy));
smartFactor2->add(measurements_cam2, views, model, sharedK); smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor3( SmartFactor::shared_ptr smartFactor3(
new SmartFactor(rankTol, linThreshold, manageDegeneracy)); 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 noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 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(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back( graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back( 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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -1029,13 +1022,13 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
measurements_cam1.push_back(cam2_uv1); measurements_cam1.push_back(cam2_uv1);
SmartFactor::shared_ptr smartFactor1(new SmartFactor()); 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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3)); Point3(0.5, 0.1, 0.3));
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize( boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(
values); values);
@ -1064,12 +1057,12 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor()); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
smartFactorInstance->add(measurements_cam1, views, model, sharedK); smartFactorInstance->add(measurements_cam1, views, model);
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above); values.insert(x3, cam3);
boost::shared_ptr<GaussianFactor> hessianFactor = boost::shared_ptr<GaussianFactor> hessianFactor =
smartFactorInstance->linearize(values); 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)); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues; Values rotValues;
rotValues.insert(x1, poseDrift.compose(level_pose)); rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK));
rotValues.insert(x2, poseDrift.compose(pose_right)); rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK));
rotValues.insert(x3, poseDrift.compose(pose_above)); rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK));
boost::shared_ptr<GaussianFactor> hessianFactorRot = boost::shared_ptr<GaussianFactor> hessianFactorRot =
smartFactorInstance->linearize(rotValues); smartFactorInstance->linearize(rotValues);
@ -1093,9 +1086,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
Point3(10, -4, 5)); Point3(10, -4, 5));
Values tranValues; Values tranValues;
tranValues.insert(x1, poseDrift2.compose(level_pose)); tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK));
tranValues.insert(x2, poseDrift2.compose(pose_right)); tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK));
tranValues.insert(x3, poseDrift2.compose(pose_above)); tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK));
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactorInstance->linearize(tranValues); smartFactorInstance->linearize(tranValues);
@ -1122,12 +1115,12 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactor(new SmartFactor()); SmartFactor::shared_ptr smartFactor(new SmartFactor());
smartFactor->add(measurements_cam1, views, model, sharedK2); smartFactor->add(measurements_cam1, views, model);
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
values.insert(x3, pose_above); values.insert(x3, cam3);
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize( boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values); values);
@ -1137,9 +1130,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
Values rotValues; Values rotValues;
rotValues.insert(x1, poseDrift.compose(level_pose)); rotValues.insert(x1, Camera(poseDrift.compose(level_pose),sharedK2));
rotValues.insert(x2, poseDrift.compose(pose_right)); rotValues.insert(x2, Camera(poseDrift.compose(pose_right),sharedK2));
rotValues.insert(x3, poseDrift.compose(pose_above)); rotValues.insert(x3, Camera(poseDrift.compose(pose_above),sharedK2));
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize( boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues); rotValues);
@ -1155,9 +1148,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
Point3(10, -4, 5)); Point3(10, -4, 5));
Values tranValues; Values tranValues;
tranValues.insert(x1, poseDrift2.compose(level_pose)); tranValues.insert(x1, Camera(poseDrift2.compose(level_pose),sharedK2));
tranValues.insert(x2, poseDrift2.compose(pose_right)); tranValues.insert(x2, Camera(poseDrift2.compose(pose_right),sharedK2));
tranValues.insert(x3, poseDrift2.compose(pose_above)); tranValues.insert(x3, Camera(poseDrift2.compose(pose_above),sharedK2));
boost::shared_ptr<GaussianFactor> hessianFactorRotTran = boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
smartFactor->linearize(tranValues); smartFactor->linearize(tranValues);
@ -1171,9 +1164,7 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
SmartFactorBundler factor(rankTol, linThreshold); SmartFactorBundler factor(rankTol, linThreshold);
boost::shared_ptr<Cal3Bundler> sharedBundlerK( factor.add(measurement1, x1, model);
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
factor.add(measurement1, x1, model, sharedBundlerK);
} }
/* *************************************************************************/ /* *************************************************************************/
@ -1215,13 +1206,13 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
views.push_back(x3); views.push_back(x3);
SmartFactorBundler::shared_ptr smartFactor1(new SmartFactorBundler()); 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()); 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()); 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); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1229,17 +1220,17 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, 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/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), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
@ -1313,15 +1304,15 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
SmartFactorBundler::shared_ptr smartFactor1( SmartFactorBundler::shared_ptr smartFactor1(
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
smartFactor1->add(measurements_cam1, views, model, sharedBundlerK); smartFactor1->add(measurements_cam1, views, model);
SmartFactorBundler::shared_ptr smartFactor2( SmartFactorBundler::shared_ptr smartFactor2(
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy));
smartFactor2->add(measurements_cam2, views, model, sharedBundlerK); smartFactor2->add(measurements_cam2, views, model);
SmartFactorBundler::shared_ptr smartFactor3( SmartFactorBundler::shared_ptr smartFactor3(
new SmartFactorBundler(rankTol, linThreshold, manageDegeneracy)); 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 noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
@ -1332,7 +1323,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
graph.push_back(smartFactor1); graph.push_back(smartFactor1);
graph.push_back(smartFactor2); graph.push_back(smartFactor2);
graph.push_back(smartFactor3); graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(PriorFactor<Camera>(x1, cam1, noisePrior));
graph.push_back( graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation)); PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back( graph.push_back(
@ -1342,10 +1333,10 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise Point3(0.1, 0.1, 0.1)); // smaller noise
Values values; Values values;
values.insert(x1, level_pose); values.insert(x1, cam2);
values.insert(x2, pose_right); values.insert(x2, cam2);
// initialize third pose with some noise, we expect it to move back to original pose_above // 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) if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: "); 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 * -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/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <string> #include <string>
#include <fstream> #include <fstream>
@ -46,6 +44,7 @@ using namespace gtsam;
int main(int argc, char** argv){ int main(int argc, char** argv){
typedef PinholePose<Cal3_S2> Camera;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor; typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
Values initial_estimate; Values initial_estimate;
@ -68,18 +67,17 @@ int main(int argc, char** argv){
cout << "Reading camera poses" << endl; cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str()); ifstream pose_file(pose_loc.c_str());
int pose_id; int i;
MatrixRowMajor m(4,4); MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses //read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) { while (pose_file >> i) {
for (int i = 0; i < 16; i++) { for (int i = 0; i < 16; i++)
pose_file >> m.data()[i]; pose_file >> m.data()[i];
} initial_estimate.insert(i, Camera(Pose3(m),K));
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
} }
// camera and landmark keys // landmark keys
size_t x, l; size_t l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification) // pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation // 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()); SmartFactor::shared_ptr factor(new SmartFactor());
size_t current_l = 3; // hardcoded landmark ID from first measurement 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) { if(current_l != l) {
graph.push_back(factor); graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor()); factor = SmartFactor::shared_ptr(new SmartFactor());
current_l = l; 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 //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 // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable // 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; LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
@ -118,7 +116,7 @@ int main(int argc, char** argv){
Values result = optimizer.optimize(); Values result = optimizer.optimize();
cout << "Final result sample:" << endl; cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>(); Values pose_values = result.filter<Camera>();
pose_values.print("Final camera poses:\n"); pose_values.print("Final camera poses:\n");
return 0; return 0;

1
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

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