renamed interp param to alpha, improved comments, added cpp
							parent
							
								
									a3ee695b85
								
							
						
					
					
						commit
						0a8ebdf8cd
					
				|  | @ -320,13 +320,14 @@ T expm(const Vector& x, int K=7) { | |||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Linear interpolation between X and Y by coefficient t in [0, 1.5] (t>1 implies extrapolation), with optional jacobians. | ||||
|  * Linear interpolation between X and Y by coefficient t (typically t \in [0,1], | ||||
|  * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians. | ||||
|  */ | ||||
| template <typename T> | ||||
| T interpolate(const T& X, const T& Y, double t, | ||||
|               typename MakeOptionalJacobian<T, T>::type Hx = boost::none, | ||||
|               typename MakeOptionalJacobian<T, T>::type Hy = boost::none) { | ||||
|   assert(t >= 0.0 && t <= 1.5); | ||||
| 
 | ||||
|   if (Hx || Hy) { | ||||
|     typename traits<T>::TangentVector log_Xinv_Y; | ||||
|     typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x; | ||||
|  |  | |||
|  | @ -0,0 +1,73 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file ProjectionFactorRollingShutter.cpp | ||||
|  * @brief Basic projection factor for rolling shutter cameras | ||||
|  * @author Yotam Stern | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam_unstable/slam/ProjectionFactorRollingShutter.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| Vector ProjectionFactorRollingShutter::evaluateError( | ||||
|     const Pose3& pose_a, const Pose3& pose_b, const Point3& point, | ||||
|     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2, | ||||
|     boost::optional<Matrix&> H3) const { | ||||
| 
 | ||||
|   try { | ||||
|     Pose3 pose = interpolate<Pose3>(pose_a, pose_b, alpha_, H1, H2); | ||||
|     gtsam::Matrix Hprj; | ||||
|     if (body_P_sensor_) { | ||||
|       if (H1 || H2 || H3) { | ||||
|         gtsam::Matrix HbodySensor; | ||||
|         PinholeCamera<Cal3_S2> camera( | ||||
|             pose.compose(*body_P_sensor_, HbodySensor), *K_); | ||||
|         Point2 reprojectionError( | ||||
|             camera.project(point, Hprj, H3, boost::none) - measured_); | ||||
|         if (H1) | ||||
|           *H1 = Hprj * HbodySensor * (*H1); | ||||
|         if (H2) | ||||
|           *H2 = Hprj * HbodySensor * (*H2); | ||||
|         return reprojectionError; | ||||
|       } else { | ||||
|         PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_); | ||||
|         return camera.project(point) - measured_; | ||||
|       } | ||||
|     } else { | ||||
|       PinholeCamera<Cal3_S2> camera(pose, *K_); | ||||
|       Point2 reprojectionError( | ||||
|           camera.project(point, Hprj, H3, boost::none) - measured_); | ||||
|       if (H1) | ||||
|         *H1 = Hprj * (*H1); | ||||
|       if (H2) | ||||
|         *H2 = Hprj * (*H2); | ||||
|       return reprojectionError; | ||||
|     } | ||||
|   } catch (CheiralityException& e) { | ||||
|     if (H1) | ||||
|       *H1 = Matrix::Zero(2, 6); | ||||
|     if (H2) | ||||
|       *H2 = Matrix::Zero(2, 6); | ||||
|     if (H3) | ||||
|       *H3 = Matrix::Zero(2, 3); | ||||
|     if (verboseCheirality_) | ||||
|       std::cout << e.what() << ": Landmark " | ||||
|           << DefaultKeyFormatter(this->key2()) << " moved behind camera " | ||||
|           << DefaultKeyFormatter(this->key1()) << std::endl; | ||||
|     if (throwCheirality_) | ||||
|       throw CheiralityException(this->key2()); | ||||
|   } | ||||
|   return Vector2::Constant(2.0 * K_->fx()); | ||||
| } | ||||
| 
 | ||||
| }  //namespace gtsam
 | ||||
|  | @ -31,8 +31,8 @@ namespace gtsam { | |||
|  * and a Point2 measurement taken starting at time A using a rolling shutter camera. | ||||
|  * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B) | ||||
|  * corresponding to the time of exposure of the row of the image the pixel belongs to. | ||||
|  * Let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by | ||||
|  * the interp_param to project the corresponding landmark to Point2. | ||||
|  * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by | ||||
|  * the alpha to project the corresponding landmark to Point2. | ||||
|  * @addtogroup SLAM | ||||
|  */ | ||||
| 
 | ||||
|  | @ -42,7 +42,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
| 
 | ||||
|   // Keep a copy of measurement and calibration for I/O
 | ||||
|   Point2 measured_;                   ///< 2D measurement
 | ||||
|   double interp_param_;  ///< interpolation parameter in [0,1] corresponding to the point2 measurement
 | ||||
|   double alpha_;  ///< interpolation parameter in [0,1] corresponding to the point2 measurement
 | ||||
|   boost::shared_ptr<Cal3_S2> K_;  ///< shared pointer to calibration object
 | ||||
|   boost::optional<Pose3> body_P_sensor_;  ///< The pose of the sensor in the body frame
 | ||||
| 
 | ||||
|  | @ -64,7 +64,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|   /// Default constructor
 | ||||
|   ProjectionFactorRollingShutter() | ||||
|       : measured_(0, 0), | ||||
|         interp_param_(0), | ||||
|         alpha_(0), | ||||
|         throwCheirality_(false), | ||||
|         verboseCheirality_(false) { | ||||
|   } | ||||
|  | @ -72,7 +72,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|   /**
 | ||||
|    * Constructor | ||||
|    * @param measured is the 2-dimensional pixel location of point in the image (the measurement) | ||||
|    * @param interp_param is the rolling shutter parameter for the measurement | ||||
|    * @param alpha in [0,1] is the rolling shutter parameter for the measurement | ||||
|    * @param model is the noise model | ||||
|    * @param poseKey_a is the key of the first camera | ||||
|    * @param poseKey_b is the key of the second camera | ||||
|  | @ -80,7 +80,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|    * @param K shared pointer to the constant calibration | ||||
|    * @param body_P_sensor is the transform from body to sensor frame (default identity) | ||||
|    */ | ||||
|   ProjectionFactorRollingShutter(const Point2& measured, double interp_param, | ||||
|   ProjectionFactorRollingShutter(const Point2& measured, double alpha, | ||||
|                                  const SharedNoiseModel& model, Key poseKey_a, | ||||
|                                  Key poseKey_b, Key pointKey, | ||||
|                                  const boost::shared_ptr<Cal3_S2>& K, | ||||
|  | @ -88,7 +88,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|                                      boost::none) | ||||
|       : Base(model, poseKey_a, poseKey_b, pointKey), | ||||
|         measured_(measured), | ||||
|         interp_param_(interp_param), | ||||
|         alpha_(alpha), | ||||
|         K_(K), | ||||
|         body_P_sensor_(body_P_sensor), | ||||
|         throwCheirality_(false), | ||||
|  | @ -98,7 +98,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|   /**
 | ||||
|    * Constructor with exception-handling flags | ||||
|    * @param measured is the 2-dimensional pixel location of point in the image (the measurement) | ||||
|    * @param interp_param is the rolling shutter parameter for the measurement | ||||
|    * @param alpha in [0,1] is the rolling shutter parameter for the measurement | ||||
|    * @param model is the noise model | ||||
|    * @param poseKey_a is the key of the first camera | ||||
|    * @param poseKey_b is the key of the second camera | ||||
|  | @ -108,7 +108,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|    * @param verboseCheirality determines whether exceptions are printed for Cheirality | ||||
|    * @param body_P_sensor is the transform from body to sensor frame  (default identity) | ||||
|    */ | ||||
|   ProjectionFactorRollingShutter(const Point2& measured, double interp_param, | ||||
|   ProjectionFactorRollingShutter(const Point2& measured, double alpha, | ||||
|                                  const SharedNoiseModel& model, Key poseKey_a, | ||||
|                                  Key poseKey_b, Key pointKey, | ||||
|                                  const boost::shared_ptr<Cal3_S2>& K, | ||||
|  | @ -117,7 +117,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|                                      boost::none) | ||||
|       : Base(model, poseKey_a, poseKey_b, pointKey), | ||||
|         measured_(measured), | ||||
|         interp_param_(interp_param), | ||||
|         alpha_(alpha), | ||||
|         K_(K), | ||||
|         body_P_sensor_(body_P_sensor), | ||||
|         throwCheirality_(throwCheirality), | ||||
|  | @ -143,7 +143,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|                  DefaultKeyFormatter) const { | ||||
|     std::cout << s << "ProjectionFactorRollingShutter, z = "; | ||||
|     traits<Point2>::Print(measured_); | ||||
|     std::cout << " rolling shutter interpolation param = " << interp_param_; | ||||
|     std::cout << " rolling shutter interpolation param = " << alpha_; | ||||
|     if (this->body_P_sensor_) | ||||
|       this->body_P_sensor_->print("  sensor pose in body frame: "); | ||||
|     Base::print("", keyFormatter); | ||||
|  | @ -152,7 +152,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|   /// equals
 | ||||
|   virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { | ||||
|     const This *e = dynamic_cast<const This*>(&p); | ||||
|     return e && Base::equals(p, tol) && (interp_param_ == e->interp_param()) | ||||
|     return e && Base::equals(p, tol) && (alpha_ == e->alpha()) | ||||
|         && traits<Point2>::Equals(this->measured_, e->measured_, tol) | ||||
|         && this->K_->equals(*e->K_, tol) | ||||
|         && (this->throwCheirality_ == e->throwCheirality_) | ||||
|  | @ -167,53 +167,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|                        const Point3& point, boost::optional<Matrix&> H1 = | ||||
|                            boost::none, | ||||
|                        boost::optional<Matrix&> H2 = boost::none, | ||||
|                        boost::optional<Matrix&> H3 = boost::none) const { | ||||
| 
 | ||||
|     try { | ||||
|       Pose3 pose = interpolate<Pose3>(pose_a, pose_b, interp_param_, H1, H2); | ||||
|       gtsam::Matrix Hprj; | ||||
|       if (body_P_sensor_) { | ||||
|         if (H1 || H2 || H3) { | ||||
|           gtsam::Matrix HbodySensor; | ||||
|           PinholeCamera<Cal3_S2> camera( | ||||
|               pose.compose(*body_P_sensor_, HbodySensor), *K_); | ||||
|           Point2 reprojectionError( | ||||
|               camera.project(point, Hprj, H3, boost::none) - measured_); | ||||
|           if (H1) | ||||
|             *H1 = Hprj * HbodySensor * (*H1); | ||||
|           if (H2) | ||||
|             *H2 = Hprj * HbodySensor * (*H2); | ||||
|           return reprojectionError; | ||||
|         } else { | ||||
|           PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_); | ||||
|           return camera.project(point) - measured_; | ||||
|         } | ||||
|       } else { | ||||
|         PinholeCamera<Cal3_S2> camera(pose, *K_); | ||||
|         Point2 reprojectionError( | ||||
|             camera.project(point, Hprj, H3, boost::none) - measured_); | ||||
|         if (H1) | ||||
|           *H1 = Hprj * (*H1); | ||||
|         if (H2) | ||||
|           *H2 = Hprj * (*H2); | ||||
|         return reprojectionError; | ||||
|       } | ||||
|     } catch (CheiralityException& e) { | ||||
|       if (H1) | ||||
|         *H1 = Matrix::Zero(2, 6); | ||||
|       if (H2) | ||||
|         *H2 = Matrix::Zero(2, 6); | ||||
|       if (H3) | ||||
|         *H3 = Matrix::Zero(2, 3); | ||||
|       if (verboseCheirality_) | ||||
|         std::cout << e.what() << ": Landmark " | ||||
|             << DefaultKeyFormatter(this->key2()) << " moved behind camera " | ||||
|             << DefaultKeyFormatter(this->key1()) << std::endl; | ||||
|       if (throwCheirality_) | ||||
|         throw CheiralityException(this->key2()); | ||||
|     } | ||||
|     return Vector2::Constant(2.0 * K_->fx()); | ||||
|   } | ||||
|                        boost::optional<Matrix&> H3 = boost::none) const; | ||||
| 
 | ||||
|   /** return the measurement */ | ||||
|   const Point2& measured() const { | ||||
|  | @ -226,8 +180,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|   } | ||||
| 
 | ||||
|   /** returns the rolling shutter interp param*/ | ||||
|   inline double interp_param() const { | ||||
|     return interp_param_; | ||||
|   inline double alpha() const { | ||||
|     return alpha_; | ||||
|   } | ||||
| 
 | ||||
|   /** return verbosity */ | ||||
|  | @ -248,7 +202,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, | |||
|   void serialize(ARCHIVE & ar, const unsigned int /*version*/) { | ||||
|     ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); | ||||
|     ar & BOOST_SERIALIZATION_NVP(measured_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(interp_param_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(alpha_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(K_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); | ||||
|     ar & BOOST_SERIALIZATION_NVP(throwCheirality_); | ||||
|  |  | |||
|  | @ -49,7 +49,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> interp_params_; | ||||
|   std::vector<double> alphas_; | ||||
| 
 | ||||
|   /// Pose of the camera in the body frame
 | ||||
|   std::vector<Pose3> body_P_sensors_; | ||||
|  | @ -92,12 +92,12 @@ PinholePose<CALIBRATION> > { | |||
|    * single landmark in a single view (the measurement), interpolated from the 2 poses | ||||
|    * @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired) | ||||
|    * @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired) | ||||
|    * @param interp_param interpolation factor in [0,1], such that if interp_param = 0 the interpolated pose is the same as world_P_body_key1 | ||||
|    * @param alpha interpolation factor in [0,1], such that if alpha = 0 the interpolated pose is the same as world_P_body_key1 | ||||
|    * @param K (fixed) camera intrinsic calibration | ||||
|    * @param body_P_sensor (fixed) camera extrinsic calibration | ||||
|    */ | ||||
|   void add(const Point2& measured, const Key& world_P_body_key1, | ||||
|            const Key& world_P_body_key2, const double& interp_param, | ||||
|            const Key& world_P_body_key2, const double& alpha, | ||||
|            const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) { | ||||
|     // store measurements in base class
 | ||||
|     this->measured_.push_back(measured); | ||||
|  | @ -113,7 +113,7 @@ PinholePose<CALIBRATION> > { | |||
|       this->keys_.push_back(world_P_body_key2);  // add only unique keys
 | ||||
| 
 | ||||
|     // store interpolation factor
 | ||||
|     interp_params_.push_back(interp_param); | ||||
|     alphas_.push_back(alpha); | ||||
| 
 | ||||
|     // store fixed intrinsic calibration
 | ||||
|     K_all_.push_back(K); | ||||
|  | @ -128,21 +128,21 @@ PinholePose<CALIBRATION> > { | |||
|    * of a single landmark in the m views (the measurements) | ||||
|    * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding | ||||
|    * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated | ||||
|    * @param interp_params vector of interpolation params, one for each measurement (in the same order) | ||||
|    * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) | ||||
|    * @param Ks vector of (fixed) intrinsic calibration objects | ||||
|    * @param body_P_sensors vector of (fixed) extrinsic calibration objects | ||||
|    */ | ||||
|   void add(const Point2Vector& measurements, | ||||
|            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, | ||||
|            const std::vector<double>& interp_params, | ||||
|            const std::vector<double>& alphas, | ||||
|            const std::vector<boost::shared_ptr<CALIBRATION>>& Ks, | ||||
|            const std::vector<Pose3> body_P_sensors) { | ||||
|     assert(world_P_body_key_pairs.size() == measurements.size()); | ||||
|     assert(world_P_body_key_pairs.size() == interp_params.size()); | ||||
|     assert(world_P_body_key_pairs.size() == alphas.size()); | ||||
|     assert(world_P_body_key_pairs.size() == Ks.size()); | ||||
|     for (size_t i = 0; i < measurements.size(); i++) { | ||||
|       add(measurements[i], world_P_body_key_pairs[i].first, | ||||
|           world_P_body_key_pairs[i].second, interp_params[i], Ks[i], | ||||
|           world_P_body_key_pairs[i].second, alphas[i], Ks[i], | ||||
|           body_P_sensors[i]); | ||||
|     } | ||||
|   } | ||||
|  | @ -154,19 +154,19 @@ PinholePose<CALIBRATION> > { | |||
|    * of a single landmark in the m views (the measurements) | ||||
|    * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding | ||||
|    * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated | ||||
|    * @param interp_params vector of interpolation params, one for each measurement (in the same order) | ||||
|    * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) | ||||
|    * @param K (fixed) camera intrinsic calibration (same for all measurements) | ||||
|    * @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements) | ||||
|    */ | ||||
|   void add(const Point2Vector& measurements, | ||||
|            const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, | ||||
|            const std::vector<double>& interp_params, | ||||
|            const std::vector<double>& alphas, | ||||
|            const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) { | ||||
|     assert(world_P_body_key_pairs.size() == measurements.size()); | ||||
|     assert(world_P_body_key_pairs.size() == interp_params.size()); | ||||
|     assert(world_P_body_key_pairs.size() == alphas.size()); | ||||
|     for (size_t i = 0; i < measurements.size(); i++) { | ||||
|       add(measurements[i], world_P_body_key_pairs[i].first, | ||||
|           world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor); | ||||
|           world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  | @ -180,9 +180,9 @@ PinholePose<CALIBRATION> > { | |||
|     return world_P_body_key_pairs_; | ||||
|   } | ||||
| 
 | ||||
|   /// return the interpolation factors interp_params
 | ||||
|   const std::vector<double> interp_params() const { | ||||
|     return interp_params_; | ||||
|   /// return the interpolation factors alphas
 | ||||
|   const std::vector<double> alphas() const { | ||||
|     return alphas_; | ||||
|   } | ||||
| 
 | ||||
|   /// return the extrinsic camera calibration body_P_sensors
 | ||||
|  | @ -204,7 +204,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 << " interp_param: " << interp_params_[i] << std::endl; | ||||
|       std::cout << " alpha: " << alphas_[i] << std::endl; | ||||
|       body_P_sensors_[i].print("extrinsic calibration:\n"); | ||||
|       K_all_[i]->print("intrinsic calibration = "); | ||||
|     } | ||||
|  | @ -239,7 +239,7 @@ PinholePose<CALIBRATION> > { | |||
|     }else{ extrinsicCalibrationEqual = false; } | ||||
| 
 | ||||
|     return e && Base::equals(p, tol) && K_all_ == e->calibration() | ||||
|         && interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual; | ||||
|         && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  | @ -267,7 +267,7 @@ PinholePose<CALIBRATION> > { | |||
|       for (size_t i = 0; i < numViews; i++) {  // for each camera/measurement
 | ||||
|         const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); | ||||
|         const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); | ||||
|         double interpolationFactor = interp_params_[i]; | ||||
|         double interpolationFactor = alphas_[i]; | ||||
|         // get interpolated pose:
 | ||||
|         const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); | ||||
|         const Pose3& body_P_cam = body_P_sensors_[i]; | ||||
|  | @ -377,7 +377,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 == interp_params_.size()); | ||||
|     assert(numViews == alphas_.size()); | ||||
|     assert(numViews == body_P_sensors_.size()); | ||||
|     assert(numViews == world_P_body_key_pairs_.size()); | ||||
| 
 | ||||
|  | @ -385,7 +385,7 @@ PinholePose<CALIBRATION> > { | |||
|     for (size_t i = 0; i < numViews; i++) {  // for each measurement
 | ||||
|       const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); | ||||
|       const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); | ||||
|       double interpolationFactor = interp_params_[i]; | ||||
|       double interpolationFactor = alphas_[i]; | ||||
|       const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); | ||||
|       const Pose3& body_P_cam = body_P_sensors_[i]; | ||||
|       const Pose3& w_P_cam = w_P_body.compose(body_P_cam); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue