1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file SmartStereoProjectionPoseFactor.cpp
14 * @brief Smart stereo factor on poses, assuming camera calibration is fixed
15 * @author Luca Carlone
16 * @author Antoni Rosinol
17 * @author Chris Beall
18 * @author Zsolt Kira
19 * @author Frank Dellaert
20 */
21
22#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
23
24#include <cassert>
25
26namespace gtsam {
27
28SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor(
29 const SharedNoiseModel& sharedNoiseModel,
30 const SmartStereoProjectionParams& params,
31 const std::optional<Pose3>& body_P_sensor)
32 : Base(sharedNoiseModel, params, body_P_sensor) {}
33
34void SmartStereoProjectionPoseFactor::add(
35 const StereoPoint2& measured, const Key& poseKey,
36 const std::shared_ptr<Cal3_S2Stereo>& K) {
37 Base::add(measured, key: poseKey);
38 K_all_.push_back(x: K);
39}
40
41void SmartStereoProjectionPoseFactor::add(
42 const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
43 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks) {
44 assert(measurements.size() == poseKeys.size());
45 assert(poseKeys.size() == Ks.size());
46 Base::add(measurements, cameraKeys: poseKeys);
47 K_all_.insert(position: K_all_.end(), first: Ks.begin(), last: Ks.end());
48}
49
50void SmartStereoProjectionPoseFactor::add(
51 const std::vector<StereoPoint2>& measurements, const KeyVector& poseKeys,
52 const std::shared_ptr<Cal3_S2Stereo>& K) {
53 assert(poseKeys.size() == measurements.size());
54 for (size_t i = 0; i < measurements.size(); i++) {
55 Base::add(measured: measurements[i], key: poseKeys[i]);
56 K_all_.push_back(x: K);
57 }
58}
59
60void SmartStereoProjectionPoseFactor::print(
61 const std::string& s, const KeyFormatter& keyFormatter) const {
62 std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
63 for (const std::shared_ptr<Cal3_S2Stereo>& K : K_all_) {
64 K->print(s: "calibration = ");
65 }
66 Base::print(s: "", keyFormatter);
67}
68
69bool SmartStereoProjectionPoseFactor::equals(const NonlinearFactor& p,
70 double tol) const {
71 const SmartStereoProjectionPoseFactor* e =
72 dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
73
74 return e && Base::equals(p, tol);
75}
76
77double SmartStereoProjectionPoseFactor::error(const Values& values) const {
78 if (this->active(c: values)) {
79 return this->totalReprojectionError(cameras: cameras(values));
80 } else { // else of active flag
81 return 0.0;
82 }
83}
84
85SmartStereoProjectionPoseFactor::Base::Cameras
86SmartStereoProjectionPoseFactor::cameras(const Values& values) const {
87 assert(keys_.size() == K_all_.size());
88 Base::Cameras cameras;
89 for (size_t i = 0; i < keys_.size(); i++) {
90 Pose3 pose = values.at<Pose3>(j: keys_[i]);
91 if (Base::body_P_sensor_) {
92 pose = pose.compose(g: *(Base::body_P_sensor_));
93 }
94 cameras.push_back(x: StereoCamera(pose, K_all_[i]));
95 }
96 return cameras;
97}
98
99} // \ namespace gtsam
100