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

Update HessianFactor.cpp 

add static_cast to make compiler happy
release/4.3a0
ShuangLiu1992 2023-04-29 22:24:28 +01:00 committed by Fan Jiang
parent 84a44eeca1
commit ac5934822e
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/linear/HessianFactor.cpp
View File

@ -72,7 +72,7 @@ HessianFactor::HessianFactor() :
/* ************************************************************************* */ /* ************************************************************************* */
HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
: GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) { : GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(G.cols()), 1}) {
if (G.rows() != G.cols() || G.rows() != g.size()) if (G.rows() != G.cols() || G.rows() != g.size())
throw invalid_argument( throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@ -85,7 +85,7 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f)
// error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu)
// where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g
HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma)
: GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) { : GaussianFactor(KeyVector{j}), info_(Dims{static_cast<Key>(Sigma.cols()), 1}) {
if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size())
throw invalid_argument( throw invalid_argument(
"Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions");
@ -99,7 +99,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11,
const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2,
double f) : double f) :
GaussianFactor(KeyVector{j1,j2}), info_( GaussianFactor(KeyVector{j1,j2}), info_(
Dims{G11.cols(),G22.cols(),1}) { Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),1}) {
info_.setDiagonalBlock(0, G11); info_.setDiagonalBlock(0, G11);
info_.setOffDiagonalBlock(0, 1, G12); info_.setOffDiagonalBlock(0, 1, G12);
info_.setDiagonalBlock(1, G22); info_.setDiagonalBlock(1, G22);
@ -113,7 +113,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11,
const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3,
double f) : double f) :
GaussianFactor(KeyVector{j1,j2,j3}), info_( GaussianFactor(KeyVector{j1,j2,j3}), info_(
Dims{G11.cols(),G22.cols(),G33.cols(),1}) { Dims{static_cast<Key>(G11.cols()),static_cast<Key>(G22.cols()),static_cast<Key>(G33.cols()),1}) {
if (G11.rows() != G11.cols() || G11.rows() != G12.rows() if (G11.rows() != G11.cols() || G11.rows() != G12.rows()
|| G11.rows() != G13.rows() || G11.rows() != g1.size() || G11.rows() != G13.rows() || G11.rows() != g1.size()
|| G22.cols() != G12.cols() || G33.cols() != G13.cols() || G22.cols() != G12.cols() || G33.cols() != G13.cols()