moved common function to cameraSet. commented issues with templated calls to functions in cameraSet
lcarlone
parent
477dd5b247
commit
91a6613d84
|
|
@ -200,6 +200,105 @@ public:
|
|||
return augmentedHessian;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
* In this version, we allow for the case where the keys in the Jacobian are organized
|
||||
* differents from the keys in the output SymmetricBlockMatrix
|
||||
* In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration)
|
||||
*/
|
||||
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
|
||||
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
|
||||
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,
|
||||
const KeyVector jacobianKeys, const KeyVector hessianKeys) {
|
||||
|
||||
size_t nrNonuniqueKeys = jacobianKeys.size();
|
||||
size_t nrUniqueKeys = hessianKeys.size();
|
||||
|
||||
// marginalize point: note - we reuse the standard SchurComplement function
|
||||
SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(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, NDD);
|
||||
dims.back() = 1;
|
||||
SymmetricBlockMatrix augmentedHessianUniqueKeys;
|
||||
|
||||
// here we have to deal with the fact that some blocks may share the same keys
|
||||
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, NDD);
|
||||
nonuniqueDims.back() = 1;
|
||||
augmentedHessian = SymmetricBlockMatrix(
|
||||
nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
|
||||
|
||||
// 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[hessianKeys[k]] = k;
|
||||
}
|
||||
|
||||
// initialize matrix to zero
|
||||
augmentedHessianUniqueKeys = SymmetricBlockMatrix(
|
||||
dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * 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 = jacobianKeys.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 = jacobianKeys.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 augmentedHessianUniqueKeys;
|
||||
}
|
||||
|
||||
/**
|
||||
* non-templated version of function above
|
||||
*/
|
||||
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6(
|
||||
const std::vector<Eigen::Matrix<double,ZDim, 12>,
|
||||
Eigen::aligned_allocator<Eigen::Matrix<double,ZDim,12> > >& Fs,
|
||||
const Matrix& E, const Eigen::Matrix<double,3,3>& P, const Vector& b,
|
||||
const KeyVector jacobianKeys, const KeyVector hessianKeys) {
|
||||
return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b,
|
||||
jacobianKeys,
|
||||
hessianKeys);
|
||||
}
|
||||
|
||||
/**
|
||||
* Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix
|
||||
* G = F' * F - F' * E * P * E' * F
|
||||
|
|
|
|||
|
|
@ -62,8 +62,6 @@ PinholePose<CALIBRATION> > {
|
|||
|
||||
/// shorthand for base class type
|
||||
typedef SmartProjectionFactor<PinholePose<CALIBRATION> > Base;
|
||||
// typedef PinholePose<CALIBRATION> Camera;
|
||||
// typedef CameraSet<Camera> Cameras;
|
||||
|
||||
/// shorthand for this class
|
||||
typedef SmartProjectionPoseFactorRollingShutter This;
|
||||
|
|
@ -299,27 +297,27 @@ PinholePose<CALIBRATION> > {
|
|||
|
||||
// we may have multiple observation sharing the same keys (due to the rolling shutter interpolation),
|
||||
// hence the number of unique keys may be smaller than 2 * nrMeasurements
|
||||
size_t nrUniqueKeys = this->keys_.size();
|
||||
size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size();
|
||||
size_t nrUniqueKeys = this->keys_.size();
|
||||
|
||||
// Create structures for Hessian Factors
|
||||
KeyVector js;
|
||||
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())
|
||||
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
|
||||
"measured_.size() inconsistent with input");
|
||||
"measured_.size() inconsistent with input");
|
||||
|
||||
// triangulate 3D point at given linearization point
|
||||
this->triangulateSafe(this->cameras(values));
|
||||
|
||||
if (!this->result_) { // failed: return "empty/zero" Hessian
|
||||
if (!this->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> > ( this->keys_, Gs, gs, 0.0);
|
||||
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||
> (this->keys_, Gs, gs, 0.0);
|
||||
}
|
||||
|
||||
// compute Jacobian given triangulated 3D Point
|
||||
|
|
@ -333,88 +331,32 @@ PinholePose<CALIBRATION> > {
|
|||
for (size_t i = 0; i < Fs.size(); i++)
|
||||
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
|
||||
|
||||
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
|
||||
Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
|
||||
// the following unfortunately does not seem to work and causes
|
||||
// an "reference to overloaded function could not be resolved; did you mean to call it?" error
|
||||
// Matrix3 P;
|
||||
// Base::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
|
||||
|
||||
// build augmented Hessian (with last row/column being the information vector)
|
||||
// and marginalize point: note - we reuse the standard SchurComplement function
|
||||
// does not work, since assumes convensional 6-dim blocks
|
||||
// SymmetricBlockMatrix augmentedHessian =
|
||||
// Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping);
|
||||
|
||||
SymmetricBlockMatrix augmentedHessian =
|
||||
Base::Cameras::SchurComplement312(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); // all are poses ..
|
||||
nonuniqueDims.back() = 1; // except b is a scalar
|
||||
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_key_pairs_.size(); i++) {
|
||||
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
|
||||
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
|
||||
}
|
||||
|
||||
// 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[ this->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));
|
||||
// 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_key_pairs_.size(); i++) {
|
||||
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).first);
|
||||
nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second);
|
||||
}
|
||||
// but we need to get the augumented hessian wrt the unique keys in key_
|
||||
SymmetricBlockMatrix augmentedHessianUniqueKeys =
|
||||
Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6(
|
||||
Fs, E, P, b, nonuniqueKeys, this->keys_);
|
||||
|
||||
// the following unfortunately does not seem to work and causes
|
||||
// an "reference to overloaded function could not be resolved; did you mean to call it?" error
|
||||
// SymmetricBlockMatrix augmentedHessianUniqueKeys =
|
||||
// Base::Cameras::SchurComplementAndRearrangeBlocks<3, DimBlock, DimPose>(
|
||||
// Fs, E, P, b, nonuniqueKeys, keys_);
|
||||
|
||||
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||
> ( this->keys_, augmentedHessianUniqueKeys);
|
||||
> (this->keys_, augmentedHessianUniqueKeys);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
/// 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 DimBlock = 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 Eigen::Matrix<double, ZDim, DimBlock> MatrixZD; // F blocks (derivatives wrt camera)
|
||||
typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
|
||||
|
||||
/**
|
||||
|
|
@ -180,7 +180,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
// 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
|
||||
Eigen::Matrix<double, ZDim, DimBlock> 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
|
||||
|
|
@ -209,8 +209,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
// 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;
|
||||
|
|
@ -246,81 +244,19 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
|
||||
// build augmented Hessian (with last row/column being the information vector)
|
||||
Matrix3 P;
|
||||
Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
|
||||
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));
|
||||
// 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));
|
||||
}
|
||||
// but we need to get the augumented hessian wrt the unique keys in key_
|
||||
SymmetricBlockMatrix augmentedHessianUniqueKeys =
|
||||
Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b,
|
||||
nonuniqueKeys, keys_);
|
||||
|
||||
return boost::make_shared < RegularHessianFactor<DimPose>
|
||||
> (keys_, augmentedHessianUniqueKeys);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue