formatting
lcarlone
parent
038c1c0b8e
commit
71c528a87d
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file SmartStereoProjectionFactorPP.h
|
||||
* @brief Smart stereo factor on poses and extrinsic calibration
|
||||
* @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations
|
||||
* @author Luca Carlone
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
|
@ -33,8 +33,8 @@ namespace gtsam {
|
|||
*/
|
||||
|
||||
/**
|
||||
* This factor optimizes the extrinsic camera calibration (pose of camera wrt body),
|
||||
* and each camera has its own extrinsic calibration.
|
||||
* This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body).
|
||||
* Each camera has its own extrinsic calibration.
|
||||
* This factor requires that values contain the involved poses and extrinsics (both Pose3).
|
||||
* @addtogroup SLAM
|
||||
*/
|
||||
|
|
@ -72,9 +72,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
* @param Isotropic measurement noise
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartStereoProjectionFactorPP(
|
||||
const SharedNoiseModel& sharedNoiseModel,
|
||||
const SmartStereoProjectionParams& params = SmartStereoProjectionParams());
|
||||
SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel,
|
||||
const SmartStereoProjectionParams& params =
|
||||
SmartStereoProjectionParams());
|
||||
|
||||
/** Virtual destructor */
|
||||
~SmartStereoProjectionFactorPP() override = default;
|
||||
|
|
@ -87,8 +87,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
* @param body_P_cam_key is key corresponding to the camera observing the same landmark
|
||||
* @param K is the (fixed) camera calibration
|
||||
*/
|
||||
void add(const StereoPoint2& measured,
|
||||
const Key& w_P_body_key, const Key& body_P_cam_key,
|
||||
void add(const StereoPoint2& measured, const Key& w_P_body_key,
|
||||
const Key& body_P_cam_key,
|
||||
const boost::shared_ptr<Cal3_S2Stereo>& K);
|
||||
|
||||
/**
|
||||
|
|
@ -128,7 +128,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override;
|
||||
|
||||
/// equals
|
||||
const KeyVector& getExtrinsicPoseKeys() const {return body_P_cam_keys_;};
|
||||
const KeyVector& getExtrinsicPoseKeys() const {
|
||||
return body_P_cam_keys_;
|
||||
}
|
||||
;
|
||||
|
||||
/**
|
||||
* error calculates the error of the factor.
|
||||
|
|
@ -153,8 +156,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
/// Assumes the point has been computed
|
||||
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
|
||||
void computeJacobiansAndCorrectForMissingMeasurements(
|
||||
FBlocks& Fs,
|
||||
Matrix& E, Vector& b, const Values& values) const {
|
||||
FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const {
|
||||
if (!result_) {
|
||||
throw("computeJacobiansWithTriangulatedPoint");
|
||||
} else { // valid result: compute jacobians
|
||||
|
|
@ -166,8 +168,11 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
|
||||
Pose3 w_P_body = values.at<Pose3>(w_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, dPoseCam_dPoseExt), K_all_[i]);
|
||||
StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i));
|
||||
StereoCamera camera(
|
||||
w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt),
|
||||
K_all_[i]);
|
||||
StereoPoint2 reprojectionError = StereoPoint2(
|
||||
camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i));
|
||||
Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
|
||||
J.block<ZDim, 6>(0, 0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6)
|
||||
J.block<ZDim, 6>(0, 6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6)
|
||||
|
|
@ -175,7 +180,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
{
|
||||
J.block<1, 12>(1, 0) = Matrix::Zero(1, 12);
|
||||
Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3);
|
||||
reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0, reprojectionError.v() );
|
||||
reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0,
|
||||
reprojectionError.v());
|
||||
}
|
||||
Fs.push_back(J);
|
||||
size_t row = 3 * i;
|
||||
|
|
@ -209,8 +215,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
m = Matrix::Zero(DimPose, DimPose);
|
||||
for (Vector& v : gs)
|
||||
v = Vector::Zero(DimPose);
|
||||
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_,
|
||||
Gs, gs, 0.0);
|
||||
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||
> (keys_, Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
|
||||
|
|
@ -240,12 +246,14 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size();
|
||||
SymmetricBlockMatrix augmentedHessianUniqueKeys;
|
||||
if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView()));
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
|
||||
dims, Matrix(augmentedHessian.selfadjointView()));
|
||||
} else { // if multiple cameras share a calibration
|
||||
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()));
|
||||
augmentedHessian = SymmetricBlockMatrix(
|
||||
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
|
||||
|
||||
// these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement)
|
||||
KeyVector nonuniqueKeys;
|
||||
|
|
@ -261,58 +269,66 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
}
|
||||
|
||||
// initialize matrix to zero
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1));
|
||||
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)
|
||||
for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
|
||||
Key key_i = nonuniqueKeys.at(i);
|
||||
|
||||
// update information vector
|
||||
augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , nrUniqueKeys,
|
||||
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));
|
||||
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],
|
||||
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());
|
||||
augmentedHessianUniqueKeys.updateDiagonalBlock(
|
||||
keyToSlotMap[key_i],
|
||||
augmentedHessian.aboveDiagonalBlock(i, j)
|
||||
+ augmentedHessian.aboveDiagonalBlock(i, j).transpose());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
|
||||
augmentedHessianUniqueKeys.updateDiagonalBlock(
|
||||
nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
|
||||
}
|
||||
|
||||
return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys);
|
||||
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||
> (keys_, augmentedHessianUniqueKeys);
|
||||
}
|
||||
|
||||
/**
|
||||
* Linearize to Gaussian Factor
|
||||
* 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 {
|
||||
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");
|
||||
throw std::runtime_error(
|
||||
"SmartStereoProjectionFactorPP: unknown linearization mode");
|
||||
}
|
||||
}
|
||||
|
||||
/// linearize
|
||||
boost::shared_ptr<GaussianFactor> linearize(
|
||||
const Values& values) const override {
|
||||
boost::shared_ptr<GaussianFactor> linearize(const Values& values) const
|
||||
override {
|
||||
return linearizeDamped(values);
|
||||
}
|
||||
|
||||
|
|
@ -325,11 +341,13 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
ar & BOOST_SERIALIZATION_NVP(K_all_);
|
||||
}
|
||||
|
||||
}; // end of class declaration
|
||||
};
|
||||
// end of class declaration
|
||||
|
||||
/// traits
|
||||
template<>
|
||||
struct traits<SmartStereoProjectionFactorPP>
|
||||
: public Testable<SmartStereoProjectionFactorPP> {};
|
||||
struct traits<SmartStereoProjectionFactorPP> : public Testable<
|
||||
SmartStereoProjectionFactorPP> {
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue