SmartProjectionFactor: Added add(measured, poseKey) method to incrementally add views to the same landmark.
Added unit test for new method and way of creating factor. Fixed missing negative when calculating b for non-blockwise version of Schur complementrelease/4.3a0
parent
a5826dff93
commit
fe860be33f
|
@ -101,6 +101,17 @@ namespace gtsam {
|
|||
measured_(measured), K_(K), noise_(model), point_(point), body_P_sensor_(body_P_sensor),
|
||||
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
|
||||
|
||||
/**
|
||||
* Constructor with exception-handling flags
|
||||
* @param model is the standard deviation (current version assumes that the uncertainty is the same for all views)
|
||||
* @param K shared pointer to the constant calibration
|
||||
*/
|
||||
SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr<CALIBRATION>& K,
|
||||
boost::optional<LANDMARK> point = boost::none,
|
||||
boost::optional<POSE> body_P_sensor = boost::none) :
|
||||
noise_(model), K_(K), point_(point), body_P_sensor_(body_P_sensor) {
|
||||
}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionFactor() {}
|
||||
|
||||
|
@ -109,6 +120,16 @@ namespace gtsam {
|
|||
// return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
||||
// gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
||||
|
||||
/**
|
||||
* add
|
||||
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
|
||||
* @param poseKey is the index corresponding to the camera observing the same landmark
|
||||
*/
|
||||
void add(const Point2 measured, const Key poseKey) {
|
||||
measured_.push_back(measured);
|
||||
keys_.push_back(poseKey);
|
||||
}
|
||||
|
||||
/**
|
||||
* print
|
||||
* @param s optional string naming the factor
|
||||
|
@ -297,7 +318,7 @@ namespace gtsam {
|
|||
Pose3 pose = cameraPoses.at(i);
|
||||
PinholeCamera<CALIBRATION> camera(pose, *K_);
|
||||
Matrix Hxi, Hli;
|
||||
Vector bi = ( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
Vector bi = -( camera.project(*point,Hxi,Hli) - measured_.at(i) ).vector();
|
||||
|
||||
noise_-> WhitenSystem(Hxi, Hli, bi);
|
||||
f += bi.squaredNorm();
|
||||
|
|
|
@ -309,7 +309,6 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){
|
|||
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
|
||||
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
|
||||
|
@ -404,6 +403,110 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){
|
|||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){
|
||||
cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << 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 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
|
||||
SimpleCamera cam1(pose1, *K);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
|
||||
SimpleCamera cam2(pose2, *K);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
|
||||
SimpleCamera cam3(pose3, *K);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||
Point2 cam3_uv1 = cam3.project(landmark1);
|
||||
measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1;
|
||||
|
||||
//
|
||||
Point2 cam1_uv2 = cam1.project(landmark2);
|
||||
Point2 cam2_uv2 = cam2.project(landmark2);
|
||||
Point2 cam3_uv2 = cam3.project(landmark2);
|
||||
measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2;
|
||||
|
||||
|
||||
Point2 cam1_uv3 = cam1.project(landmark3);
|
||||
Point2 cam2_uv3 = cam2.project(landmark3);
|
||||
Point2 cam3_uv3 = cam3.project(landmark3);
|
||||
measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3;
|
||||
|
||||
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(noiseProjection, K));
|
||||
smartFactor1->add(cam1_uv1, views[0]);
|
||||
smartFactor1->add(cam2_uv1, views[1]);
|
||||
smartFactor1->add(cam3_uv1, views[2]);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(noiseProjection, K));
|
||||
smartFactor2->add(cam1_uv2, views[0]);
|
||||
smartFactor2->add(cam2_uv2, views[1]);
|
||||
smartFactor2->add(cam3_uv2, views[2]);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(noiseProjection, K));
|
||||
smartFactor3->add(cam1_uv3, views[0]);
|
||||
smartFactor3->add(cam2_uv3, views[1]);
|
||||
smartFactor3->add(cam3_uv3, views[2]);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.push_back(PriorFactor<Pose3>(x1, pose1, 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)); // noise from regular projection factor test below
|
||||
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, pose1);
|
||||
values.insert(x2, pose2);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose3
|
||||
values.insert(x3, pose3*noise_pose);
|
||||
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
gttic_(SmartProjectionFactor);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartProjectionFactor);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
// result.print("results of 3 camera, 3 landmark optimization \n");
|
||||
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
|
||||
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
|
||||
tictoc_print_();
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionFactor, 3poses_projection_factor ){
|
||||
|
|
Loading…
Reference in New Issue