fixed another test, few more to go
							parent
							
								
									1c3ff0580b
								
							
						
					
					
						commit
						934413522d
					
				|  | @ -52,7 +52,7 @@ PinholePose<CALIBRATION> > { | |||
|   std::vector<std::pair<Key, Key>> world_P_body_key_pairs_; | ||||
| 
 | ||||
|   /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses
 | ||||
|   std::vector<double> gammas_; | ||||
|   std::vector<double> interp_param_; | ||||
| 
 | ||||
|   /// Pose of the camera in the body frame
 | ||||
|   std::vector<Pose3> body_P_sensors_;  ///< Pose of the camera in the body frame
 | ||||
|  | @ -117,7 +117,7 @@ PinholePose<CALIBRATION> > { | |||
|       this->keys_.push_back(world_P_body_key2);  // add only unique keys
 | ||||
| 
 | ||||
|     // store interpolation factors
 | ||||
|     gammas_.push_back(gamma); | ||||
|     interp_param_.push_back(gamma); | ||||
| 
 | ||||
|     // store fixed calibration
 | ||||
|     K_all_.push_back(K); | ||||
|  | @ -180,7 +180,7 @@ PinholePose<CALIBRATION> > { | |||
| 
 | ||||
|   /// return the interpolation factors gammas
 | ||||
|   const std::vector<double> getGammas() const { | ||||
|     return gammas_; | ||||
|     return interp_param_; | ||||
|   } | ||||
| 
 | ||||
|   /// return the extrinsic camera calibration body_P_sensors
 | ||||
|  | @ -202,7 +202,7 @@ PinholePose<CALIBRATION> > { | |||
|           << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; | ||||
|       std::cout << " pose2 key: " | ||||
|           << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; | ||||
|       std::cout << " gamma: " << gammas_[i] << std::endl; | ||||
|       std::cout << " gamma: " << interp_param_[i] << std::endl; | ||||
|       body_P_sensors_[i].print("extrinsic calibration:\n"); | ||||
|       K_all_[i]->print("intrinsic calibration = "); | ||||
|     } | ||||
|  | @ -237,7 +237,7 @@ PinholePose<CALIBRATION> > { | |||
|     }else{ extrinsicCalibrationEqual = false; } | ||||
| 
 | ||||
|     return e && Base::equals(p, tol) && K_all_ == e->calibration() | ||||
|         && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; | ||||
|         && interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -264,7 +264,7 @@ PinholePose<CALIBRATION> > { | |||
|       for (size_t i = 0; i < numViews; i++) {  // for each camera/measurement
 | ||||
|         Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); | ||||
|         Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); | ||||
|         double interpolationFactor = gammas_[i]; | ||||
|         double interpolationFactor = interp_param_[i]; | ||||
|         // get interpolated pose:
 | ||||
|         Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); | ||||
|         Pose3 body_P_cam = body_P_sensors_[i]; | ||||
|  | @ -322,7 +322,7 @@ PinholePose<CALIBRATION> > { | |||
| 
 | ||||
|     // compute Jacobian given triangulated 3D Point
 | ||||
|     FBlocks Fs; | ||||
|     Matrix F, E; | ||||
|     Matrix E; | ||||
|     Vector b; | ||||
|     this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); | ||||
| 
 | ||||
|  | @ -369,7 +369,7 @@ PinholePose<CALIBRATION> > { | |||
|   typename Base::Cameras cameras(const Values& values) const override { | ||||
|     size_t numViews = this->measured_.size(); | ||||
|     assert(numViews == K_all_.size()); | ||||
|     assert(numViews == gammas_.size()); | ||||
|     assert(numViews == interp_param_.size()); | ||||
|     assert(numViews == body_P_sensors_.size()); | ||||
|     assert(numViews == world_P_body_key_pairs_.size()); | ||||
| 
 | ||||
|  | @ -377,7 +377,7 @@ PinholePose<CALIBRATION> > { | |||
|     for (size_t i = 0; i < numViews; i++) {  // for each measurement
 | ||||
|       Pose3 w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); | ||||
|       Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); | ||||
|       double interpolationFactor = gammas_[i]; | ||||
|       double interpolationFactor = interp_param_[i]; | ||||
|       Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); | ||||
|       Pose3 body_P_cam = body_P_sensors_[i]; | ||||
|       Pose3 w_P_cam = w_P_body.compose(body_P_cam); | ||||
|  |  | |||
|  | @ -298,8 +298,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { | |||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { | ||||
|   std::cout << "===================" << std::endl; | ||||
| TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { | ||||
| 
 | ||||
|   using namespace vanillaPoseRS; | ||||
|   Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; | ||||
|  | @ -365,173 +364,101 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) | |||
|   EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************
 | ||||
|  TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { | ||||
|   // here we replicate a test in SmartProjectionPoseFactor by setting interpolation
 | ||||
|   // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements)
 | ||||
|   // Note: this is a quite extreme test since in typical camera you would not have more than
 | ||||
|   // 1 measurement per landmark at each interpolated pose
 | ||||
|   using namespace vanillaPose; | ||||
| 
 | ||||
|  using namespace vanillaPose; | ||||
|   // Default cameras for simple derivatives
 | ||||
|   static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); | ||||
| 
 | ||||
|  // Default cameras for simple derivatives
 | ||||
|  Rot3 R; | ||||
|  static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); | ||||
|  Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( | ||||
|  Pose3(R, Point3(1, 0, 0)), sharedK); | ||||
|   Rot3 R = Rot3::identity(); | ||||
|   Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); | ||||
|   Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); | ||||
|   Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); | ||||
|   Pose3 body_P_sensorId = Pose3::identity(); | ||||
| 
 | ||||
|  // one landmarks 1m in front of camera
 | ||||
|  Point3 landmark1(0, 0, 10); | ||||
|   // one landmarks 1m in front of camera
 | ||||
|   Point3 landmark1(0, 0, 10); | ||||
| 
 | ||||
|  Point2Vector measurements_cam1; | ||||
|   Point2Vector measurements_cam1; | ||||
| 
 | ||||
|  // Project 2 landmarks into 2 cameras
 | ||||
|  measurements_cam1.push_back(cam1.project(landmark1)); | ||||
|  measurements_cam1.push_back(cam2.project(landmark1)); | ||||
|   // Project 2 landmarks into 2 cameras
 | ||||
|   measurements_cam1.push_back(cam1.project(landmark1)); | ||||
|   measurements_cam1.push_back(cam2.project(landmark1)); | ||||
| 
 | ||||
|  // Create smart factors
 | ||||
|  KeyVector views {x1, x2}; | ||||
|   SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); | ||||
|   double interp_factor = 0;  // equivalent to measurement taken at pose 1
 | ||||
|   smartFactor1->add(measurements_cam1[0], x1, x2, interp_factor, sharedKSimple, | ||||
|                     body_P_sensorId); | ||||
|   interp_factor = 1;  // equivalent to measurement taken at pose 2
 | ||||
|   smartFactor1->add(measurements_cam1[1], x1, x2, interp_factor, sharedKSimple, | ||||
|                     body_P_sensorId); | ||||
| 
 | ||||
|  SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK); | ||||
|  smartFactor1->add(measurements_cam1, views); | ||||
|   SmartFactor::Cameras cameras; | ||||
|   cameras.push_back(cam1); | ||||
|   cameras.push_back(cam2); | ||||
| 
 | ||||
|  SmartFactor::Cameras cameras; | ||||
|  cameras.push_back(cam1); | ||||
|  cameras.push_back(cam2); | ||||
|   // Make sure triangulation works
 | ||||
|   CHECK(smartFactor1->triangulateSafe(cameras)); | ||||
|   CHECK(!smartFactor1->isDegenerate()); | ||||
|   CHECK(!smartFactor1->isPointBehindCamera()); | ||||
|   boost::optional<Point3> p = smartFactor1->point(); | ||||
|   CHECK(p); | ||||
|   EXPECT(assert_equal(landmark1, *p)); | ||||
| 
 | ||||
|  // Make sure triangulation works
 | ||||
|  CHECK(smartFactor1->triangulateSafe(cameras)); | ||||
|  CHECK(!smartFactor1->isDegenerate()); | ||||
|  CHECK(!smartFactor1->isPointBehindCamera()); | ||||
|  boost::optional<Point3> p = smartFactor1->point(); | ||||
|  CHECK(p); | ||||
|  EXPECT(assert_equal(landmark1, *p)); | ||||
|   VectorValues zeroDelta; | ||||
|   Vector6 delta; | ||||
|   delta.setZero(); | ||||
|   zeroDelta.insert(x1, delta); | ||||
|   zeroDelta.insert(x2, delta); | ||||
| 
 | ||||
|  VectorValues zeroDelta; | ||||
|  Vector6 delta; | ||||
|  delta.setZero(); | ||||
|  zeroDelta.insert(x1, delta); | ||||
|  zeroDelta.insert(x2, delta); | ||||
|   VectorValues perturbedDelta; | ||||
|   delta.setOnes(); | ||||
|   perturbedDelta.insert(x1, delta); | ||||
|   perturbedDelta.insert(x2, delta); | ||||
|   double expectedError = 2500; | ||||
| 
 | ||||
|  VectorValues perturbedDelta; | ||||
|  delta.setOnes(); | ||||
|  perturbedDelta.insert(x1, delta); | ||||
|  perturbedDelta.insert(x2, delta); | ||||
|  double expectedError = 2500; | ||||
|   // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
 | ||||
|   Matrix16 A1, A2; | ||||
|   A1 << -10, 0, 0, 0, 1, 0; | ||||
|   A2 << 10, 0, 1, 0, -1, 0; | ||||
|   A1 *= 10. / sigma; | ||||
|   A2 *= 10. / sigma; | ||||
|   Matrix expectedInformation;  // filled below
 | ||||
| 
 | ||||
|  // After eliminating the point, A1 and A2 contain 2-rank information on cameras:
 | ||||
|  Matrix16 A1, A2; | ||||
|  A1 << -10, 0, 0, 0, 1, 0; | ||||
|  A2 << 10, 0, 1, 0, -1, 0; | ||||
|  A1 *= 10. / sigma; | ||||
|  A2 *= 10. / sigma; | ||||
|  Matrix expectedInformation; // filled below
 | ||||
|  { | ||||
|  // createHessianFactor
 | ||||
|  Matrix66 G11 = 0.5 * A1.transpose() * A1; | ||||
|  Matrix66 G12 = 0.5 * A1.transpose() * A2; | ||||
|  Matrix66 G22 = 0.5 * A2.transpose() * A2; | ||||
|   // createHessianFactor
 | ||||
|   Matrix66 G11 = 0.5 * A1.transpose() * A1; | ||||
|   Matrix66 G12 = 0.5 * A1.transpose() * A2; | ||||
|   Matrix66 G22 = 0.5 * A2.transpose() * A2; | ||||
| 
 | ||||
|  Vector6 g1; | ||||
|  g1.setZero(); | ||||
|  Vector6 g2; | ||||
|  g2.setZero(); | ||||
|   Vector6 g1; | ||||
|   g1.setZero(); | ||||
|   Vector6 g2; | ||||
|   g2.setZero(); | ||||
| 
 | ||||
|  double f = 0; | ||||
|   double f = 0; | ||||
| 
 | ||||
|  RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); | ||||
|  expectedInformation = expected.information(); | ||||
|   RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); | ||||
|   expectedInformation = expected.information(); | ||||
| 
 | ||||
|  boost::shared_ptr<RegularHessianFactor<6> > actual = | ||||
|  smartFactor1->createHessianFactor(cameras, 0.0); | ||||
|  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); | ||||
|  EXPECT(assert_equal(expected, *actual, 1e-6)); | ||||
|  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); | ||||
|  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); | ||||
|  } | ||||
| 
 | ||||
|  { | ||||
|  Matrix26 F1; | ||||
|  F1.setZero(); | ||||
|  F1(0, 1) = -100; | ||||
|  F1(0, 3) = -10; | ||||
|  F1(1, 0) = 100; | ||||
|  F1(1, 4) = -10; | ||||
|  Matrix26 F2; | ||||
|  F2.setZero(); | ||||
|  F2(0, 1) = -101; | ||||
|  F2(0, 3) = -10; | ||||
|  F2(0, 5) = -1; | ||||
|  F2(1, 0) = 100; | ||||
|  F2(1, 2) = 10; | ||||
|  F2(1, 4) = -10; | ||||
|  Matrix E(4, 3); | ||||
|  E.setZero(); | ||||
|  E(0, 0) = 10; | ||||
|  E(1, 1) = 10; | ||||
|  E(2, 0) = 10; | ||||
|  E(2, 2) = 1; | ||||
|  E(3, 1) = 10; | ||||
|  SmartFactor::FBlocks Fs = list_of<Matrix>(F1)(F2); | ||||
|  Vector b(4); | ||||
|  b.setZero(); | ||||
| 
 | ||||
|  // Create smart factors
 | ||||
|  KeyVector keys; | ||||
|  keys.push_back(x1); | ||||
|  keys.push_back(x2); | ||||
| 
 | ||||
|  // createJacobianQFactor
 | ||||
|  SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); | ||||
|  Matrix3 P = (E.transpose() * E).inverse(); | ||||
|  JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); | ||||
|  EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); | ||||
| 
 | ||||
|  boost::shared_ptr<JacobianFactorQ<6, 2> > actualQ = | ||||
|  smartFactor1->createJacobianQFactor(cameras, 0.0); | ||||
|  CHECK(actualQ); | ||||
|  EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); | ||||
|  EXPECT(assert_equal(expectedQ, *actualQ)); | ||||
|  EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); | ||||
|  EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); | ||||
| 
 | ||||
|  // Whiten for RegularImplicitSchurFactor (does not have noise model)
 | ||||
|  model->WhitenSystem(E, b); | ||||
|  Matrix3 whiteP = (E.transpose() * E).inverse(); | ||||
|  Fs[0] = model->Whiten(Fs[0]); | ||||
|  Fs[1] = model->Whiten(Fs[1]); | ||||
| 
 | ||||
|  // createRegularImplicitSchurFactor
 | ||||
|  RegularImplicitSchurFactor<Camera> expected(keys, Fs, E, whiteP, b); | ||||
| 
 | ||||
|  boost::shared_ptr<RegularImplicitSchurFactor<Camera> > actual = | ||||
|  smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); | ||||
|  CHECK(actual); | ||||
|  EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); | ||||
|  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); | ||||
|  EXPECT(assert_equal(expected, *actual)); | ||||
|  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); | ||||
|  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); | ||||
|  } | ||||
| 
 | ||||
|  { | ||||
|  // createJacobianSVDFactor
 | ||||
|  Vector1 b; | ||||
|  b.setZero(); | ||||
|  double s = sigma * sin(M_PI_4); | ||||
|  SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); | ||||
|  JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); | ||||
|  EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); | ||||
| 
 | ||||
|  boost::shared_ptr<JacobianFactor> actual = | ||||
|  smartFactor1->createJacobianSVDFactor(cameras, 0.0); | ||||
|  CHECK(actual); | ||||
|  EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); | ||||
|  EXPECT(assert_equal(expected, *actual)); | ||||
|  EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); | ||||
|  EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); | ||||
|  } | ||||
|  } | ||||
|   Values values; | ||||
|   values.insert(x1, pose1); | ||||
|   values.insert(x2, pose2); | ||||
|   boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 | ||||
|       ->createHessianFactor(values); | ||||
|   EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); | ||||
|   EXPECT(assert_equal(expected, *actual, 1e-6)); | ||||
|   EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); | ||||
|   EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); | ||||
| } | ||||
| 
 | ||||
|  /* *************************************************************************
 | ||||
|  TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { | ||||
| 
 | ||||
|   std::cout << "===================" << std::endl; | ||||
|  using namespace vanillaPose; | ||||
| 
 | ||||
|  KeyVector views {x1, x2, x3}; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue