12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

renamed params. need one last test

release/4.3a0
lcarlone 2021-07-23 19:03:23 -04:00
parent 5350e3463e
commit 1f07142b5b
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -52,7 +52,7 @@ PinholePose<CALIBRATION> > {
std::vector<std::pair<Key, Key>> world_P_body_key_pairs_; std::vector<std::pair<Key, Key>> world_P_body_key_pairs_;
/// interpolation factor (one for each observation) to interpolate between pair of consecutive poses /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses
std::vector<double> interp_param_; std::vector<double> interp_params_;
/// Pose of the camera in the body frame /// Pose of the camera in the body frame
std::vector<Pose3> body_P_sensors_; ///< Pose of the camera in the body frame std::vector<Pose3> body_P_sensors_; ///< Pose of the camera in the body frame
@ -95,11 +95,11 @@ PinholePose<CALIBRATION> > {
* single landmark in the a single view (the measurement), interpolated from the 2 poses * single landmark in the a single view (the measurement), interpolated from the 2 poses
* @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired) * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired)
* @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired) * @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired)
* @param gamma in [0,1] is the interpolation factor, such that if gamma = 0 the interpolated pose is the same as world_P_body_key * @param interp_param in [0,1] is the interpolation factor, such that if interp_param = 0 the interpolated pose is the same as world_P_body_key
* @param K is the (fixed) camera intrinsic calibration * @param K is the (fixed) camera intrinsic calibration
*/ */
void add(const Point2& measured, const Key& world_P_body_key1, void add(const Point2& measured, const Key& world_P_body_key1,
const Key& world_P_body_key2, const double& gamma, const Key& world_P_body_key2, const double& interp_param,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) { const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
// store measurements in base class (note: we manyally add keys below to make sure they are unique // store measurements in base class (note: we manyally add keys below to make sure they are unique
this->measured_.push_back(measured); this->measured_.push_back(measured);
@ -117,7 +117,7 @@ PinholePose<CALIBRATION> > {
this->keys_.push_back(world_P_body_key2); // add only unique keys this->keys_.push_back(world_P_body_key2); // add only unique keys
// store interpolation factors // store interpolation factors
interp_param_.push_back(gamma); interp_params_.push_back(interp_param);
// store fixed calibration // store fixed calibration
K_all_.push_back(K); K_all_.push_back(K);
@ -135,15 +135,15 @@ PinholePose<CALIBRATION> > {
*/ */
void add(const Point2Vector& measurements, void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& gammas, const std::vector<double>& interp_params,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks, const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3> body_P_sensors) { const std::vector<Pose3> body_P_sensors) {
assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == measurements.size());
assert(world_P_body_key_pairs.size() == gammas.size()); assert(world_P_body_key_pairs.size() == interp_params.size());
assert(world_P_body_key_pairs.size() == Ks.size()); assert(world_P_body_key_pairs.size() == Ks.size());
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first, add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, gammas[i], Ks[i], world_P_body_key_pairs[i].second, interp_params[i], Ks[i],
body_P_sensors[i]); body_P_sensors[i]);
} }
} }
@ -158,13 +158,13 @@ PinholePose<CALIBRATION> > {
*/ */
void add(const Point2Vector& measurements, void add(const Point2Vector& measurements,
const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs, const std::vector<std::pair<Key, Key>>& world_P_body_key_pairs,
const std::vector<double>& gammas, const std::vector<double>& interp_params,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) { 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() == measurements.size());
assert(world_P_body_key_pairs.size() == gammas.size()); assert(world_P_body_key_pairs.size() == interp_params.size());
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], world_P_body_key_pairs[i].first, add(measurements[i], world_P_body_key_pairs[i].first,
world_P_body_key_pairs[i].second, gammas[i], K, body_P_sensor); world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor);
} }
} }
@ -178,9 +178,9 @@ PinholePose<CALIBRATION> > {
return world_P_body_key_pairs_; return world_P_body_key_pairs_;
} }
/// return the interpolation factors gammas /// return the interpolation factors interp_params
const std::vector<double> getGammas() const { const std::vector<double> interp_params() const {
return interp_param_; return interp_params_;
} }
/// return the extrinsic camera calibration body_P_sensors /// return the extrinsic camera calibration body_P_sensors
@ -202,7 +202,7 @@ PinholePose<CALIBRATION> > {
<< keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl;
std::cout << " pose2 key: " std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " gamma: " << interp_param_[i] << std::endl; std::cout << " interp_param: " << interp_params_[i] << std::endl;
body_P_sensors_[i].print("extrinsic calibration:\n"); body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = "); K_all_[i]->print("intrinsic calibration = ");
} }
@ -237,7 +237,7 @@ PinholePose<CALIBRATION> > {
}else{ extrinsicCalibrationEqual = false; } }else{ extrinsicCalibrationEqual = false; }
return e && Base::equals(p, tol) && K_all_ == e->calibration() return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; && interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual;
} }
/** /**
@ -264,7 +264,7 @@ PinholePose<CALIBRATION> > {
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement 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_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); Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = interp_param_[i]; double interpolationFactor = interp_params_[i];
// get interpolated pose: // get interpolated pose:
Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); 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]; Pose3 body_P_cam = body_P_sensors_[i];
@ -368,7 +368,7 @@ PinholePose<CALIBRATION> > {
typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras(const Values& values) const override {
size_t numViews = this->measured_.size(); size_t numViews = this->measured_.size();
assert(numViews == K_all_.size()); assert(numViews == K_all_.size());
assert(numViews == interp_param_.size()); assert(numViews == interp_params_.size());
assert(numViews == body_P_sensors_.size()); assert(numViews == body_P_sensors_.size());
assert(numViews == world_P_body_key_pairs_.size()); assert(numViews == world_P_body_key_pairs_.size());
@ -376,7 +376,7 @@ PinholePose<CALIBRATION> > {
for (size_t i = 0; i < numViews; i++) { // for each measurement 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_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); Pose3 w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = interp_param_[i]; double interpolationFactor = interp_params_[i];
Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); Pose3 w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
Pose3 body_P_cam = body_P_sensors_[i]; Pose3 body_P_cam = body_P_sensors_[i];
Pose3 w_P_cam = w_P_body.compose(body_P_cam); Pose3 w_P_cam = w_P_body.compose(body_P_cam);