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 SmartStereoProjectionFactorPP.h
14 * @brief Smart stereo factor on poses and extrinsic calibration
15 * @author Luca Carlone
16 * @author Frank Dellaert
17 */
18
19#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
20
21#include <cassert>
22
23namespace gtsam {
24
25SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
26 const SharedNoiseModel& sharedNoiseModel,
27 const SmartStereoProjectionParams& params)
28 : Base(sharedNoiseModel, params) {} // note: no extrinsic specified!
29
30void SmartStereoProjectionFactorPP::add(
31 const StereoPoint2& measured,
32 const Key& w_P_body_key, const Key& body_P_cam_key,
33 const std::shared_ptr<Cal3_S2Stereo>& K) {
34 // we index by cameras..
35 Base::add(measured, key: w_P_body_key);
36 // but we also store the extrinsic calibration keys in the same order
37 world_P_body_keys_.push_back(x: w_P_body_key);
38 body_P_cam_keys_.push_back(x: body_P_cam_key);
39
40 // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
41 if(std::find(first: keys_.begin(), last: keys_.end(), val: body_P_cam_key) == keys_.end())
42 keys_.push_back(x: body_P_cam_key); // add only unique keys
43
44 K_all_.push_back(x: K);
45}
46
47void SmartStereoProjectionFactorPP::add(
48 const std::vector<StereoPoint2>& measurements,
49 const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
50 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks) {
51 assert(world_P_body_keys.size() == measurements.size());
52 assert(world_P_body_keys.size() == body_P_cam_keys.size());
53 assert(world_P_body_keys.size() == Ks.size());
54 for (size_t i = 0; i < measurements.size(); i++) {
55 Base::add(measured: measurements[i], key: world_P_body_keys[i]);
56 // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
57 if(std::find(first: keys_.begin(), last: keys_.end(), val: body_P_cam_keys[i]) == keys_.end())
58 keys_.push_back(x: body_P_cam_keys[i]); // add only unique keys
59
60 world_P_body_keys_.push_back(x: world_P_body_keys[i]);
61 body_P_cam_keys_.push_back(x: body_P_cam_keys[i]);
62
63 K_all_.push_back(x: Ks[i]);
64 }
65}
66
67void SmartStereoProjectionFactorPP::add(
68 const std::vector<StereoPoint2>& measurements,
69 const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
70 const std::shared_ptr<Cal3_S2Stereo>& K) {
71 assert(world_P_body_keys.size() == measurements.size());
72 assert(world_P_body_keys.size() == body_P_cam_keys.size());
73 for (size_t i = 0; i < measurements.size(); i++) {
74 Base::add(measured: measurements[i], key: world_P_body_keys[i]);
75 // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
76 if(std::find(first: keys_.begin(), last: keys_.end(), val: body_P_cam_keys[i]) == keys_.end())
77 keys_.push_back(x: body_P_cam_keys[i]); // add only unique keys
78
79 world_P_body_keys_.push_back(x: world_P_body_keys[i]);
80 body_P_cam_keys_.push_back(x: body_P_cam_keys[i]);
81
82 K_all_.push_back(x: K);
83 }
84}
85
86void SmartStereoProjectionFactorPP::print(
87 const std::string& s, const KeyFormatter& keyFormatter) const {
88 std::cout << s << "SmartStereoProjectionFactorPP: \n ";
89 for (size_t i = 0; i < K_all_.size(); i++) {
90 K_all_[i]->print(s: "calibration = ");
91 std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl;
92 }
93 Base::print(s: "", keyFormatter);
94}
95
96bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p,
97 double tol) const {
98 const SmartStereoProjectionFactorPP* e =
99 dynamic_cast<const SmartStereoProjectionFactorPP*>(&p);
100
101 return e && Base::equals(p, tol) &&
102 body_P_cam_keys_ == e->getExtrinsicPoseKeys();
103}
104
105double SmartStereoProjectionFactorPP::error(const Values& values) const {
106 if (this->active(c: values)) {
107 return this->totalReprojectionError(cameras: cameras(values));
108 } else { // else of active flag
109 return 0.0;
110 }
111}
112
113SmartStereoProjectionFactorPP::Base::Cameras
114SmartStereoProjectionFactorPP::cameras(const Values& values) const {
115 assert(world_P_body_keys_.size() == K_all_.size());
116 assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
117 Base::Cameras cameras;
118 for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
119 Pose3 w_P_body = values.at<Pose3>(j: world_P_body_keys_[i]);
120 Pose3 body_P_cam = values.at<Pose3>(j: body_P_cam_keys_[i]);
121 Pose3 w_P_cam = w_P_body.compose(g: body_P_cam);
122 cameras.push_back(x: StereoCamera(w_P_cam, K_all_[i]));
123 }
124 return cameras;
125}
126
127} // \ namespace gtsam
128