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

Modifications to SmartProjectionFactor and unit test: work in progress

release/4.3a0
Luca Carlone 2013-08-05 15:09:03 +00:00
parent bf34b019e9
commit 39ec641c4a
2 changed files with 141 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -181,7 +181,46 @@ namespace gtsam {
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& values, const Ordering& ordering) const {
std::vector<Matrix> Hx(keys_.size());
std::vector<Matrix> Hl(keys_.size());
std::vector<Vector> b(keys_.size());
// Collect all poses (Cameras)
std::vector<Pose3> cameraPoses;
BOOST_FOREACH(const Key& k, keys_) {
if(body_P_sensor_)
cameraPoses.push_back(values.at<Pose3>(k).compose(*body_P_sensor_));
else
cameraPoses.push_back(values.at<Pose3>(k));
}
// We triangulate the 3D position of the landmark
boost::optional<Point3> point = triangulatePoint3(cameraPoses, measured_, *K_);
if(point){
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_);
b.at(i) = ( camera.project(*point,Hx.at(i),Hl.at(i)) - measured_.at(i) ).vector();
}
}
else{
return HessianFactor::shared_ptr(new HessianFactor());
}
// Allocate m^2 matrix blocks
std::vector< std::vector<Matrix> > Hxl(keys_.size(), std::vector<Matrix>( keys_.size()));
// Allocate inv(Hl'Hl)
Matrix3 C;
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
C += Hl.at(i1).transpose() * Hl.at(i1);
}
C = C.inverse();
// fill in the keys
std::vector<Index> js;
@ -189,25 +228,32 @@ namespace gtsam {
js += ordering[k];
}
// Calculate sub blocks
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
Hxl[i1][i2] = Hx.at(i1).transpose() * Hl.at(i1) * C * Hl.at(i2).transpose();
}
}
std::vector<Matrix> Gs;
std::vector<Vector> gs;
// Shur complement trick
// double e = u + b - z , e2 = e * e;
// double c = 2 * logSqrt2PI - log(p) + e2 * p;
// Vector g1 = Vector_(1, -e * p);
// Vector g2 = Vector_(1, 0.5 / p - 0.5 * e2);
// Vector g3 = Vector_(1, -e * p);
// Matrix G11 = Matrix_(1, 1, p);
// Matrix G12 = Matrix_(1, 1, e);
// Matrix G13 = Matrix_(1, 1, p);
// Matrix G22 = Matrix_(1, 1, 0.5 / (p * p));
// Matrix G23 = Matrix_(1, 1, e);
// Matrix G33 = Matrix_(1, 1, p);
// Populate Gs and gs
std::vector<Matrix> Gs(keys_.size()*(keys_.size()+1)/2);
std::vector<Vector> gs(keys_.size());
double f = 0;
int GsCount = 0;
for(size_t i1 = 0; i1 < keys_.size(); i1++) {
gs.at(i1) = Hx.at(i1).transpose() * b.at(i1);
for(size_t i2 = 0; i2 < keys_.size(); i2++) {
gs.at(i1) += Hxl[i1][i2] * b.at(i2);
if (i2 >= i1) {
Gs.at(GsCount) = Hx.at(i1).transpose() * Hx.at(i1) - Hxl[i1][i2] * Hx.at(i2);
GsCount++;
}
}
}
return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f));
}

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

@ -110,9 +110,6 @@ TEST( MultiProjectionFactor, noiseless ){
TEST( MultiProjectionFactor, noisy ){
cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl;
Values theta;
NonlinearFactorGraph graph;
Symbol x1('X', 1);
Symbol x2('X', 2);
// Symbol x3('X', 3);
@ -139,24 +136,98 @@ TEST( MultiProjectionFactor, noisy ){
Point2 level_uv = level_camera.project(landmark) + pixelError;
Point2 level_uv_right = level_camera_right.project(landmark);
Values value;
value.insert(x1, level_pose);
value.insert(x2, level_pose_right);
Values values;
values.insert(x1, level_pose);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
values.insert(x2, level_pose_right.compose(noise_pose));
// poses += level_pose, level_pose_right;
vector<Point2> measurements;
measurements += level_uv, level_uv_right;
SmartProjectionFactor<Pose3, Point3, Cal3_S2> smartFactor(measurements, noiseProjection, views, K);
SmartProjectionFactor<Pose3, Point3, Cal3_S2>::shared_ptr
smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K));
double actualError = smartFactor.error(value);
double actualError = smartFactor->error(values);
double expectedError = sqrt(0.08);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor);
graph.add(PriorFactor<Pose3>(x1, level_pose, noisePrior));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
result.print("results of the optimization \n");
// we do not expect to be able to predict the error, since the error on the pixel will change
// the triangulation of the landmark which is internal to the factor.
// DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
///* ************************************************************************* */
TEST( MultiProjectionFactor, 3poses ){
cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl;
Symbol x1('X', 1);
Symbol x2('X', 2);
Symbol x3('X', 3);
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
std::vector<Key> views;
views += x1, x2; //, x3;
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K);
// create second camera 1 meter to the right of first camera
Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera level_camera_right(level_pose_right, *K);
// landmark ~5 meters infront of camera
Point3 landmark(5, 0.5, 1.2);
// 1. Project two landmarks into two cameras and triangulate
Point2 pixelError(0.2,0.2);
Point2 level_uv = level_camera.project(landmark) + pixelError;
Point2 level_uv_right = level_camera_right.project(landmark);
Values values;
values.insert(x1, level_pose);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
values.insert(x2, level_pose_right.compose(noise_pose));
// poses += level_pose, level_pose_right;
vector<Point2> measurements;
measurements += level_uv, level_uv_right;
SmartProjectionFactor<Pose3, Point3, Cal3_S2>::shared_ptr
smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K));
double actualError = smartFactor->error(values);
double expectedError = sqrt(0.08);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor);
graph.add(PriorFactor<Pose3>(x1, level_pose, noisePrior));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
result.print("results of the optimization \n");
// we do not expect to be able to predict the error, since the error on the pixel will change
// the triangulation of the landmark which is internal to the factor.
// DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}
///* ************************************************************************* */
//TEST( ProjectionFactor, nonStandard ) {
// GenericProjectionFactor<Pose3, Point3, Cal3DS2> f;