diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 160395997..eacca44b5 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -230,7 +230,6 @@ public: Z z3 = measured_.at(i); if(isnan(z3.vector()[1])){ // .. and the right pixel is invalid // delete influence of right point on jacobian Fs - std::cout << "unwhitenedError:: isnan(z3->uR()" << z3.vector() << std::endl; MatrixZD& Fi = Fs->at(i); for(size_t ii=0; ii::quiet_NaN(); + vector stereo_projectToMultipleCameras(const StereoCamera& cam1, const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) { @@ -167,7 +169,6 @@ TEST( SmartProjectionPoseFactor, noiselessWithMissingMeasurements ) { // 1. Project two landmarks into two cameras and triangulate StereoPoint2 level_uv = level_camera.project(landmark); StereoPoint2 level_uv_right = level_camera_right.project(landmark); - double missing_uR = std::numeric_limits::quiet_NaN(); StereoPoint2 level_uv_right_missing(level_uv_right.uL(),missing_uR,level_uv_right.v()); Values values; @@ -462,6 +463,78 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { EXPECT(assert_equal(pose3, result.at(x3))); } +/* *************************************************************************/ +TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { + + vector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + // 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), Point3(0, 0, 1)); + StereoCamera cam1(pose1, K); + // create second camera 1 meter to the right of first camera + Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)); + StereoCamera cam2(pose2, K); + // create third camera 1 meter above the first camera + Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0)); + StereoCamera 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); + + // 1. Project three landmarks into three cameras and triangulate + vector measurements_cam1 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark1); + vector measurements_cam2 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark2); + vector measurements_cam3 = stereo_projectToMultipleCameras(cam1, + cam2, cam3, landmark3); + + // DELETE SOME MEASUREMENTS + StereoPoint2 sp = measurements_cam1[1]; + measurements_cam1[1] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + sp = measurements_cam2[2]; + measurements_cam2[2] = StereoPoint2(sp.uL(), missing_uR, sp.v()); + + SmartStereoProjectionParams params; + params.setLinearizationMode(JACOBIAN_SVD); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params)); + smartFactor1->add(measurements_cam1, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor2->add(measurements_cam2, views, K); + + SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params)); + smartFactor3->add(measurements_cam3, views, K); + + 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(x1, pose1, noisePrior)); + graph.push_back(PriorFactor(x2, pose2, 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), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + values.insert(x3, pose3 * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); + result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3),1e-7)); +} + /* *************************************************************************/ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {