void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix,
const std::vector<int>& rows, const std::vector<int>& cols, int M_rows, int q_start, int q_subvector_size) {
if (q_subvector_size < 0) {
q_subvector_size = dM.cols() - q_start;
}
int index = 0;
for (int col : cols) {
for (int row : rows) {
dM.block(row + col * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++);
}
}
}
void setSubMatrixGradient(Eigen::MatrixBase<DerivedA>& dM, const Eigen::MatrixBase<DerivedB>& dM_submatrix,
const std::vector<int>& rows, const std::vector<int>& cols, typename DerivedA::Index M_rows, typename DerivedA::Index q_start, typename DerivedA::Index q_subvector_size) {
if (q_subvector_size < 0) {
q_subvector_size = dM.cols() - q_start;
}
int index = 0;
for (typename std::vector<int>::const_iterator col = cols.begin(); col != cols.end(); ++col) {
for (typename std::vector<int>::const_iterator row = rows.begin(); row != rows.end(); ++row) {
dM.block((*row) + (*col) * M_rows, q_start, 1, q_subvector_size) = dM_submatrix.row(index++);
}
}
}
Eigen::Matrix<typename DerivedDA::Scalar, matGradMultNumRows(DerivedDA::RowsAtCompileTime, Derivedb::RowsAtCompileTime), DerivedDA::ColsAtCompileTime>
matGradMult(const Eigen::MatrixBase<DerivedDA>& dA, const Eigen::MatrixBase<Derivedb>& b) {
const int nq = dA.cols();
assert(b.cols() == 1);
assert(dA.rows() % b.rows() == 0);
const int A_rows = dA.rows() / b.rows();
Eigen::Matrix<typename DerivedDA::Scalar, matGradMultNumRows(DerivedDA::RowsAtCompileTime, Derivedb::RowsAtCompileTime), DerivedDA::ColsAtCompileTime> ret(A_rows, nq);
ret.setZero();
for (int row = 0; row < b.rows(); row++) {
ret += b(row, 0) * dA.block(row * A_rows, 0, A_rows, nq);
}
return ret;
}
Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic> getSubMatrixGradient(
const Eigen::MatrixBase<Derived>& dM, const std::vector<int>& rows, const std::vector<int>& cols,
int M_rows, int q_start, int q_subvector_size) {
if (q_subvector_size < 0) {
q_subvector_size = dM.cols() - q_start;
}
Eigen::MatrixXd dM_submatrix(rows.size() * cols.size(), q_subvector_size);
int index = 0;
for (int col : cols) {
for (int row : rows) {
dM_submatrix.row(index) = dM.block(row + col * M_rows, q_start, 1, q_subvector_size);
index++;
}
}
return dM_submatrix;
}
Eigen::Matrix<typename Derived::Scalar, Eigen::Dynamic, Eigen::Dynamic> getSubMatrixGradient(
const Eigen::MatrixBase<Derived>& dM, const std::vector<int>& rows, const std::vector<int>& cols,
typename Derived::Index M_rows, int q_start, typename Derived::Index q_subvector_size) {
if (q_subvector_size < 0) {
q_subvector_size = dM.cols() - q_start;
}
Eigen::MatrixXd dM_submatrix(rows.size() * cols.size(), q_subvector_size);
int index = 0;
for (std::vector<int>::const_iterator col = cols.begin(); col != cols.end(); ++col) {
for (std::vector<int>::const_iterator row = rows.begin(); row != rows.end(); ++row) {
dM_submatrix.row(index) = dM.block(*row + *col * M_rows, q_start, 1, q_subvector_size);
index++;
}
}
return dM_submatrix;
}
bool CameraModel::project_trans
( const Eigen::MatrixBase<Derived1>& mR,
const Eigen::MatrixBase<Derived2>& mt,
const Eigen::MatrixBase<Derived3>& mP3D,
Eigen::MatrixBase<Derived4>& mP2D,
Eigen::MatrixBase<Derived5>& mdP2DE,
Eigen::MatrixBase<Derived6>& mdP2DI
) const {
typedef typename Eigen::internal::traits<Derived4>::Scalar Derived4Type;
typedef typename Eigen::internal::traits<Derived5>::Scalar Derived5Type;
typedef Eigen::Matrix<Derived4Type, 1, 3> Derived4Vec;
assert( mP3D.rows() == 3 );
assert( mP2D.rows() == 2 );
assert( mP3D.cols() == mP2D.cols() );
assert( mR.rows() == 3 );
assert( mR.cols() == 3 );
assert( mt.rows() == 3 );
assert( mt.cols() == 1 );
Derived3 mP3DT = mR * mP3D;
mP3DT.colwise() += mt;
Eigen::Matrix<Derived5Type,2,Eigen::Dynamic,Derived5::Base::Options> mdP2DP3D( 2, 3*mP3D.cols() );
bool bSuccess = project( mP3DT, mP2D, mdP2DP3D, mdP2DI );
if( !bSuccess ) { return bSuccess; }
Derived4 mdR0 = mR*CEIGEN::skew_rot<Derived4>(1,0,0);
Derived4 mdR1 = mR*CEIGEN::skew_rot<Derived4>(0,1,0);
Derived4 mdR2 = mR*CEIGEN::skew_rot<Derived4>(0,0,1);
for( int ii=0; ii<mP3D.cols(); ii++ ) {
// Chain rule: right part of the extrinsic matrix
// computation related to the rotation and translation
Derived4 mdRtX( 3, 6 );
// Rotation part
mdRtX.block(0,0,3,1) = mdR0*mP3D.col( ii );
mdRtX.block(0,1,3,1) = mdR1*mP3D.col( ii );
mdRtX.block(0,2,3,1) = mdR2*mP3D.col( ii );
// Translation part
mdRtX.block(0,3,3,3) = Derived4::Identity(3,3);
// Combine extrinsic Jacobian with position Jacobian (right hand side)
mdP2DE.block(0,6*ii,2,6) = mdP2DP3D.block(0,3*ii,2,3) * mdRtX;
}
return true;
}
// typename DerivedA::Scalar
void pseudo_inverse_svd(const Eigen::MatrixBase<DerivedA>& M,
Eigen::MatrixBase<OutputMatrixType>& Minv,
typename DerivedA::Scalar epsilon = 1e-6)//std::numeric_limits<typename DerivedA::Scalar>::epsilon())
{
// CONTROLIT_INFO << "Method called!\n epsilon = " << epsilon << ", M = \n" << M;
// Ensure matrix Minv has the correct size. Its size should be equal to M.transpose().
assert_msg(M.rows() == Minv.cols(), "Minv has invalid number of columns. Expected " << M.rows() << " got " << Minv.cols());
assert_msg(M.cols() == Minv.rows(), "Minv has invalid number of rows. Expected " << M.cols() << " got " << Minv.rows());
// According to Eigen documentation, "If the input matrix has inf or nan coefficients, the result of the
// computation is undefined, but the computation is guaranteed to terminate in finite (and reasonable) time."
Eigen::JacobiSVD<DerivedA> svd = M.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
// Get the max singular value
typename DerivedA::Scalar maxSingularValue = svd.singularValues().array().abs().maxCoeff();
// Use Minv to temporarily hold sigma
Minv.setZero();
typename DerivedA::Scalar tolerance = 0;
// Only compute sigma if the max singular value is greater than zero.
if (maxSingularValue > epsilon)
{
tolerance = epsilon * std::max(M.cols(), M.rows()) * maxSingularValue;
// For each singular value of matrix M's SVD decomposition, check if it is greater than
// the tolerance value. If it is, save 1/(singular value) in the sigma vector.
// Otherwise save zero in the sigma vector.
DerivedA sigmaVector = DerivedA( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) );
// DerivedA zeroSVs = DerivedA( (svd.singularValues().array().abs() <= tolerance).select(svd.singularValues().array().inverse(), 0) );
// CONTROLIT_INFO << "epsilon: " << epsilon << ", std::max(M.cols(), M.rows()): " << std::max(M.cols(), M.rows()) << ", maxSingularValue: " << maxSingularValue << ", tolerance: " << tolerance;
// CONTROLIT_INFO << "sigmaVector = " << sigmaVector.transpose();
// CONTROLIT_INFO << "zeroSigmaVector : "<< zeroSVs.transpose();
Minv.block(0, 0, sigmaVector.rows(), sigmaVector.rows()) = sigmaVector.asDiagonal();
}
// Double check to make sure the matrices have the correct dimensions
assert_msg(svd.matrixV().cols() == Minv.rows(),
"Matrix dimension mismatch, svd.matrixV().cols() = " << svd.matrixV().cols() << ", Minv.rows() = " << Minv.rows() << ".");
assert_msg(Minv.cols() == svd.matrixU().adjoint().rows(),
"Matrix dimension mismatch, Minv.cols() = " << Minv.cols() << ", svd.matrixU().adjoint().rows() = " << svd.matrixU().adjoint().rows() << ".");
Minv = svd.matrixV() *
Minv *
svd.matrixU().adjoint(); // take the transpose of matrix U
// CONTROLIT_INFO << "Done method call! Minv = " << Minv;
// typename DerivedA::Scalar errorNorm = std::abs((M * Minv - DerivedA::Identity(M.rows(), Minv.cols())).norm());
// if (tolerance != 0 && errorNorm > tolerance * 10)
// {
// CONTROLIT_WARN << "Problems computing pseudoinverse. Perhaps the tolerance is too high?\n"
// << " - epsilon: " << epsilon << "\n"
// << " - tolerance: " << tolerance << "\n"
// << " - maxSingularValue: " << maxSingularValue << "\n"
// << " - errorNorm: " << errorNorm << "\n"
// << " - M:\n" << M << "\n"
// << " - Minv:\n" << Minv;
// }
// return errorNorm;
}
