Go to the documentation of this file.
29 #include "../../../../Eigen/src/Core/util/NonMPL2.h"
31 #ifndef EIGEN_CONSTRAINEDCG_H
32 #define EIGEN_CONSTRAINEDCG_H
34 #include "../../../../Eigen/Core"
46 template <
typename CMatrix,
typename CINVMatrix>
62 std::vector<T> tripletList;
75 l =
C.transpose() *
p;
82 p = (rho/rho_1) *
p + r;
85 l =
C.transpose() *
e;
89 tripletList.push_back(
T(
i,
j,
l(
j)));
94 CINV.setFromTriplets(tripletList.begin(), tripletList.end());
104 template<
typename TMatrix,
typename CMatrix,
105 typename VectorX,
typename VectorB,
typename VectorF>
116 TmpVec
p(xSize),
q(xSize), q2(xSize),
117 r(xSize), old_z(xSize),
z(xSize),
119 std::vector<bool> satured(
C.
rows());
122 if (
iter.rhsNorm() == 0.0)
iter.setRhsNorm(1.0);
135 bool transition =
false;
149 for (
typename CMatrix::InnerIterator it(
C,
i); it; ++it)
150 z.coeffRef(it.index()) -= bb*it.value();
160 if (
iter.finished(rho))
break;
161 if (transition ||
iter.first())
gamma = 0.0;
187 #endif // EIGEN_CONSTRAINEDCG_H
Namespace containing all symbols from the Eigen library.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
Controls the iterations of the iterative solvers.
void pseudo_inverse(const CMatrix &C, CINVMatrix &CINV)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Eigen::Triplet< double > T
void constrained_cg(const TMatrix &A, const CMatrix &C, VectorX &x, const VectorB &b, const VectorF &f, IterationController &iter)
EIGEN_DEVICE_FUNC const Scalar & q
static const Line3 l(Rot3(), 1, 1)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Matrix< Scalar, Dynamic, Dynamic > C
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
iterator iter(handle obj)
Matrix< Scalar, Dynamic, 1 > VectorX
Jet< T, N > sqrt(const Jet< T, N > &f)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:11