added test which optimize smartStereoFactor with missing measurements (uR)
							parent
							
								
									b90e224f59
								
							
						
					
					
						commit
						6c163b0a4d
					
				|  | @ -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<Dim; ii++) | ||||
|             Fi(1,ii) = 0.0; | ||||
|  |  | |||
|  | @ -60,6 +60,8 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re | |||
| static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), | ||||
|     Point3(0.25, -0.10, 1.0)); | ||||
| 
 | ||||
| static double missing_uR = std::numeric_limits<double>::quiet_NaN(); | ||||
| 
 | ||||
| vector<StereoPoint2> 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<double>::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<Pose3>(x3))); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) { | ||||
| 
 | ||||
|   vector<Key> 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<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, | ||||
|       cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> 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<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), 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<Pose3>(x3),1e-7)); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue