diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index dc0fd0e64..7b63b7814 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -181,7 +181,46 @@ namespace gtsam { } /// linearize returns a Hessianfactor that is an approximation of error(p) - virtual boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + virtual boost::shared_ptr linearize(const Values& values, const Ordering& ordering) const { + + std::vector Hx(keys_.size()); + std::vector Hl(keys_.size()); + std::vector b(keys_.size()); + + // Collect all poses (Cameras) + std::vector cameraPoses; + + BOOST_FOREACH(const Key& k, keys_) { + if(body_P_sensor_) + cameraPoses.push_back(values.at(k).compose(*body_P_sensor_)); + else + cameraPoses.push_back(values.at(k)); + } + + // We triangulate the 3D position of the landmark + boost::optional point = triangulatePoint3(cameraPoses, measured_, *K_); + + if(point){ + for(size_t i = 0; i < measured_.size(); i++) { + Pose3 pose = cameraPoses.at(i); + PinholeCamera 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 > Hxl(keys_.size(), std::vector( 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 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 Gs; - - std::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 Gs(keys_.size()*(keys_.size()+1)/2); + std::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)); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index c7bb9f483..f152b7def 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -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 measurements; measurements += level_uv, level_uv_right; - SmartProjectionFactor smartFactor(measurements, noiseProjection, views, K); + SmartProjectionFactor::shared_ptr + smartFactor(new SmartProjectionFactor(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(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 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 measurements; + measurements += level_uv, level_uv_right; + + SmartProjectionFactor::shared_ptr + smartFactor(new SmartProjectionFactor(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(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 f;