12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Some formatting

release/4.3a0
Frank Dellaert 2021-08-28 17:19:19 -04:00
parent f712d62150
commit 0c622294d2
2 changed files with 93 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
gtsam/geometry/CameraSet.h
View File

@ -147,68 +147,74 @@ public:
* G = F' * F - F' * E * P * E' * F * G = F' * F - F' * E * P * E' * F
* g = F' * (b - E * P * E' * b) * g = F' * (b - E * P * E' * b)
* Fixed size version * Fixed size version
*/ */
template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension template <int N,
static SymmetricBlockMatrix SchurComplement( int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs, static SymmetricBlockMatrix SchurComplement(
const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) { 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();
// a single point is observed in m cameras // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
size_t m = Fs.size(); 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));
// Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) // Blockwise Schur complement
size_t M1 = ND * m + 1; for (size_t i = 0; i < m; i++) { // for each camera
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 const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
for (size_t i = 0; i < m; i++) { // for each camera const auto FiT = Fi.transpose();
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i]; // D = (Dx2) * ZDim
const auto FiT = Fi.transpose(); augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
const Eigen::Matrix<double, ZDim, N> Ei_P = // - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b augmentedHessian.setDiagonalBlock(i, FiT
- FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
// (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) // upper triangular part of the hessian
augmentedHessian.setDiagonalBlock(i, FiT for (size_t j = i + 1; j < m; j++) { // for each camera
* (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
// upper triangular part of the hessian // (DxD) = (Dx2) * ( (2x2) * (2xD) )
for (size_t j = i + 1; j < m; j++) { // for each camera augmentedHessian.setOffDiagonalBlock(i, j, -FiT
const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j]; * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
}
} // end of for over cameras
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
augmentedHessian.setOffDiagonalBlock(i, j, -FiT return augmentedHessian;
* (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
* g = F' * (b - E * P * E' * b) * g = F' * (b - E * P * E' * b)
* In this version, we allow for the case where the keys in the Jacobian are organized * In this version, we allow for the case where the keys in the Jacobian are
* differently from the keys in the output SymmetricBlockMatrix * organized differently from the keys in the output SymmetricBlockMatrix In
* In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) * particular: each diagonal block of the Jacobian F captures 2 poses (useful
* such that F keeps the block structure that makes the Schur complement trick fast. * for rolling shutter and extrinsic calibration) such that F keeps the block
* structure that makes the Schur complement trick fast.
*
* N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is
* the Hessian block dimension
*/ */
template<int N, int ND, int NDD> // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension template <int N, int ND, int NDD>
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
const std::vector<Eigen::Matrix<double, ZDim, ND>, const std::vector<
Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND> > >& Fs, 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, const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
const KeyVector jacobianKeys, const KeyVector hessianKeys) { const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrNonuniqueKeys = jacobianKeys.size();
size_t nrUniqueKeys = hessianKeys.size(); size_t nrUniqueKeys = hessianKeys.size();

71
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/geometry/CameraSet.h>
namespace gtsam { namespace gtsam {
/** /**
@ -229,7 +230,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
keyPairsEqual = false; break; keyPairsEqual = false; break;
} }
} }
}else{ keyPairsEqual = false; } } else {
keyPairsEqual = false;
}
double extrinsicCalibrationEqual = true; double extrinsicCalibrationEqual = true;
if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ if(this->body_P_sensors_.size() == e->body_P_sensors().size()){
@ -238,7 +241,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
extrinsicCalibrationEqual = false; break; extrinsicCalibrationEqual = false; break;
} }
} }
}else{ extrinsicCalibrationEqual = false; } } else {
extrinsicCalibrationEqual = false;
}
return e && Base::equals(p, tol) && K_all_ == e->calibration() return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual;
@ -248,9 +253,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
* Compute jacobian F, E and error vector at a given linearization point * Compute jacobian F, E and error vector at a given linearization point
* @param values Values structure which must contain camera poses * @param values Values structure which must contain camera poses
* corresponding to keys involved in this factor * corresponding to keys involved in this factor
* @return Return arguments are the camera jacobians Fs (including the jacobian with * @return Return arguments are the camera jacobians Fs (including the
* respect to both body poses we interpolate from), the point Jacobian E, * jacobian with respect to both body poses we interpolate from), the point
* and the error vector b. Note that the jacobians are computed for a given point. * Jacobian E, and the error vector b. Note that the jacobians are computed
* for a given point.
*/ */
void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const { const Values& values) const {
@ -267,13 +273,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
Eigen::Matrix<double, ZDim, 3> Ei; Eigen::Matrix<double, ZDim, 3> Ei;
for (size_t i = 0; i < numViews; i++) { // for each camera/measurement for (size_t i = 0; i < numViews; i++) { // for each camera/measurement
const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); auto w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first);
const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); auto w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second);
double interpolationFactor = alphas_[i]; double interpolationFactor = alphas_[i];
// get interpolated pose: // get interpolated pose:
const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); auto w_P_body =
const Pose3& body_P_cam = body_P_sensors_[i]; interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
auto body_P_cam = body_P_sensors_[i];
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]); PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]);
// get jacobians and error vector for current measurement // get jacobians and error vector for current measurement
@ -296,37 +304,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM
} }
/// linearize and return a Hessianfactor that is an approximation of error(p) /// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor( boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping = const Values& values, const double lambda = 0.0,
false) const { bool diagonalDamping = false) const {
// we may have multiple observation sharing the same keys (due to the
// we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // rolling shutter interpolation), hence the number of unique keys may be
// hence the number of unique keys may be smaller than 2 * nrMeasurements // smaller than 2 * nrMeasurements
size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys size_t nrUniqueKeys =
this->keys_
.size(); // note: by construction, keys_ only contains unique keys
// Create structures for Hessian Factors // Create structures for Hessian Factors
KeyVector js; KeyVector js;
std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
std::vector < Vector > gs(nrUniqueKeys); std::vector<Vector> gs(nrUniqueKeys);
if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera if (this->measured_.size() !=
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " this->cameras(values).size()) // 1 observation per interpolated camera
"measured_.size() inconsistent with input"); throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point // triangulate 3D point at given linearization point
this->triangulateSafe(this->cameras(values)); this->triangulateSafe(this->cameras(values));
if (!this->result_) { // failed: return "empty/zero" Hessian if (!this->result_) { // failed: return "empty/zero" Hessian
if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
for (Matrix& m : Gs) for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
m = Matrix::Zero(DimPose, DimPose); for (Vector& v : gs) v = Vector::Zero(DimPose);
for (Vector& v : gs) return boost::make_shared<RegularHessianFactor<DimPose>>(this->keys_,
v = Vector::Zero(DimPose); Gs, gs, 0.0);
return boost::make_shared < RegularHessianFactor<DimPose>
> (this->keys_, Gs, gs, 0.0);
} else { } else {
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " throw std::runtime_error(
"only supported degeneracy mode is ZERO_ON_DEGENERACY"); "SmartProjectionPoseFactorRollingShutter: "
"only supported degeneracy mode is ZERO_ON_DEGENERACY");
} }
} }
// compute Jacobian given triangulated 3D Point // compute Jacobian given triangulated 3D Point