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

Updated to work with unordered GTSAM

release/4.3a0
Luca Carlone 2013-08-14 19:12:21 +00:00
parent fee021e51c
commit d7e6f43fa7
2 changed files with 29 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -22,6 +22,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/HessianFactor.h>
#include <vector> #include <vector>
#include <gtsam_unstable/geometry/triangulation.h> #include <gtsam_unstable/geometry/triangulation.h>
#include <boost/optional.hpp> #include <boost/optional.hpp>
@ -147,7 +148,7 @@ namespace gtsam {
} }
/// 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<GaussianFactor> linearize(const Values& values, const Ordering& ordering) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
bool debug = false; bool debug = false;
bool blockwise = true; bool blockwise = true;
@ -185,22 +186,16 @@ namespace gtsam {
std::cout << "point " << *point << std::endl; std::cout << "point " << *point << std::endl;
} }
std::vector<Index> js;
std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2); std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2);
std::vector<Vector> gs(keys_.size()); std::vector<Vector> gs(keys_.size());
double f=0; double f=0;
// fill in the keys
BOOST_FOREACH(const Key& k, keys_) {
js += ordering[k];
}
// point is behind one of the cameras, turn factor off by setting everything to 0 // point is behind one of the cameras, turn factor off by setting everything to 0
if (!point) { if (!point) {
std::cout << "WARNING: Could not triangulate during linearize" << std::endl; std::cout << "WARNING: Could not triangulate during linearize" << std::endl;
BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6); BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6,6);
BOOST_FOREACH(Vector& v, gs) v = zero(6); BOOST_FOREACH(Vector& v, gs) v = zero(6);
return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
} }
// For debug only // For debug only
@ -374,7 +369,7 @@ namespace gtsam {
} }
// ========================================================================================================== // ==========================================================================================================
return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f));
} }
/** /**

61
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -19,12 +19,7 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream> #include <iostream>
TEST(SmartProjectionFactor, disabled)
{
CHECK(("*** testSmartProjectionFactor is disabled *** - Needs conversion for unordered", 0));
}
#if 0
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
@ -234,20 +229,20 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graphWithOriginalFactor; NonlinearFactorGraph graphWithOriginalFactor;
graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K));
graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K));
graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K));
graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K));
graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K));
graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K));
graphWithOriginalFactor.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K));
graphWithOriginalFactor.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K));
graphWithOriginalFactor.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); graphWithOriginalFactor.push_back(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K));
graphWithOriginalFactor.add(PriorFactor<Pose3>(x1, pose1, noisePrior)); graphWithOriginalFactor.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graphWithOriginalFactor.add(PriorFactor<Pose3>(x2, pose2, noisePrior)); graphWithOriginalFactor.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
Values valuesOriginalFactor; Values valuesOriginalFactor;
@ -262,8 +257,8 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
graphWithSmartFactor.push_back(smartFactor1); graphWithSmartFactor.push_back(smartFactor1);
graphWithSmartFactor.push_back(smartFactor2); graphWithSmartFactor.push_back(smartFactor2);
graphWithSmartFactor.push_back(smartFactor3); graphWithSmartFactor.push_back(smartFactor3);
graphWithSmartFactor.add(PriorFactor<Pose3>(x1, pose1, noisePrior)); graphWithSmartFactor.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graphWithSmartFactor.add(PriorFactor<Pose3>(x2, pose2, noisePrior)); graphWithSmartFactor.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Values valuesSmartFactor; Values valuesSmartFactor;
valuesSmartFactor.insert(x1, pose1); valuesSmartFactor.insert(x1, pose1);
@ -442,22 +437,22 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
// 1. Project three landmarks into three cameras and triangulate // 1. Project three landmarks into three cameras and triangulate
graph.add(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K)); graph.push_back(ProjectionFactor(cam1.project(landmark1), noiseProjection, x1, L(1), K));
graph.add(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K)); graph.push_back(ProjectionFactor(cam2.project(landmark1), noiseProjection, x2, L(1), K));
graph.add(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K)); graph.push_back(ProjectionFactor(cam3.project(landmark1), noiseProjection, x3, L(1), K));
// //
graph.add(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K)); graph.push_back(ProjectionFactor(cam1.project(landmark2), noiseProjection, x1, L(2), K));
graph.add(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K)); graph.push_back(ProjectionFactor(cam2.project(landmark2), noiseProjection, x2, L(2), K));
graph.add(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K)); graph.push_back(ProjectionFactor(cam3.project(landmark2), noiseProjection, x3, L(2), K));
graph.add(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K)); graph.push_back(ProjectionFactor(cam1.project(landmark3), noiseProjection, x1, L(3), K));
graph.add(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K)); graph.push_back(ProjectionFactor(cam2.project(landmark3), noiseProjection, x2, L(3), K));
graph.add(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K)); graph.push_back(ProjectionFactor(cam3.project(landmark3), noiseProjection, x3, L(3), K));
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.add(PriorFactor<Pose3>(x1, pose1, noisePrior)); graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.add(PriorFactor<Pose3>(x2, pose2, noisePrior)); graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
Values values; Values values;
@ -520,11 +515,7 @@ TEST( SmartProjectionFactor, Hessian ){
values.insert(x2, pose2); values.insert(x2, pose2);
// values.insert(L(1), landmark1); // values.insert(L(1), landmark1);
Ordering ordering; boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor.linearize(values);
ordering.push_back(x1);
ordering.push_back(x2);
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor.linearize(values, ordering);
hessianFactor->print("Hessian factor \n"); hessianFactor->print("Hessian factor \n");
// compute triangulation from linearization point // compute triangulation from linearization point
@ -538,8 +529,6 @@ TEST( SmartProjectionFactor, Hessian ){
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */