100 lines
3.1 KiB
C++
100 lines
3.1 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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 SmartStereoProjectionPoseFactor.cpp
|
|
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
|
|
* @author Luca Carlone
|
|
* @author Antoni Rosinol
|
|
* @author Chris Beall
|
|
* @author Zsolt Kira
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
|
|
|
|
#include <cassert>
|
|
|
|
namespace gtsam {
|
|
|
|
SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
|
|
const SharedNoiseModel& sharedNoiseModel,
|
|
const SmartStereoProjectionParams& params,
|
|
const std::optional<Pose3>& body_P_sensor)
|
|
: Base(sharedNoiseModel, params, body_P_sensor) {}
|
|
|
|
void SmartStereoProjectionPoseFactor::add(
|
|
const StereoPoint2& measured, const Key& poseKey,
|
|
const std::shared_ptr<Cal3_S2Stereo>& K) {
|
|
Base::add(measured, poseKey);
|
|
K_all_.push_back(K);
|
|
}
|
|
|
|
void SmartStereoProjectionPoseFactor::add(
|
|
const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
|
|
const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks) {
|
|
assert(measurements.size() == poseKeys.size());
|
|
assert(poseKeys.size() == Ks.size());
|
|
Base::add(measurements, poseKeys);
|
|
K_all_.insert(K_all_.end(), Ks.begin(), Ks.end());
|
|
}
|
|
|
|
void SmartStereoProjectionPoseFactor::add(
|
|
const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
|
|
const std::shared_ptr<Cal3_S2Stereo>& K) {
|
|
assert(poseKeys.size() == measurements.size());
|
|
for (size_t i = 0; i < measurements.size(); i++) {
|
|
Base::add(measurements[i], poseKeys[i]);
|
|
K_all_.push_back(K);
|
|
}
|
|
}
|
|
|
|
void SmartStereoProjectionPoseFactor::print(
|
|
const std::string& s, const KeyFormatter& keyFormatter) const {
|
|
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
|
|
for (const std::shared_ptr<Cal3_S2Stereo>& K : K_all_) {
|
|
K->print("calibration = ");
|
|
}
|
|
Base::print("", keyFormatter);
|
|
}
|
|
|
|
bool SmartStereoProjectionPoseFactor::equals(const NonlinearFactor& p,
|
|
double tol) const {
|
|
const SmartStereoProjectionPoseFactor* e =
|
|
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
|
|
|
|
return e && Base::equals(p, tol);
|
|
}
|
|
|
|
double SmartStereoProjectionPoseFactor::error(const Values& values) const {
|
|
if (this->active(values)) {
|
|
return this->totalReprojectionError(cameras(values));
|
|
} else { // else of active flag
|
|
return 0.0;
|
|
}
|
|
}
|
|
|
|
SmartStereoProjectionPoseFactor::Base::Cameras
|
|
SmartStereoProjectionPoseFactor::cameras(const Values& values) const {
|
|
assert(keys_.size() == K_all_.size());
|
|
Base::Cameras cameras;
|
|
for (size_t i = 0; i < keys_.size(); i++) {
|
|
Pose3 pose = values.at<Pose3>(keys_[i]);
|
|
if (Base::body_P_sensor_) {
|
|
pose = pose.compose(*(Base::body_P_sensor_));
|
|
}
|
|
cameras.push_back(StereoCamera(pose, K_all_[i]));
|
|
}
|
|
return cameras;
|
|
}
|
|
|
|
} // \ namespace gtsam
|