Merge pull request #696 from borglab/feature/smartFactorsWithExtrinsicCalibration
smart factors with extrinsics calibrationrelease/4.3a0
Frank Dellaert
GitHub
commit
1011055007
|
|
@ -139,6 +139,57 @@ public:
|
||||||
return ErrorVector(project2(point, Fs, E), measured);
|
return ErrorVector(project2(point, Fs, E), measured);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
|
||||||
|
* G = F' * F - F' * E * P * E' * F
|
||||||
|
* g = F' * (b - E * P * E' * b)
|
||||||
|
* Fixed size version
|
||||||
|
*/
|
||||||
|
template<int N, int ND> // N = 2 or 3, ND is the camera dimension
|
||||||
|
static SymmetricBlockMatrix SchurComplement(
|
||||||
|
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs,
|
||||||
|
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
|
||||||
|
|
||||||
|
// a single point is observed in m cameras
|
||||||
|
size_t m = Fs.size();
|
||||||
|
|
||||||
|
// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
|
||||||
|
size_t M1 = ND * m + 1;
|
||||||
|
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
|
||||||
|
std::fill(dims.begin(), dims.end() - 1, ND);
|
||||||
|
dims.back() = 1;
|
||||||
|
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
|
||||||
|
|
||||||
|
// Blockwise Schur complement
|
||||||
|
for (size_t i = 0; i < m; i++) { // for each camera
|
||||||
|
|
||||||
|
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
|
||||||
|
const auto FiT = Fi.transpose();
|
||||||
|
const Eigen::Matrix<double, ZDim, N> Ei_P = //
|
||||||
|
E.block(ZDim * i, 0, ZDim, N) * P;
|
||||||
|
|
||||||
|
// D = (Dx2) * ZDim
|
||||||
|
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
|
||||||
|
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
|
||||||
|
|
||||||
|
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
|
||||||
|
augmentedHessian.setDiagonalBlock(i, FiT
|
||||||
|
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
|
||||||
|
|
||||||
|
// upper triangular part of the hessian
|
||||||
|
for (size_t j = i + 1; j < m; j++) { // for each camera
|
||||||
|
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
|
||||||
|
|
||||||
|
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
|
||||||
|
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
|
||||||
|
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
|
||||||
|
}
|
||||||
|
} // end of for over cameras
|
||||||
|
|
||||||
|
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
|
||||||
|
return augmentedHessian;
|
||||||
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
|
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
|
||||||
* G = F' * F - F' * E * P * E' * F
|
* G = F' * F - F' * E * P * E' * F
|
||||||
|
|
@ -148,45 +199,7 @@ public:
|
||||||
template<int N> // N = 2 or 3
|
template<int N> // N = 2 or 3
|
||||||
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
|
static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs,
|
||||||
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
|
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
|
||||||
|
return SchurComplement<N,D>(Fs, E, P, b);
|
||||||
// a single point is observed in m cameras
|
|
||||||
size_t m = Fs.size();
|
|
||||||
|
|
||||||
// Create a SymmetricBlockMatrix
|
|
||||||
size_t M1 = D * m + 1;
|
|
||||||
std::vector<DenseIndex> dims(m + 1); // this also includes the b term
|
|
||||||
std::fill(dims.begin(), dims.end() - 1, D);
|
|
||||||
dims.back() = 1;
|
|
||||||
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
|
|
||||||
|
|
||||||
// Blockwise Schur complement
|
|
||||||
for (size_t i = 0; i < m; i++) { // for each camera
|
|
||||||
|
|
||||||
const MatrixZD& Fi = Fs[i];
|
|
||||||
const auto FiT = Fi.transpose();
|
|
||||||
const Eigen::Matrix<double, ZDim, N> Ei_P = //
|
|
||||||
E.block(ZDim * i, 0, ZDim, N) * P;
|
|
||||||
|
|
||||||
// D = (Dx2) * ZDim
|
|
||||||
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
|
|
||||||
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
|
|
||||||
|
|
||||||
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
|
|
||||||
augmentedHessian.setDiagonalBlock(i, FiT
|
|
||||||
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
|
|
||||||
|
|
||||||
// upper triangular part of the hessian
|
|
||||||
for (size_t j = i + 1; j < m; j++) { // for each camera
|
|
||||||
const MatrixZD& Fj = Fs[j];
|
|
||||||
|
|
||||||
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
|
|
||||||
augmentedHessian.setOffDiagonalBlock(i, j, -FiT
|
|
||||||
* (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
|
|
||||||
}
|
|
||||||
} // end of for over cameras
|
|
||||||
|
|
||||||
augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
|
|
||||||
return augmentedHessian;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Computes Point Covariance P, with lambda parameter
|
/// Computes Point Covariance P, with lambda parameter
|
||||||
|
|
|
||||||
|
|
@ -450,8 +450,8 @@ public:
|
||||||
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
|
* This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan)
|
||||||
*/
|
*/
|
||||||
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
|
void correctForMissingMeasurements(const Cameras& cameras, Vector& ue,
|
||||||
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
|
||||||
boost::optional<Matrix&> E = boost::none) const override
|
boost::optional<Matrix&> E = boost::none) const override
|
||||||
{
|
{
|
||||||
// when using stereo cameras, some of the measurements might be missing:
|
// when using stereo cameras, some of the measurements might be missing:
|
||||||
for(size_t i=0; i < cameras.size(); i++){
|
for(size_t i=0; i < cameras.size(); i++){
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,125 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 SmartStereoProjectionFactorPP.h
|
||||||
|
* @brief Smart stereo factor on poses and extrinsic calibration
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam_unstable/slam/SmartStereoProjectionFactorPP.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP(
|
||||||
|
const SharedNoiseModel& sharedNoiseModel,
|
||||||
|
const SmartStereoProjectionParams& params)
|
||||||
|
: Base(sharedNoiseModel, params) {} // note: no extrinsic specified!
|
||||||
|
|
||||||
|
void SmartStereoProjectionFactorPP::add(
|
||||||
|
const StereoPoint2& measured,
|
||||||
|
const Key& w_P_body_key, const Key& body_P_cam_key,
|
||||||
|
const boost::shared_ptr<Cal3_S2Stereo>& K) {
|
||||||
|
// we index by cameras..
|
||||||
|
Base::add(measured, w_P_body_key);
|
||||||
|
// but we also store the extrinsic calibration keys in the same order
|
||||||
|
world_P_body_keys_.push_back(w_P_body_key);
|
||||||
|
body_P_cam_keys_.push_back(body_P_cam_key);
|
||||||
|
|
||||||
|
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
|
||||||
|
if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end())
|
||||||
|
keys_.push_back(body_P_cam_key); // add only unique keys
|
||||||
|
|
||||||
|
K_all_.push_back(K);
|
||||||
|
}
|
||||||
|
|
||||||
|
void SmartStereoProjectionFactorPP::add(
|
||||||
|
const std::vector<StereoPoint2>& measurements,
|
||||||
|
const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
|
||||||
|
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks) {
|
||||||
|
assert(world_P_body_keys.size() == measurements.size());
|
||||||
|
assert(world_P_body_keys.size() == body_P_cam_keys.size());
|
||||||
|
assert(world_P_body_keys.size() == Ks.size());
|
||||||
|
for (size_t i = 0; i < measurements.size(); i++) {
|
||||||
|
Base::add(measurements[i], world_P_body_keys[i]);
|
||||||
|
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
|
||||||
|
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
|
||||||
|
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
|
||||||
|
|
||||||
|
world_P_body_keys_.push_back(world_P_body_keys[i]);
|
||||||
|
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
|
||||||
|
|
||||||
|
K_all_.push_back(Ks[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SmartStereoProjectionFactorPP::add(
|
||||||
|
const std::vector<StereoPoint2>& measurements,
|
||||||
|
const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys,
|
||||||
|
const boost::shared_ptr<Cal3_S2Stereo>& K) {
|
||||||
|
assert(world_P_body_keys.size() == measurements.size());
|
||||||
|
assert(world_P_body_keys.size() == body_P_cam_keys.size());
|
||||||
|
for (size_t i = 0; i < measurements.size(); i++) {
|
||||||
|
Base::add(measurements[i], world_P_body_keys[i]);
|
||||||
|
// pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared
|
||||||
|
if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end())
|
||||||
|
keys_.push_back(body_P_cam_keys[i]); // add only unique keys
|
||||||
|
|
||||||
|
world_P_body_keys_.push_back(world_P_body_keys[i]);
|
||||||
|
body_P_cam_keys_.push_back(body_P_cam_keys[i]);
|
||||||
|
|
||||||
|
K_all_.push_back(K);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SmartStereoProjectionFactorPP::print(
|
||||||
|
const std::string& s, const KeyFormatter& keyFormatter) const {
|
||||||
|
std::cout << s << "SmartStereoProjectionFactorPP: \n ";
|
||||||
|
for (size_t i = 0; i < K_all_.size(); i++) {
|
||||||
|
K_all_[i]->print("calibration = ");
|
||||||
|
std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl;
|
||||||
|
}
|
||||||
|
Base::print("", keyFormatter);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p,
|
||||||
|
double tol) const {
|
||||||
|
const SmartStereoProjectionFactorPP* e =
|
||||||
|
dynamic_cast<const SmartStereoProjectionFactorPP*>(&p);
|
||||||
|
|
||||||
|
return e && Base::equals(p, tol) &&
|
||||||
|
body_P_cam_keys_ == e->getExtrinsicPoseKeys();
|
||||||
|
}
|
||||||
|
|
||||||
|
double SmartStereoProjectionFactorPP::error(const Values& values) const {
|
||||||
|
if (this->active(values)) {
|
||||||
|
return this->totalReprojectionError(cameras(values));
|
||||||
|
} else { // else of active flag
|
||||||
|
return 0.0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
SmartStereoProjectionFactorPP::Base::Cameras
|
||||||
|
SmartStereoProjectionFactorPP::cameras(const Values& values) const {
|
||||||
|
assert(world_P_body_keys_.size() == K_all_.size());
|
||||||
|
assert(world_P_body_keys_.size() == body_P_cam_keys_.size());
|
||||||
|
Base::Cameras cameras;
|
||||||
|
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
|
||||||
|
Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_[i]);
|
||||||
|
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_[i]);
|
||||||
|
Pose3 w_P_cam = w_P_body.compose(body_P_cam);
|
||||||
|
cameras.push_back(StereoCamera(w_P_cam, K_all_[i]));
|
||||||
|
}
|
||||||
|
return cameras;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // \ namespace gtsam
|
||||||
|
|
@ -0,0 +1,369 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 SmartStereoProjectionFactorPP.h
|
||||||
|
* @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Frank Dellaert
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam_unstable/slam/SmartStereoProjectionFactor.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
/**
|
||||||
|
*
|
||||||
|
* @addtogroup SLAM
|
||||||
|
*
|
||||||
|
* If you are using the factor, please cite:
|
||||||
|
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert,
|
||||||
|
* Eliminating conditionally independent sets in factor graphs:
|
||||||
|
* a unifying perspective based on smart factors,
|
||||||
|
* Int. Conf. on Robotics and Automation (ICRA), 2014.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body).
|
||||||
|
* Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras.
|
||||||
|
* This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables).
|
||||||
|
* @addtogroup SLAM
|
||||||
|
*/
|
||||||
|
class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
||||||
|
protected:
|
||||||
|
/// shared pointer to calibration object (one for each camera)
|
||||||
|
std::vector<boost::shared_ptr<Cal3_S2Stereo>> K_all_;
|
||||||
|
|
||||||
|
/// The keys corresponding to the pose of the body (with respect to an external world frame) for each view
|
||||||
|
KeyVector world_P_body_keys_;
|
||||||
|
|
||||||
|
/// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body)
|
||||||
|
KeyVector body_P_cam_keys_;
|
||||||
|
|
||||||
|
public:
|
||||||
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||||
|
|
||||||
|
/// shorthand for base class type
|
||||||
|
typedef SmartStereoProjectionFactor Base;
|
||||||
|
|
||||||
|
/// shorthand for this class
|
||||||
|
typedef SmartStereoProjectionFactorPP This;
|
||||||
|
|
||||||
|
/// shorthand for a smart pointer to a factor
|
||||||
|
typedef boost::shared_ptr<This> shared_ptr;
|
||||||
|
|
||||||
|
static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose
|
||||||
|
static const int DimPose = 6; ///< Pose3 dimension
|
||||||
|
static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement)
|
||||||
|
typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrt camera)
|
||||||
|
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Constructor
|
||||||
|
* @param Isotropic measurement noise
|
||||||
|
* @param params internal parameters of the smart factors
|
||||||
|
*/
|
||||||
|
SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel,
|
||||||
|
const SmartStereoProjectionParams& params =
|
||||||
|
SmartStereoProjectionParams());
|
||||||
|
|
||||||
|
/** Virtual destructor */
|
||||||
|
~SmartStereoProjectionFactorPP() override = default;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* add a new measurement, with a pose key, and an extrinsic pose key
|
||||||
|
* @param measured is the 3-dimensional location of the projection of a
|
||||||
|
* single landmark in the a single (stereo) view (the measurement)
|
||||||
|
* @param world_P_body_key is the key corresponding to the body poses observing the same landmark
|
||||||
|
* @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration
|
||||||
|
* @param K is the (fixed) camera intrinsic calibration
|
||||||
|
*/
|
||||||
|
void add(const StereoPoint2& measured, const Key& world_P_body_key,
|
||||||
|
const Key& body_P_cam_key,
|
||||||
|
const boost::shared_ptr<Cal3_S2Stereo>& K);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Variant of the previous one in which we include a set of measurements
|
||||||
|
* @param measurements vector of the 3m dimensional location of the projection
|
||||||
|
* of a single landmark in the m (stereo) view (the measurements)
|
||||||
|
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
|
||||||
|
* @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
|
||||||
|
* (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
|
||||||
|
* @param Ks vector of intrinsic calibration objects
|
||||||
|
*/
|
||||||
|
void add(const std::vector<StereoPoint2>& measurements,
|
||||||
|
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
|
||||||
|
const std::vector<boost::shared_ptr<Cal3_S2Stereo>>& Ks);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Variant of the previous one in which we include a set of measurements with
|
||||||
|
* the same noise and calibration
|
||||||
|
* @param measurements vector of the 3m dimensional location of the projection
|
||||||
|
* of a single landmark in the m (stereo) view (the measurements)
|
||||||
|
* @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark
|
||||||
|
* @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration
|
||||||
|
* (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration)
|
||||||
|
* @param K the (known) camera calibration (same for all measurements)
|
||||||
|
*/
|
||||||
|
void add(const std::vector<StereoPoint2>& measurements,
|
||||||
|
const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys,
|
||||||
|
const boost::shared_ptr<Cal3_S2Stereo>& K);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* print
|
||||||
|
* @param s optional string naming the factor
|
||||||
|
* @param keyFormatter optional formatter useful for printing Symbols
|
||||||
|
*/
|
||||||
|
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
|
||||||
|
DefaultKeyFormatter) const override;
|
||||||
|
|
||||||
|
/// equals
|
||||||
|
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
|
||||||
|
|
||||||
|
/// equals
|
||||||
|
const KeyVector& getExtrinsicPoseKeys() const {
|
||||||
|
return body_P_cam_keys_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* error calculates the error of the factor.
|
||||||
|
*/
|
||||||
|
double error(const Values& values) const override;
|
||||||
|
|
||||||
|
/** return the calibration object */
|
||||||
|
inline std::vector<boost::shared_ptr<Cal3_S2Stereo>> calibration() const {
|
||||||
|
return K_all_;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Collect all cameras involved in this factor
|
||||||
|
* @param values Values structure which must contain camera poses
|
||||||
|
* corresponding
|
||||||
|
* to keys involved in this factor
|
||||||
|
* @return vector of Values
|
||||||
|
*/
|
||||||
|
Base::Cameras cameras(const Values& values) const override;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Compute jacobian F, E and error vector at a given linearization point
|
||||||
|
* @param values Values structure which must contain camera poses
|
||||||
|
* corresponding to keys involved in this factor
|
||||||
|
* @return Return arguments are the camera jacobians Fs (including the jacobian with
|
||||||
|
* respect to both the body pose and extrinsic pose), the point Jacobian E,
|
||||||
|
* and the error vector b. Note that the jacobians are computed for a given point.
|
||||||
|
*/
|
||||||
|
void computeJacobiansAndCorrectForMissingMeasurements(
|
||||||
|
FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
|
||||||
|
if (!result_) {
|
||||||
|
throw("computeJacobiansWithTriangulatedPoint");
|
||||||
|
} else { // valid result: compute jacobians
|
||||||
|
size_t numViews = measured_.size();
|
||||||
|
E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian)
|
||||||
|
b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view
|
||||||
|
Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei;
|
||||||
|
|
||||||
|
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
|
||||||
|
Pose3 w_P_body = values.at<Pose3>(world_P_body_keys_.at(i));
|
||||||
|
Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i));
|
||||||
|
StereoCamera camera(
|
||||||
|
w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i),
|
||||||
|
K_all_[i]);
|
||||||
|
// get jacobians and error vector for current measurement
|
||||||
|
StereoPoint2 reprojectionError_i = StereoPoint2(
|
||||||
|
camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i));
|
||||||
|
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
|
||||||
|
J.block<ZDim, 6>(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6)
|
||||||
|
J.block<ZDim, 6>(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6)
|
||||||
|
// if the right pixel is invalid, fix jacobians
|
||||||
|
if (std::isnan(measured_.at(i).uR()))
|
||||||
|
{
|
||||||
|
J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
|
||||||
|
Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
|
||||||
|
reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0,
|
||||||
|
reprojectionError_i.v());
|
||||||
|
}
|
||||||
|
// fit into the output structures
|
||||||
|
Fs.push_back(J);
|
||||||
|
size_t row = 3 * i;
|
||||||
|
b.segment<ZDim>(row) = -reprojectionError_i.vector();
|
||||||
|
E.block<3, 3>(row, 0) = Ei;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// linearize and return a Hessianfactor that is an approximation of error(p)
|
||||||
|
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
|
||||||
|
const Values& values, const double lambda = 0.0, bool diagonalDamping =
|
||||||
|
false) const {
|
||||||
|
|
||||||
|
// we may have multiple cameras sharing the same extrinsic cals, hence the number
|
||||||
|
// of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
|
||||||
|
// have a body key and an extrinsic calibration key for each measurement)
|
||||||
|
size_t nrUniqueKeys = keys_.size();
|
||||||
|
size_t nrNonuniqueKeys = world_P_body_keys_.size()
|
||||||
|
+ body_P_cam_keys_.size();
|
||||||
|
|
||||||
|
// Create structures for Hessian Factors
|
||||||
|
KeyVector js;
|
||||||
|
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
|
||||||
|
std::vector<Vector> gs(nrUniqueKeys);
|
||||||
|
|
||||||
|
if (this->measured_.size() != cameras(values).size())
|
||||||
|
throw std::runtime_error("SmartStereoProjectionHessianFactor: this->"
|
||||||
|
"measured_.size() inconsistent with input");
|
||||||
|
|
||||||
|
// triangulate 3D point at given linearization point
|
||||||
|
triangulateSafe(cameras(values));
|
||||||
|
|
||||||
|
if (!result_) { // failed: return "empty/zero" Hessian
|
||||||
|
for (Matrix& m : Gs)
|
||||||
|
m = Matrix::Zero(DimPose, DimPose);
|
||||||
|
for (Vector& v : gs)
|
||||||
|
v = Vector::Zero(DimPose);
|
||||||
|
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||||
|
> (keys_, Gs, gs, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute Jacobian given triangulated 3D Point
|
||||||
|
FBlocks Fs;
|
||||||
|
Matrix F, E;
|
||||||
|
Vector b;
|
||||||
|
computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values);
|
||||||
|
|
||||||
|
// Whiten using noise model
|
||||||
|
noiseModel_->WhitenSystem(E, b);
|
||||||
|
for (size_t i = 0; i < Fs.size(); i++)
|
||||||
|
Fs[i] = noiseModel_->Whiten(Fs[i]);
|
||||||
|
|
||||||
|
// build augmented Hessian (with last row/column being the information vector)
|
||||||
|
Matrix3 P;
|
||||||
|
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
|
||||||
|
|
||||||
|
// marginalize point: note - we reuse the standard SchurComplement function
|
||||||
|
SymmetricBlockMatrix augmentedHessian =
|
||||||
|
Cameras::SchurComplement<3, Dim>(Fs, E, P, b);
|
||||||
|
|
||||||
|
// now pack into an Hessian factor
|
||||||
|
std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
|
||||||
|
std::fill(dims.begin(), dims.end() - 1, 6);
|
||||||
|
dims.back() = 1;
|
||||||
|
SymmetricBlockMatrix augmentedHessianUniqueKeys;
|
||||||
|
|
||||||
|
// here we have to deal with the fact that some cameras may share the same extrinsic key
|
||||||
|
if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
|
||||||
|
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
|
||||||
|
dims, Matrix(augmentedHessian.selfadjointView()));
|
||||||
|
} else { // if multiple cameras share a calibration we have to rearrange
|
||||||
|
// the results of the Schur complement matrix
|
||||||
|
std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
|
||||||
|
std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6);
|
||||||
|
nonuniqueDims.back() = 1;
|
||||||
|
augmentedHessian = SymmetricBlockMatrix(
|
||||||
|
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
|
||||||
|
|
||||||
|
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
|
||||||
|
KeyVector nonuniqueKeys;
|
||||||
|
for (size_t i = 0; i < world_P_body_keys_.size(); i++) {
|
||||||
|
nonuniqueKeys.push_back(world_P_body_keys_.at(i));
|
||||||
|
nonuniqueKeys.push_back(body_P_cam_keys_.at(i));
|
||||||
|
}
|
||||||
|
|
||||||
|
// get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
|
||||||
|
std::map<Key, size_t> keyToSlotMap;
|
||||||
|
for (size_t k = 0; k < nrUniqueKeys; k++) {
|
||||||
|
keyToSlotMap[keys_[k]] = k;
|
||||||
|
}
|
||||||
|
|
||||||
|
// initialize matrix to zero
|
||||||
|
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
|
||||||
|
dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1));
|
||||||
|
|
||||||
|
// add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
|
||||||
|
// and populates an Hessian that only includes the unique keys (that is what we want to return)
|
||||||
|
for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
|
||||||
|
Key key_i = nonuniqueKeys.at(i);
|
||||||
|
|
||||||
|
// update information vector
|
||||||
|
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
|
||||||
|
keyToSlotMap[key_i], nrUniqueKeys,
|
||||||
|
augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
|
||||||
|
|
||||||
|
// update blocks
|
||||||
|
for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
|
||||||
|
Key key_j = nonuniqueKeys.at(j);
|
||||||
|
if (i == j) {
|
||||||
|
augmentedHessianUniqueKeys.updateDiagonalBlock(
|
||||||
|
keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
|
||||||
|
} else { // (i < j)
|
||||||
|
if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
|
||||||
|
augmentedHessianUniqueKeys.updateOffDiagonalBlock(
|
||||||
|
keyToSlotMap[key_i], keyToSlotMap[key_j],
|
||||||
|
augmentedHessian.aboveDiagonalBlock(i, j));
|
||||||
|
} else {
|
||||||
|
augmentedHessianUniqueKeys.updateDiagonalBlock(
|
||||||
|
keyToSlotMap[key_i],
|
||||||
|
augmentedHessian.aboveDiagonalBlock(i, j)
|
||||||
|
+ augmentedHessian.aboveDiagonalBlock(i, j).transpose());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// update bottom right element of the matrix
|
||||||
|
augmentedHessianUniqueKeys.updateDiagonalBlock(
|
||||||
|
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
|
||||||
|
}
|
||||||
|
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||||
|
> (keys_, augmentedHessianUniqueKeys);
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
|
||||||
|
* @param values Values structure which must contain camera poses and extrinsic pose for this factor
|
||||||
|
* @return a Gaussian factor
|
||||||
|
*/
|
||||||
|
boost::shared_ptr<GaussianFactor> linearizeDamped(
|
||||||
|
const Values& values, const double lambda = 0.0) const {
|
||||||
|
// depending on flag set on construction we may linearize to different linear factors
|
||||||
|
switch (params_.linearizationMode) {
|
||||||
|
case HESSIAN:
|
||||||
|
return createHessianFactor(values, lambda);
|
||||||
|
default:
|
||||||
|
throw std::runtime_error(
|
||||||
|
"SmartStereoProjectionFactorPP: unknown linearization mode");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// linearize
|
||||||
|
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
|
||||||
|
override {
|
||||||
|
return linearizeDamped(values);
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
/// Serialization function
|
||||||
|
friend class boost::serialization::access;
|
||||||
|
template<class ARCHIVE>
|
||||||
|
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||||
|
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
|
||||||
|
ar & BOOST_SERIALIZATION_NVP(K_all_);
|
||||||
|
}
|
||||||
|
|
||||||
|
};
|
||||||
|
// end of class declaration
|
||||||
|
|
||||||
|
/// traits
|
||||||
|
template<>
|
||||||
|
struct traits<SmartStereoProjectionFactorPP> : public Testable<
|
||||||
|
SmartStereoProjectionFactorPP> {
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue