From c9cf6321152e914c2d2da0f459bdb0d7ac6cc949 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 16 Jan 2020 11:49:48 +0100
Subject: [PATCH] factor auto diff rowmajor changes
---
include/core/factor/factor_autodiff.h | 156 +++++++++++++-------------
1 file changed, 78 insertions(+), 78 deletions(-)
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 6f2fd3142..d5e86d2f1 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -252,18 +252,18 @@ class FactorAutodiff : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -517,18 +517,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -686,14 +686,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
// call functor
(*static_cast<FacT const*>(this))(jets_0_->data(),
- jets_1_->data(),
- jets_2_->data(),
- jets_3_->data(),
- jets_4_->data(),
- jets_5_->data(),
- jets_6_->data(),
- jets_7_->data(),
- residuals_jets_->data());
+ jets_1_->data(),
+ jets_2_->data(),
+ jets_3_->data(),
+ jets_4_->data(),
+ jets_5_->data(),
+ jets_6_->data(),
+ jets_7_->data(),
+ residuals_jets_->data());
// fill the residual array
for (unsigned int i = 0; i < RES; i++)
@@ -758,18 +758,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -988,18 +988,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -1207,18 +1207,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
}
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
@@ -1410,18 +1410,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
}
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
@@ -1602,18 +1602,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -1787,18 +1787,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -1961,18 +1961,18 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
@@ -2124,18 +2124,18 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// fill the jacobian matrices
for (unsigned int i = 0; i<n_blocks; i++)
+ {
+ // Create a row major Jacobian
+ Eigen::MatrixRowXd Ji(Eigen::MatrixRowXd::Zero(RES, state_block_sizes_[i]));
+ // Fill Jacobian rows from jets
if (!state_ptrs_[i]->isFixed())
- {
- // Create a row major Jacobian
- Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> Ji(Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor>::Zero(RES, state_block_sizes_[i]));
- // Fill Jacobian rows from jets
for (unsigned int row = 0; row < RES; row++)
std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
(*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
Ji.data() + row * state_block_sizes_.at(i));
- // add to the Jacobians vector
- jacobians_.push_back(Ji);
- }
+ // add to the Jacobians vector
+ jacobians_.push_back(Ji);
+ }
// print jacobian matrices
// for (unsigned int i = 0; i < n_blocks; i++)
--
GitLab