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

use safer eigen indexing syntax

release/4.3a0
Varun Agrawal 2021-11-09 17:56:03 -05:00
parent bfb21c2faa
commit 9165041299
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/nonlinear/factorTesting.h
View File

@ -36,7 +36,6 @@ namespace gtsam {
*/ */
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values, double delta = 1e-5) { const Values& values, double delta = 1e-5) {
// We will fill a vector of key/Jacobians pairs (a map would sort) // We will fill a vector of key/Jacobians pairs (a map would sort)
std::vector<std::pair<Key, Matrix> > jacobians; std::vector<std::pair<Key, Matrix> > jacobians;
@ -53,11 +52,11 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
Matrix J = Matrix::Zero(rows, cols); Matrix J = Matrix::Zero(rows, cols);
for (size_t col = 0; col < cols; ++col) { for (size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
dx[col] = delta; dx(col) = delta;
dX[key] = dx; dX[key] = dx;
Values eval_values = values.retract(dX); Values eval_values = values.retract(dX);
const Eigen::VectorXd left = factor.whitenedError(eval_values); const Eigen::VectorXd left = factor.whitenedError(eval_values);
dx[col] = -delta; dx(col) = -delta;
dX[key] = dx; dX[key] = dx;
eval_values = values.retract(dX); eval_values = values.retract(dX);
const Eigen::VectorXd right = factor.whitenedError(eval_values); const Eigen::VectorXd right = factor.whitenedError(eval_values);