47 std::vector<std::shared_ptr<Cal3_S2Stereo>>
K_all_;
68 static const int DimPose = 6;
71 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >
FBlocks;
91 const Key& body_P_cam_key,
92 const std::shared_ptr<Cal3_S2Stereo>&
K);
105 const std::vector<std::shared_ptr<Cal3_S2Stereo>>& Ks);
119 const std::shared_ptr<Cal3_S2Stereo>&
K);
134 return body_P_cam_keys_;
143 inline std::vector<std::shared_ptr<Cal3_S2Stereo>>
calibration()
const {
167 throw(
"computeJacobiansWithTriangulatedPoint");
169 size_t numViews = measured_.size();
170 E = Matrix::Zero(3 * numViews, 3);
171 b = Vector::Zero(3 * numViews);
172 Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei;
174 for (
size_t i = 0;
i < numViews;
i++) {
178 w_P_body.
compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i),
182 camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(
i));
184 J.block<
ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i;
185 J.block<
ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i;
189 J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
190 Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
192 reprojectionError_i.
v());
198 E.block<3, 3>(
row, 0) = Ei;
206 bool diagonalDamping =
false)
const {
210 size_t nrUniqueKeys = keys_.size();
214 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
215 std::vector<Vector> gs(nrUniqueKeys);
217 if (this->measured_.size() != cameras(
values).
size())
218 throw std::runtime_error(
"SmartStereoProjectionHessianFactor: this->"
219 "measured_.size() inconsistent with input");
226 for (
Matrix&
m : Gs)
m = Matrix::Zero(DimPose, DimPose);
227 for (
Vector&
v : gs)
v = Vector::Zero(DimPose);
228 return std::make_shared<RegularHessianFactor<DimPose>>(keys_, Gs, gs,
236 computeJacobiansAndCorrectForMissingMeasurements(Fs,
E,
b,
values);
239 noiseModel_->WhitenSystem(
E,
b);
240 for (
size_t i = 0;
i < Fs.size();
i++) {
241 Fs[
i] = noiseModel_->Whiten(Fs[
i]);
246 Cameras::ComputePointCovariance<3>(
P,
E,
lambda, diagonalDamping);
250 for (
size_t i = 0;
i < world_P_body_keys_.size();
i++) {
251 nonuniqueKeys.push_back(world_P_body_keys_.at(
i));
252 nonuniqueKeys.push_back(body_P_cam_keys_.at(
i));
256 Base::Cameras::template SchurComplementAndRearrangeBlocks<3,
DimBlock,
258 Fs,
E,
P,
b, nonuniqueKeys, keys_);
260 return std::make_shared<RegularHessianFactor<DimPose>>(
261 keys_, augmentedHessianUniqueKeys);
272 switch (params_.linearizationMode) {
276 throw std::runtime_error(
277 "SmartStereoProjectionFactorPP: unknown linearization mode");
284 return linearizeDamped(
values);
288 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
289 friend class boost::serialization::access;
291 template<
class ARCHIVE>
292 void serialize(ARCHIVE& ar,
const unsigned int ) {
293 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(
Base);
294 ar & BOOST_SERIALIZATION_NVP(K_all_);
303 SmartStereoProjectionFactorPP> {